| """ |
| 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 |
|
|
|
|
