def quaternion_from_two_vectors(a: Vector3r, b: Vector3r) -> Quaternionr:
""" What rotation (around the intersection of the two vectors) we need to rotate `a` to `b`? """
# ref.: https://gitlab.com/libeigen/eigen/-/blob/master/Eigen/src/Geometry/Quaternion.h (FromTwoVectors)
v0 = a / a.get_length()
v1 = b / b.get_length()
c = v1.dot(v0)
assert c > -1 # FIXME handle the case when the vectors are nearly opposites
s = np.sqrt((1 + c) * 2)
axis = v0.cross(v1) / s
return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=(s / 2))
def quaternion_from_rotation_axis_angle(axis: Vector3r, angle: float, is_degrees: bool = False) -> Quaternionr:
if is_degrees:
angle = np.deg2rad(angle)
half_angle = angle / 2
axis /= axis.get_length() # normalize
axis *= np.sin(half_angle)
return Quaternionr(axis.x_val, axis.y_val, axis.z_val, w_val=np.cos(half_angle))
def vector_projected_onto_plane(v: Vector3r, plane_normal: Vector3r) -> Vector3r:
""" Returns the projection of `v` onto the plane defined by `plane_normal`. """
n = plane_normal / plane_normal.get_length()
return v - vector_projected_onto_vector(v, n)