Skip to content
Snippets Groups Projects

Draft: Develop

7 files
+ 313
3
Compare changes
  • Side-by-side
  • Inline

Files

+ 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