Interpolation and Animation

@classmethod
def slerp(cls, q0, q1, amount=0.5):
    """
    Spherical Linear Interpolation between quaternions.
    
    Finds a valid quaternion rotation at a specified distance along the
    minor arc of a great circle passing through two quaternion endpoints
    on the unit radius hypersphere.
    
    Parameters:
        q0: Quaternion, first endpoint rotation
        q1: Quaternion, second endpoint rotation  
        amount: float, interpolation parameter [0, 1]
               0.0 returns q0, 1.0 returns q1, 0.5 returns midpoint
    
    Returns:
        Quaternion: Interpolated unit quaternion
    
    Note:
        Input quaternions are automatically normalized to unit length.
        Takes the shorter rotation path by flipping quaternion sign if needed.
    """

from pyquaternion import Quaternion
import numpy as np

# Create start and end orientations
q_start = Quaternion(axis=[0, 0, 1], degrees=0)    # No rotation
q_end = Quaternion(axis=[0, 0, 1], degrees=90)     # 90° about Z

# Interpolate at various points
q_quarter = Quaternion.slerp(q_start, q_end, 0.25)  # 25% along path
q_half = Quaternion.slerp(q_start, q_end, 0.5)      # 50% along path  
q_three_quarter = Quaternion.slerp(q_start, q_end, 0.75)  # 75% along path

print(f"Start: {q_start.degrees:.1f}°")           # 0.0°
print(f"Quarter: {q_quarter.degrees:.1f}°")       # ~22.5°
print(f"Half: {q_half.degrees:.1f}°")             # ~45.0°
print(f"Three-quarter: {q_three_quarter.degrees:.1f}°")  # ~67.5°
print(f"End: {q_end.degrees:.1f}°")               # 90.0°

# Animate rotation by applying to vector
vector = [1, 0, 0]
for t in np.linspace(0, 1, 11):
    q_t = Quaternion.slerp(q_start, q_end, t)
    rotated = q_t.rotate(vector)
    print(f"t={t:.1f}: {rotated}")

@classmethod
def intermediates(cls, q0, q1, n, include_endpoints=False):
    """
    Generate evenly spaced quaternion rotations between two endpoints.
    
    Convenience method based on slerp() for creating animation sequences.
    
    Parameters:
        q0: Quaternion, initial endpoint rotation
        q1: Quaternion, final endpoint rotation
        n: int, number of intermediate quaternions within the interval
        include_endpoints: bool, if True, includes q0 and q1 in sequence
                          resulting in n+2 total quaternions
                          if False, excludes endpoints (default)
    
    Yields:
        Generator[Quaternion]: Sequence of intermediate quaternions
        
    Note:
        Input quaternions are automatically normalized to unit length.
    """

# Create smooth rotation sequence
q_start = Quaternion()  # Identity
q_end = Quaternion(axis=[1, 1, 1], degrees=120)

# Generate 5 intermediate steps (excluding endpoints)
intermediates = list(Quaternion.intermediates(q_start, q_end, 5))
print(f"Generated {len(intermediates)} intermediate quaternions")

# Generate sequence including endpoints
full_sequence = list(Quaternion.intermediates(q_start, q_end, 5, include_endpoints=True))
print(f"Full sequence has {len(full_sequence)} quaternions")

# Use in animation loop
vector = [1, 0, 0]
print("Animation frames:")
for i, q in enumerate(full_sequence):
    rotated = q.rotate(vector)
    print(f"Frame {i}: {rotated}")

# Create continuous rotation animation
for q in Quaternion.intermediates(q_start, q_end, 20):
    angle_deg = q.degrees if q.degrees is not None else 0.0
    print(f"Rotation: {angle_deg:.1f}° about {q.axis}")

def derivative(self, rate):
    """
    Get instantaneous quaternion derivative for a quaternion rotating at 3D rate.
    
    Computes the time derivative of the quaternion given angular velocity,
    useful for integrating rotational motion.
    
    Parameters:
        rate: array-like, 3-element angular velocity vector [ωx, ωy, ωz]
              Units should be consistent (e.g., rad/s)
              Represents rotation rates about global x, y, z axes
    
    Returns:
        Quaternion: Quaternion derivative representing rotation rate
                   Formula: 0.5 * self * Quaternion(vector=rate)
    """

def integrate(self, rate, timestep):
    """
    Advance quaternion to its value after time interval with constant angular velocity.
    
    Modifies the quaternion object in-place using closed-form integration.
    The quaternion remains normalized throughout the process.
    
    Parameters:
        rate: array-like, 3-element angular velocity [ωx, ωy, ωz]
              Rotation rates about global x, y, z axes (e.g., rad/s)
        timestep: float, time interval for integration
                 Must have consistent units with rate (e.g., seconds)
    
    Note:
        - Modifies the quaternion object directly
        - Assumes constant angular velocity over the timestep
        - Smaller timesteps give more accurate results for varying rates
        - Automatically maintains unit quaternion property
    """

import numpy as np

# Initial orientation
q = Quaternion()  # Identity quaternion

# Angular velocity (rad/s)
angular_velocity = [0, 0, 1.0]  # 1 rad/s about Z-axis
dt = 0.1  # 100ms timestep

# Simulate rotation over time
print("Quaternion integration simulation:")
print(f"Time: 0.0s, Angle: {q.degrees:.1f}°")

for step in range(10):
    # Get derivative
    q_dot = q.derivative(angular_velocity)
    
    # Integrate one timestep
    q.integrate(angular_velocity, dt)
    
    time = (step + 1) * dt
    angle = q.degrees if q.degrees is not None else 0.0
    print(f"Time: {time:.1f}s, Angle: {angle:.1f}°")

# Verify final result
expected_angle = np.degrees(1.0)  # 1 rad/s * 1.0s total
print(f"Expected final angle: {expected_angle:.1f}°")

# Integration with varying angular velocity
q = Quaternion()  # Reset
time_points = np.linspace(0, 2*np.pi, 100)
dt = time_points[1] - time_points[0]

trajectory = []
for t in time_points:
    # Time-varying angular velocity (sinusoidal)
    omega = [np.sin(t), np.cos(t), 0.5]
    q.integrate(omega, dt)
    trajectory.append((t, q.degrees, q.axis.copy()))

# Analyze trajectory
print(f"\nTrajectory analysis:")
print(f"Final time: {time_points[-1]:.2f}s")
print(f"Final angle: {trajectory[-1][1]:.1f}°")
print(f"Final axis: {trajectory[-1][2]}")

# Spacecraft attitude control simulation
class SpacecraftAttitude:
    def __init__(self):
        self.orientation = Quaternion()  # Initial orientation
        self.angular_velocity = np.zeros(3)  # rad/s
    
    def apply_torque(self, torque, dt):
        """Apply torque for time dt (simplified dynamics)."""
        # Simplified: assume torque directly changes angular velocity
        self.angular_velocity += torque * dt
        
        # Integrate orientation
        self.orientation.integrate(self.angular_velocity, dt)
    
    def get_attitude(self):
        """Get current attitude as rotation matrix."""
        return self.orientation.rotation_matrix

# Usage
spacecraft = SpacecraftAttitude()

# Apply control torques
control_torque = [0.1, 0, 0]  # Torque about x-axis
dt = 0.01  # 10ms control timestep

for i in range(1000):  # 10 seconds of simulation
    spacecraft.apply_torque(control_torque, dt)
    
    if i % 100 == 0:  # Print every second
        time = i * dt
        angle = spacecraft.orientation.degrees
        print(f"t={time:.1f}s: attitude angle={angle:.1f}°")