Rotation Conversions

def as_rotation_matrix(q):
    """
    Convert quaternions to 3x3 rotation matrices.
    
    Args:
        q (quaternion or quaternion array): Input quaternions (need not be normalized)
        
    Returns:
        ndarray: Rotation matrices with shape q.shape + (3, 3)
        
    Raises:
        ZeroDivisionError: If any input quaternion has zero norm
        
    Notes:
        For quaternion q and vector v, the rotated vector is:
        rotated_v = rotation_matrix @ v.vec
        where v.vec is the 3D vector part of quaternion v.
    """

def from_rotation_matrix(rot, nonorthogonal=True):
    """
    Convert 3x3 rotation matrices to unit quaternions.
    
    Args:
        rot (array_like): Rotation matrices with shape (..., 3, 3)
        nonorthogonal (bool): Use robust algorithm for non-orthogonal matrices (default: True)
        
    Returns:
        quaternion array: Unit quaternions with shape rot.shape[:-2]
        
    Raises:
        LinAlgError: If eigenvalue solutions do not converge (scipy algorithm)
        
    Notes:
        Uses Bar-Itzhack algorithm (scipy.linalg) when nonorthogonal=True for robustness.
        Falls back to Markley algorithm when scipy unavailable or nonorthogonal=False.
    """

def rotate_vectors(R, v, axis=-1):
    """
    Rotate 3D vectors using quaternions.
    
    Args:
        R (quaternion array): Rotation quaternions
        v (array_like): 3D vectors to rotate
        axis (int): Axis of v containing vector components (must have length 3)
        
    Returns:
        ndarray: Rotated vectors with shape R.shape + v.shape
        
    Notes:
        More efficient than individual quaternion products when rotating
        multiple vectors with the same quaternion. Uses matrix multiplication
        internally for vectorized operations.
    """

def as_rotation_vector(q):
    """
    Convert quaternions to axis-angle representation.
    
    Args:
        q (quaternion or quaternion array): Input quaternions (need not be normalized)
        
    Returns:
        ndarray: Rotation vectors with shape q.shape + (3,)
        
    Notes:
        Each vector represents rotation axis with magnitude equal to rotation
        angle in radians. No error for zero-norm quaternions (produces NaN).
    """

def from_rotation_vector(rot):
    """
    Convert axis-angle vectors to unit quaternions.
    
    Args:
        rot (array_like): Rotation vectors with shape (..., 3)
        
    Returns:
        quaternion array: Unit quaternions with shape rot.shape[:-1]
        
    Notes:
        Vector magnitude is rotation angle in radians, direction is rotation axis.
        Uses quaternion exponential for conversion.
    """

def as_euler_angles(q):
    """
    Convert quaternions to Euler angles (ZYZ convention).
    
    Args:
        q (quaternion or quaternion array): Input quaternions (need not be normalized)
        
    Returns:
        ndarray: Euler angles (alpha, beta, gamma) with shape q.shape + (3,)
        
    Raises:
        AllHell: Metaphorically, if you use Euler angles when you shouldn't
        
    Notes:
        Uses ZYZ convention: R = exp(alpha*z/2) * exp(beta*y/2) * exp(gamma*z/2)
        Angles in radians. Euler angles have singularities and are generally
        discouraged. See package documentation warnings about Euler angles.
    """

def from_euler_angles(alpha_beta_gamma, beta=None, gamma=None):
    """
    Create quaternions from Euler angles (ZYZ convention).
    
    Args:
        alpha_beta_gamma (array_like): Euler angles array with last dim 3, or alpha values
        beta (array_like, optional): Beta angles if alpha_beta_gamma contains only alpha  
        gamma (array_like, optional): Gamma angles if alpha_beta_gamma contains only alpha
        
    Returns:
        quaternion array: Quaternions from Euler angle inputs
        
    Notes:
        Uses ZYZ convention with angles in radians. Inputs must broadcast together.
        Strongly consider using other rotation representations instead.
    """

def as_spherical_coords(q):
    """
    Convert quaternions to spherical coordinates.
    
    Args:
        q (quaternion or quaternion array): Input quaternions (must be nonzero)
        
    Returns:
        ndarray: Spherical coordinates (theta, phi) with shape q.shape + (2,)
        
    Notes:
        Returns point on sphere where quaternion rotates z-axis. Loses some
        information compared to full quaternion representation.
    """

def from_spherical_coords(theta_phi, phi=None):
    """
    Create quaternions from spherical coordinates.
    
    Args:
        theta_phi (array_like): Coordinate array with last dim 2, or theta values
        phi (array_like, optional): Phi coordinates if theta_phi contains only theta
        
    Returns:
        quaternion array: Quaternions rotating z-axis to specified coordinates
        
    Notes:
        Creates quaternion R = exp(phi*z/2) * exp(theta*y/2). Angles in radians.
        Rotates z-axis to given spherical coordinates and x,y to local tangent basis.
    """

import quaternion
import numpy as np

# Create sample quaternions
q = quaternion.quaternion(1, 0, 0, 0)  # Identity
q_rot = quaternion.from_euler_angles(0, np.pi/4, 0)  # 45° rotation around y

# Convert to rotation matrix
R = quaternion.as_rotation_matrix(q_rot)
print(f"Rotation matrix shape: {R.shape}")  # (3, 3)

# Rotate vectors
vectors = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])  # Identity matrix columns
rotated = quaternion.rotate_vectors(q_rot, vectors.T, axis=0)
print(f"Rotated vectors shape: {rotated.shape}")

# Convert to axis-angle representation  
axis_angle = quaternion.as_rotation_vector(q_rot)
print(f"Axis-angle vector: {axis_angle}")

# Round-trip conversion
q_recovered = quaternion.from_rotation_vector(axis_angle)
print(f"Original close to recovered: {quaternion.allclose(q_rot, q_recovered)}")

# Convert to spherical coordinates
theta_phi = quaternion.as_spherical_coords(q_rot)
print(f"Spherical coords: theta={theta_phi[0]:.3f}, phi={theta_phi[1]:.3f}")

# Create from rotation matrix
R_input = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])  # 90° rotation around z
q_from_matrix = quaternion.from_rotation_matrix(R_input)
print(f"Quaternion from matrix: {q_from_matrix}")

# Demonstrate quaternion-matrix equivalence
test_vector = np.array([1, 0, 0])
rotated_by_q = quaternion.as_vector_part(q_from_matrix * quaternion.from_vector_part(test_vector) * q_from_matrix.conjugate())
rotated_by_matrix = R_input @ test_vector
print(f"Quaternion rotation: {rotated_by_q}")
print(f"Matrix rotation: {rotated_by_matrix}")
print(f"Results match: {np.allclose(rotated_by_q, rotated_by_matrix)}")