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)