def quaternion_rotate_vector(quat: np.quaternion, v: np.array) -> np.array:
r"""Rotates a vector by a quaternion
Args:
quaternion: The quaternion to rotate by
v: The vector to rotate
Returns:
np.array: The rotated vector
"""
vq = np.quaternion(0, 0, 0, 0)
vq.imag = v
return (quat * vq * quat.inverse()).imag
def _compute_pointgoal(
source_position: np.ndarray,
source_rotation: np.quaternion,
goal_position: np.ndarray,
):
direction_vector = goal_position - source_position
direction_vector_agent = GPSCompassSensorRoboThor.quaternion_rotate_vector(
source_rotation.inverse(), direction_vector)
rho, phi = GPSCompassSensorRoboThor.cartesian_to_polar(
direction_vector_agent[2], -direction_vector_agent[0])
return np.array([rho, phi], dtype=np.float32)
def quat_rotate_vector(q: np.quaternion, v: np.array) -> np.array:
r"""Helper function to rotate a vector by a quaternion, simply does
v = (q * np.quaternion(0, *v) * q.inverse()).imag
Args:
q (np.quaternion): The quaternion to rotate the vector with
v (np.array): The vector to rotate
Returns:
np.array: The rotated vector
"""
vq = np.quaternion(0, 0, 0, 0)
vq.imag = v
return (q * vq * q.inverse()).imag
def quat_rotate_vector(q: np.quaternion, v: np.ndarray) -> np.ndarray:
r"""Helper function to rotate a vector by a quaternion
:param q: The quaternion to rotate the vector with
:param v: The vector to rotate
:return: The rotated vector
Does
.. code:: py
v = (q * np.quaternion(0, *v) * q.inverse()).imag
"""
vq = np.quaternion(0, 0, 0, 0)
vq.imag = v
return (q * vq * q.inverse()).imag
def rotate_vector(q: np.quaternion, vec: Sequence[float]):
return (q.conjugate() * np.quaternion(0, vec[0], vec[1], vec[2]) *
q).components[1:4]
