Untitled
unknown
plain_text
a year ago
1.6 kB
6
Indexable
Never
import numpy as np from scipy.spatial.transform import Rotation def quaternion_slerp(q1, q2, t, eps=1e-6): # Normalize input quaternions q1 = q1 / np.linalg.norm(q1) q2 = q2 / np.linalg.norm(q2) # Compute the cosine of the angle between the two quaternions cos_theta = np.dot(q1, q2) # If the dot product is negative, negate one of the quaternions if cos_theta < 0.0: q1 = -q1 cos_theta = -cos_theta # If the quaternions are very close, use linear interpolation to avoid numerical instability if cos_theta > 1.0 - eps: return q1 * (1 - t) + q2 * t # Compute the angle between the quaternions theta = np.arccos(cos_theta) # Compute the scale factors for quaternion interpolation sin_theta = np.sin(theta) s1 = np.sin((1 - t) * theta) / sin_theta s2 = np.sin(t * theta) / sin_theta # Compute the interpolated quaternion q_slerp = q1 * s1 + q2 * s2 return q_slerp # Example usage: q1 = np.array([1.0, 0.0, 0.0, 0.0]) # Identity quaternion q2 = Rotation.from_euler('xyz', [90, 0, 0], degrees=True).as_quat() # Quaternion representing a 90-degree rotation around the x-axis t = 0.5 # Interpolation parameter (0 <= t <= 1) q_slerp = quaternion_slerp(q1, q2, t) # Convert the resulting quaternion to an equivalent Euler angle representation euler_angles = Rotation.from_quat(q_slerp).as_euler('xyz', degrees=True) print("Interpolated quaternion:", q_slerp) print("Equivalent Euler angles:", euler_angles)