Advanced Mathematics

@classmethod
def exp(cls, q):
    """
    Quaternion exponential function.
    
    Computes exp(q) for any quaternion using the formula:
    exp(q) = exp(q.scalar) * (cos(|q.vector|) + (q.vector/|q.vector|) * sin(|q.vector|))
    
    Parameters:
        q: Quaternion, input quaternion argument
    
    Returns:
        Quaternion: exp(q)
        
    Note:
        Can compute exponential of any quaternion, not just unit quaternions.
        For pure imaginary quaternions, reduces to Euler's formula.
    """

@classmethod
def log(cls, q):
    """
    Quaternion natural logarithm.
    
    Computes log(q) using the formula:
    log(q) = log(|q|) + (q.vector/|q.vector|) * acos(q.scalar/|q|)
    
    Parameters:
        q: Quaternion, input quaternion (should be non-zero)
    
    Returns:
        Quaternion: log(q) = (log(|q|), v/|v| * acos(w/|q|))
                   For zero quaternion: (-inf, nan*vector)
                   For real quaternions: (log(|q|), [0,0,0])
    
    Note:
        Undefined for zero quaternion (returns -inf + nan*i + nan*j + nan*k).
        For real quaternions, returns real logarithm with zero vector part.
    """

from pyquaternion import Quaternion
import numpy as np

# Basic exponential and logarithm
q = Quaternion(1, 0.5, 0.3, 0.2)
exp_q = Quaternion.exp(q)
log_exp_q = Quaternion.log(exp_q)

print(f"Original: {q}")
print(f"exp(q): {exp_q}")
print(f"log(exp(q)): {log_exp_q}")  # Should approximately equal original

# Pure imaginary quaternion (like Euler's formula)
pure_imag = Quaternion(0, 1, 0, 0)  # i
exp_i = Quaternion.exp(pure_imag)
print(f"exp(i): {exp_i}")  # Should be cos(1) + i*sin(1)

# Real quaternion
real_q = Quaternion(2.0)  # Pure real
exp_real = Quaternion.exp(real_q)
log_real = Quaternion.log(real_q)
print(f"exp(2): {exp_real}")  # Should be (e^2, 0, 0, 0)
print(f"log(2): {log_real}")  # Should be (ln(2), 0, 0, 0)

# Zero and problematic cases
try:
    zero_q = Quaternion(0, 0, 0, 0)
    log_zero = Quaternion.log(zero_q)
    print(f"log(0): {log_zero}")  # (-inf, nan, nan, nan)
except:
    print("Zero quaternion logarithm handling")

@classmethod
def exp_map(cls, q, eta):
    """
    Quaternion exponential map on Riemannian manifold.
    
    Computes the endpoint of geodesic starting at q in direction eta,
    with length equal to magnitude of eta.
    
    Parameters:
        q: Quaternion, base point of exponential map
        eta: Quaternion, tangent vector argument (direction and magnitude)
    
    Returns:
        Quaternion: Endpoint quaternion p = q * exp(eta)
        
    Note:
        Important for integrating orientation variations (angular velocities).
        Projects quaternion tangent vectors onto the quaternion manifold.
    """

@classmethod
def sym_exp_map(cls, q, eta):
    """
    Quaternion symmetrized exponential map.
    
    Symmetric formulation analogous to exponential maps for 
    symmetric positive definite tensors.
    
    Parameters:
        q: Quaternion, base point
        eta: Quaternion, tangent vector argument
    
    Returns:
        Quaternion: p = sqrt(q) * exp(eta) * sqrt(q)
    """

@classmethod
def log_map(cls, q, p):
    """
    Quaternion logarithm map on Riemannian manifold.
    
    Finds tangent vector with length and direction given by
    the geodesic joining q and p.
    
    Parameters:
        q: Quaternion, base point where logarithm is computed
        p: Quaternion, target point
    
    Returns:
        Quaternion: Tangent vector from q to p
                   Formula: log(q^(-1) * p)
    """

@classmethod
def sym_log_map(cls, q, p):
    """
    Quaternion symmetrized logarithm map.
    
    Symmetric formulation for numerically stable gradient descent
    on the Riemannian quaternion manifold.
    
    Parameters:
        q: Quaternion, base point
        p: Quaternion, target point
    
    Returns:
        Quaternion: Symmetrized tangent vector
                   Formula: log(q^(-1/2) * p * q^(-1/2))
    """

# Manifold operations for trajectory planning
q_start = Quaternion(axis=[0, 0, 1], degrees=0)
q_end = Quaternion(axis=[0, 0, 1], degrees=90)

# Compute tangent vector from start to end
tangent_vec = Quaternion.log_map(q_start, q_end)
print(f"Tangent vector: {tangent_vec}")

# Move along geodesic using exponential map
fraction = 0.3  # 30% of the way
scaled_tangent = Quaternion(tangent_vec.elements * fraction)
intermediate = Quaternion.exp_map(q_start, scaled_tangent)
print(f"30% along geodesic: {intermediate}")

# Verify round-trip
reconstructed_end = Quaternion.exp_map(q_start, tangent_vec)
print(f"Original end: {q_end}")
print(f"Reconstructed: {reconstructed_end}")

