# 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)
```