-
Notifications
You must be signed in to change notification settings - Fork 0
/
vehicle.py
119 lines (91 loc) · 4.15 KB
/
vehicle.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
import sys
import math
from enum import Enum
class GeoCoordinate(Enum) :
LONGITUDE = 0
LATITUDE = 1
EARTH_RADIUS = float(6367444.50)
SIDEREAL_DAY_SECS = float(86164.09)
def main() :
for line in sys.stdin :
if line == '':
break
tokens = line.split()
if(len(tokens) == 0) :
break
#store the tokenized data into the correct data types
t_beg = float(tokens[0])
t_end = float(tokens[1])
t_steps = int(tokens[2])
psi_d = int(tokens[3])
psi_m = int(tokens[4])
psi_s = float(tokens[5])
ns_value = int(tokens[6])
lambda_d = int(tokens[7])
lambda_m = int(tokens[8])
lambda_s = float(tokens[9])
ew_value = int(tokens[10])
altitude = float(tokens[11])
if(t_steps == 0) :
t_steps = 1
delta_time = 1
if(t_steps > 1) :
delta_time = (t_end - t_beg) / t_steps
lat_off = degreeToRad(psi_d, psi_m, psi_s, ns_value, GeoCoordinate.LATITUDE)
lon_off = degreeToRad(lambda_d, lambda_m, lambda_s, ew_value, GeoCoordinate.LONGITUDE)
time_rad = 0
vec = getCartesianCoords(EARTH_RADIUS + altitude, lon_off, lat_off)
sys.stdout.write("{},{},{}\n".format(vec[0],vec[1],vec[2]))
new_coord = cartesianToRad(vec)
# calculate all positions of the object in each time step
for step in range(t_steps + 1) :
time = delta_time * step
if(t_steps > 1 and step > 0) :
time_rad = (2.0 * math.pi) * (time / SIDEREAL_DAY_SECS)
lat_data = radToDegree(lat_off, GeoCoordinate.LATITUDE)
lon_data = radToDegree(lon_off + time_rad, GeoCoordinate.LONGITUDE)
#sys.stdout.write("{} {} {} {} {} {} {} {} {} {}\n".format(t_beg + t_v, lat_data[0], lat_data[1], lat_data[2], lat_data[3], lon_data[0], lon_data[1], lon_data[2], lon_data[3], altitude))
pass
def getCartesianCoords(radius, angle_theta, angle_phi) :
x = radius * math.cos(angle_theta)*math.sin(angle_phi)
y = radius * math.sin(angle_theta)*math.sin(angle_phi)
z = radius * math.cos(angle_phi)
return [x, y, z]
def degreeToRad(angle, angle_minute, angle_second, dir_value, geo_coordinate) :
absolute_angle = angle + (angle_minute / 60.0) + (angle_second / 3600.0)
# convert the angle to be within (0,360)
if(geo_coordinate == GeoCoordinate.LONGITUDE) :
absolute_angle = ((1 - ((1 + dir_value) * 0.5)) * 360.0) + (dir_value * absolute_angle)
# convert the angle to be within (0, 180)
elif(geo_coordinate == GeoCoordinate.LATITUDE) :
absolute_angle = 90.0 - (dir_value * absolute_angle)
return absolute_angle * math.pi / 180.0
def radToDegree(angle, geo_coordinate) :
absolute_angle = angle * 180.0 / math.pi
dir_value = 1
# convert the range from (0, 360) to (0, 180)
# also gets the EW value
if(geo_coordinate == GeoCoordinate.LONGITUDE) :
dir_value = int(-1 if (absolute_angle > math.pi) else 1)
absolute_angle = (absolute_angle - ((1 - ((1 + dir_value) * 0.5)) * 360.0)) * dir_value
# converts the range from (0, 180) to (0,90)
# also gets the NS value
if(geo_coordinate == GeoCoordinate.LATITUDE) :
dir_value = int(-1 if (absolute_angle > 90) else 1)
absolute_angle = (absolute_angle - 90.0) * -dir_value
# calculates the angle in degree, minutes, and seconds
angle = int(math.floor(absolute_angle))
absolute_angle = (absolute_angle - angle) * 60.0
angle_min = int(math.floor(absolute_angle))
angle_sec = float((absolute_angle - angle_min) * 60.0)
return [angle, angle_min, angle_sec, dir_value]
def magnitude(u):
return math.sqrt((u[0] * u[0]) + (u[1] * u[1]) + (u[2] * u[2]))
def cartesianToRad(x) :
mag = magnitude(x)
altitude = mag - EARTH_RADIUS
mag2 = math.sqrt(x[0] * x[0] + x[1] * x[1])
theta = math.atan(x[1]/ x[0]) + math.pi
phi = (math.pi / 2) - math.atan(x[2] / mag2)
return [theta, phi, altitude]
main()