def print_state(self, o):
"""Print information about state of the quaternion"""
current_X_IMU = qt.apply_rotation_on_vector(o, self.X_IMU_ref_IMU)
current_Y_IMU = qt.apply_rotation_on_vector(o, self.Y_IMU_ref_IMU)
current_Z_IMU = qt.apply_rotation_on_vector(o, self.Z_IMU_ref_IMU)
print(
"Print state of quaternion --------------------------------------------"
)
print(
"state orientation: current X, Y, Z of the IMU, in referential Earth:"
)
qt.print_vector(current_X_IMU)
qt.print_vector(current_Y_IMU)
qt.print_vector(current_Z_IMU)
return (current_X_IMU, current_Y_IMU, current_Z_IMU)
def rotateQ(self, q):
"""Apply rotation described by quaternion q to this 3D point"""
v_rotated = qt.apply_rotation_on_vector(q, self.v)
return Point3D(v_rotated.vx, v_rotated.vy, v_rotated.vz)