# Symmetrized operations (more numerically stable)
sym_tangent = Quaternion.sym_log_map(q_start, q_end)
sym_intermediate = Quaternion.sym_exp_map(q_start, scaled_tangent)
print(f"Symmetric tangent: {sym_tangent}")
print(f"Symmetric intermediate: {sym_intermediate}")

@classmethod
def absolute_distance(cls, q0, q1):
    """
    Quaternion absolute distance accounting for sign ambiguity.
    
    Computes chord distance of shortest path connecting q0 to q1,
    accounting for the fact that q and -q represent the same rotation.
    
    Parameters:
        q0: Quaternion, first quaternion
        q1: Quaternion, second quaternion
    
    Returns:
        float: Positive chord distance, good indicator for rotation similarity
               Returns min(|q0 - q1|, |q0 + q1|)
               
    Note:
        Does not measure distance on hypersphere, but accounts for
        quaternion double-cover (q ≡ -q for rotations).
    """

@classmethod
def distance(cls, q0, q1):
    """
    Intrinsic geodesic distance between quaternions.
    
    Computes length of geodesic arc connecting q0 to q1 on the
    quaternion manifold.
    
    Parameters:
        q0: Quaternion, first quaternion
        q1: Quaternion, second quaternion
    
    Returns:
        float: Geodesic arc length
               Formula: |log_map(q0, q1).norm|
    """

@classmethod
def sym_distance(cls, q0, q1):
    """
    Symmetrized geodesic distance between quaternions.
    
    More numerically stable for iterative gradient descent on
    Riemannian quaternion manifold.
    
    Parameters:
        q0: Quaternion, first quaternion
        q1: Quaternion, second quaternion
    
    Returns:
        float: Symmetrized geodesic distance
        
    Note:
        Distance between q and -q equals π, making this less useful
        for rotation similarity when samples span angles > π/2.
    """

# Compare different distance metrics
q1 = Quaternion(axis=[1, 0, 0], degrees=30)
q2 = Quaternion(axis=[1, 0, 0], degrees=60)
q3 = Quaternion(axis=[0, 1, 0], degrees=30)  # Different axis

# Compute various distances
abs_dist_12 = Quaternion.absolute_distance(q1, q2)
abs_dist_13 = Quaternion.absolute_distance(q1, q3)

geo_dist_12 = Quaternion.distance(q1, q2)
geo_dist_13 = Quaternion.distance(q1, q3)

sym_dist_12 = Quaternion.sym_distance(q1, q2)
sym_dist_13 = Quaternion.sym_distance(q1, q3)

print("Distance comparison:")
print(f"q1 to q2 (same axis, 30° apart):")
print(f"  Absolute: {abs_dist_12:.4f}")
print(f"  Geodesic: {geo_dist_12:.4f}")
print(f"  Symmetric: {sym_dist_12:.4f}")

print(f"q1 to q3 (different axis, same angle):")
print(f"  Absolute: {abs_dist_13:.4f}")
print(f"  Geodesic: {geo_dist_13:.4f}")
print(f"  Symmetric: {sym_dist_13:.4f}")

# Demonstrate sign ambiguity handling
q_pos = Quaternion(1, 0, 0, 0)
q_neg = Quaternion(-1, 0, 0, 0)  # Same rotation, opposite quaternion

regular_dist = (q_pos - q_neg).norm  # Regular Euclidean distance
absolute_dist = Quaternion.absolute_distance(q_pos, q_neg)

print(f"\nSign ambiguity demonstration:")
print(f"q = {q_pos}")
print(f"-q = {q_neg}")
print(f"Regular Euclidean distance: {regular_dist:.4f}")
print(f"Absolute distance: {absolute_dist:.4f}")  # Should be ~0

# Example: Quaternion averaging using distance metrics
def quaternion_mean_iterative(quaternions, max_iterations=100, tolerance=1e-6):
    """
    Compute quaternion mean using iterative optimization on manifold.
    """
    # Initialize with first quaternion
    mean_q = quaternions[0].normalised
    
    for iteration in range(max_iterations):
        # Compute tangent vectors to all quaternions
        tangent_sum = Quaternion(0, 0, 0, 0)
        
        for q in quaternions:
            tangent_vec = Quaternion.log_map(mean_q, q)
            tangent_sum += tangent_vec
        
        # Average tangent vector
        avg_tangent = Quaternion(tangent_sum.elements / len(quaternions))
        
        # Update mean using exponential map
        step_size = 0.5  # Learning rate
        scaled_tangent = Quaternion(avg_tangent.elements * step_size)
        new_mean = Quaternion.exp_map(mean_q, scaled_tangent)
        
        # Check convergence
        update_distance = Quaternion.distance(mean_q, new_mean)
        if update_distance < tolerance:
            break
            
        mean_q = new_mean.normalised
    
    return mean_q

# Usage
sample_quaternions = [
    Quaternion(axis=[1, 0, 0], degrees=10),
    Quaternion(axis=[1, 0, 0], degrees=20),
    Quaternion(axis=[1, 0, 0], degrees=15),
    Quaternion(axis=[1, 0, 0], degrees=12),
]

mean_quaternion = quaternion_mean_iterative(sample_quaternions)
print(f"Mean quaternion: {mean_quaternion}")
print(f"Mean angle: {mean_quaternion.degrees:.1f}°")