-
Notifications
You must be signed in to change notification settings - Fork 0
/
util.py
49 lines (41 loc) · 1.31 KB
/
util.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
import math
import numpy as np
# TODO: refactor to use the function from coordinator
# Currently can't import due to import error in satellite
EARTH_RADIUS = 6371000 # in meter
ALTITUDE = 550 # Orbit Altitude (Km)
# according to wikipedia
STD_GRAVITATIONAL_PARAMETER_EARTH = 3.986004418e14
# seconds the earth needs to make one whole rotation
SECONDS_PER_DAY = 60 * 60 * 24
# earth's z axis (eg a vector in the positive z direction)
EARTH_ROTATION_AXIS = [
0,
0,
1,
] # this means the north pole is on the top and the south pole is on the bottom
SEMI_MAJOR_AXIS = float(ALTITUDE) * 1000 + EARTH_RADIUS
def transform_geo_to_xyz(lat, lon):
"""
Transforms any latitude and longitude to xyz coordinates.
Taken from Ben S. Kempton.
Parameters
----------
lat
lon
Returns
-------
"""
# must convert the lat/long/alt to cartesian coordinates
radius = EARTH_RADIUS + ALTITUDE
init_pos = [0, 0, 0]
latitude = math.radians(lat)
longitude = math.radians(lon)
init_pos[0] = radius * math.cos(latitude) * math.cos(longitude) # x
init_pos[1] = radius * math.cos(latitude) * math.sin(longitude) # y
init_pos[2] = radius * math.sin(latitude) # z
pos = init_pos
x = np.int32(pos[0])
y = np.int32(pos[1])
z = np.int32(pos[2])
return x, y, z