def quaternion_that_rotates_axes_frame(
source_xyz_axes: Tuple[Vector3r, Vector3r, Vector3r],
target_xyz_axes: Tuple[Vector3r, Vector3r, Vector3r],
assume_normalized: bool = False, # warn if it isn't
) -> Quaternionr:
""" Returns the quaternion that rotates vectors from the `source` coordinate system to the `target` axes frame. """
if not assume_normalized:
assert all(np.isclose(_.get_length(), 1) for _ in source_xyz_axes + target_xyz_axes)
# ref.: https://math.stackexchange.com/a/909245
i, j, k = source_xyz_axes
a, b, c = target_xyz_axes
def quaternion_from_vector(v: Vector3r) -> Quaternionr:
return Quaternionr(v.x_val, v.y_val, v.z_val, w_val=0)
qx = quaternion_from_vector(a) * quaternion_from_vector(i)
qy = quaternion_from_vector(b) * quaternion_from_vector(j)
qz = quaternion_from_vector(c) * quaternion_from_vector(k)
rx = qx.x_val + qy.x_val + qz.x_val
ry = qx.y_val + qy.y_val + qz.y_val
rz = qx.z_val + qy.z_val + qz.z_val
rw = qx.w_val + qy.w_val + qz.w_val
rotation = Quaternionr(-rx, -ry, -rz, w_val=(1 - rw))
length = rotation.get_length()
assert not np.isclose(length, 0)
return rotation / length # normalize
def vector_rotated_by_quaternion(v: Vector3r, q: Quaternionr) -> Vector3r:
q /= q.get_length() # normalize
# Extract the vector and scalar parts of q:
u, s = Vector3r(q.x_val, q.y_val, q.z_val), q.w_val
# NOTE the results from these two methods are the same up to 7 decimal places.
# ref.: https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion
# return u * (2 * u.dot(v)) + v * (s * s - u.dot(u)) + u.cross(v) * (2 * s)
# ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (_transformVector)
uv = u.cross(v)
uv += uv
return v + uv * s + u.cross(uv)
