File size: 2,104 Bytes
a162e39 |
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 |
"""
works with all the transformations and calculation associated to position, speed, acceleration
"""
import numpy as np
def position_to_xyz(position: [float]) -> [float]:
"""
allows to get the 3D xyz coordinates from a polar representation
:param position: array (3,) with rho in meter, theta in rad, zed in meter
:return: float array (3,) with x, y, z in meter
"""
pos = position[0] * np.exp(1j * position[1])
return [np.real(pos), np.imag(pos), position[2]]
"""
def _test_position_to_norm():
assert position_to_norm([param.PERIMETER, 0, 100]) == [1, 0, 1]
assert position_to_norm([0, -np.pi / 2, 0]) == [0, 0.75, 0]
assert position_to_norm([0, np.pi / 2, 0]) == [0, 0.25, 0]
"""
def is_in_the_cone(position1: [float], position2: [float], vector2: [float], angle: float) -> bool:
"""
checks if the point @ position 2 is in the cone from position 1 with an angle of angle
:param position1: in x, y, z
:param position2: in x, y, z
:param vector2: in x, y, z
:param angle: in rad
:return:
"""
vector1 = np.array(position2, dtype=float) - np.array(position1)
vector1 /= np.linalg.norm(vector1)
vector2 = np.array(vector2, dtype=float)
vector2 /= np.linalg.norm(vector2)
cos_theta = np.dot(vector1, vector2)
if 0 < cos_theta:
theta = np.arcsin(np.sqrt(1 - cos_theta ** 2))
return theta < angle
return False
def _test_is_in_the_cone():
assert is_in_the_cone([0, 0, 0], [1, 0.1, 0], [1, 0, 0], np.pi / 5)
assert is_in_the_cone([0, 0, 0], [1, 0.1, 0], [0, 1, 0], np.pi / 5)
pass
def rhotheta_to_latlon(rho: float, theta: float, lat_tg: float, lon_tg: float) -> [float, float]:
"""
transforms polar coordinates into lat, lon
:param rho:
:param theta:
:param lat_tg: latitude de la target (0,0)
:param lon_tg: longitude de la target (0,0)
:return:
"""
z = rho * np.exp(1j * theta)
lat = np.imag(z) * 360 / (40075 * 1000) + lat_tg
lon = np.real(z) * 360 / (40075 * 1000 * np.cos(np.pi / 180 * lat)) + lon_tg
return lat, lon
|