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)