Skip to content
Snippets Groups Projects

Draft: Develop

Open Raphael Sturgis requested to merge develop into main
Files
9
+ 64
0
import random
from copy import deepcopy
import numpy as np
from skais.utils.geometry import vectorize_angle, local_ref, transpose_matrix, \
quaternion_multiply, angleize_vector, rotation_quaternions, spherical_to_carthesian, carthesian_to_spherical, \
rotate_vector_by_quaternion
base_ref = np.array(
[
[1, 0, 0],
[0, 1, 0],
[0, 0, 1]
]
)
def shift_positions(aisPositions, axis_lat, axis_long, angle):
copy = deepcopy(aisPositions)
rotation_quaternion, inverse_rotation_quaternion = rotation_quaternions(axis_lat, axis_long, angle)
latitudes = np.radians(aisPositions.df['latitude'].to_numpy())
longitudes = np.radians(aisPositions.df['longitude'].to_numpy())
cogs = np.radians(aisPositions.df['cog'].to_numpy())
headings = np.radians(aisPositions.df['heading'].to_numpy())
for i in range(len(latitudes)):
lat = latitudes[i]
long = longitudes[i]
v_cog = vectorize_angle(cogs[i])
v_heading = vectorize_angle(headings[i])
new_position = rotate_vector_by_quaternion(spherical_to_carthesian(lat, long), rotation_quaternion, inverse_rotation_quaternion)
new_lat, new_long = carthesian_to_spherical(new_position)
origin_ref = local_ref(lat, long)
target_ref = local_ref(new_lat, new_long)
matrix_ob = transpose_matrix(origin_ref, base_ref)
matrix_bt = transpose_matrix(base_ref, target_ref)
# transpose vectors to base referential
v_cog = np.matmul(matrix_ob, v_cog)
v_heading = np.matmul(matrix_ob, v_heading)
# rotate vector
v_cog = rotate_vector_by_quaternion(v_cog, rotation_quaternion, inverse_rotation_quaternion)
v_heading = rotate_vector_by_quaternion(v_heading, rotation_quaternion, inverse_rotation_quaternion)
# transpose vector back to local referential of the new position
v_cog = np.matmul(matrix_bt, v_cog)
v_heading = np.matmul(matrix_bt, v_heading)
latitudes[i] = new_lat
longitudes[i] = new_long
cogs[i] = angleize_vector(v_cog)
headings[i] = angleize_vector(v_heading)
copy.df['latitude'] = np.degrees(latitudes)
copy.df['longitude'] = np.degrees(longitudes)
copy.df['cog'] = np.degrees(cogs)
copy.df['heading'] = np.degrees(headings)
return copy
Loading