3D Rotation

def rotate(self, vector):
    """
    Rotate a 3D vector by the rotation stored in the quaternion.
    
    Parameters:
        vector: 3-element sequence (list, tuple, numpy array) or Quaternion
               For sequences: [x, y, z] coordinates
               For Quaternion: uses vector part as 3D vector
    
    Returns:
        Same type as input: Rotated vector
        - list input → list output
        - tuple input → tuple output  
        - numpy array → numpy array
        - Quaternion → Quaternion (rotated vector part)
    
    Raises:
        TypeError: If vector elements cannot be converted to real numbers
        ValueError: If vector cannot be interpreted as 3-vector
    """

from pyquaternion import Quaternion
import numpy as np

# 90-degree rotation about Y-axis
q = Quaternion(axis=[0, 1, 0], degrees=90)

# Rotate different vector types
vector_list = [1, 0, 0]
rotated_list = q.rotate(vector_list)        # [0.0, 0.0, -1.0]

vector_tuple = (1, 0, 0)
rotated_tuple = q.rotate(vector_tuple)      # (0.0, 0.0, -1.0)

vector_array = np.array([1, 0, 0])
rotated_array = q.rotate(vector_array)      # array([0., 0., -1.])

# Rotate quaternion's vector part
q_vector = Quaternion(vector=[1, 0, 0])
rotated_q = q.rotate(q_vector)              # Quaternion with rotated vector

def get_axis(self, undefined=np.zeros(3)):
    """
    Get the rotation axis with fallback for undefined cases.
    
    For unit quaternions representing rotations, returns the axis of rotation.
    For edge cases (identity quaternion, pure real quaternions), returns 
    the provided fallback value.
    
    Parameters:
        undefined: array-like, default [0, 0, 0]
                  Fallback value when axis is undefined
    
    Returns:
        numpy.ndarray: Unit vector representing rotation axis,
                      or undefined value for edge cases
    
    Note:
        The axis property may return [0, 0, 0] for identity rotations,
        while this method allows custom handling of such cases.
    """

from pyquaternion import Quaternion
import numpy as np

# Regular rotation - has well-defined axis
q = Quaternion(axis=[1, 1, 0], degrees=45)
axis = q.get_axis()
print(f"Rotation axis: {axis}")  # Normalized [1, 1, 0]

# Identity quaternion - no rotation, undefined axis
q_identity = Quaternion()
axis_default = q_identity.get_axis()  # [0, 0, 0]
axis_custom = q_identity.get_axis(undefined=[0, 0, 1])  # [0, 0, 1]

print(f"Identity axis (default): {axis_default}")
print(f"Identity axis (custom): {axis_custom}")

@property
def rotation_matrix(self):
    """
    Get 3x3 rotation matrix equivalent of the quaternion rotation.
    
    The quaternion is automatically normalized to unit length.
    
    Returns:
        numpy.ndarray: 3x3 orthogonal rotation matrix
        
    Note:
        Only meaningful for unit quaternions. Non-unit quaternions
        are automatically normalized before conversion.
    """

@property
def transformation_matrix(self):
    """
    Get 4x4 homogeneous transformation matrix.
    
    Creates a 4x4 matrix with the rotation in the upper-left 3x3 block,
    zero translation, and [0, 0, 0, 1] in the bottom row.
    
    Returns:
        numpy.ndarray: 4x4 homogeneous transformation matrix
    """

# Create rotation quaternion
q = Quaternion(axis=[0, 0, 1], degrees=45)  # 45° about Z-axis

# Get rotation matrix
rot_matrix = q.rotation_matrix
print(f"Rotation matrix shape: {rot_matrix.shape}")  # (3, 3)
print(f"Determinant: {np.linalg.det(rot_matrix)}")   # ~1.0 (orthogonal)

# Get transformation matrix
transform_matrix = q.transformation_matrix
print(f"Transform matrix shape: {transform_matrix.shape}")  # (4, 4)

# Apply to multiple points
points = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]).T  # 3x3
rotated_points = rot_matrix @ points

@property
def yaw_pitch_roll(self):
    """
    Get equivalent yaw-pitch-roll angles (intrinsic Tait-Bryan z-y'-x'').
    
    The quaternion is automatically normalized to unit length.
    
    Returns:
        tuple: (yaw, pitch, roll) angles in radians
               yaw: rotation about z-axis, range [-π, π]
               pitch: rotation about y'-axis, range [-π/2, π/2]  
               roll: rotation about x''-axis, range [-π, π]
               
    Note:
        Rotation matrix equivalent: R = R_x(roll) * R_y(pitch) * R_z(yaw)
    """

# Create quaternion from Euler angles equivalent
q = Quaternion(axis=[0, 0, 1], degrees=30)  # Yaw rotation

# Extract Euler angles
yaw, pitch, roll = q.yaw_pitch_roll
print(f"Yaw: {np.degrees(yaw):.1f}°")     # ~30.0°
print(f"Pitch: {np.degrees(pitch):.1f}°") # ~0.0°
print(f"Roll: {np.degrees(roll):.1f}°")   # ~0.0°

@property
def axis(self):
    """
    Get the unit vector axis of rotation.
    
    Returns:
        numpy.ndarray: 3-element unit vector [x, y, z]
                      Returns [0, 0, 0] for null rotations
    """

def get_axis(self, undefined=np.zeros(3)):
    """
    Get rotation axis with custom fallback for null rotations.
    
    Parameters:
        undefined: 3-element sequence to return for null rotations
                  Default: [0, 0, 0]
    
    Returns:
        numpy.ndarray: 3-element unit vector or undefined value
    """

@property
def angle(self):
    """
    Get rotation angle in radians.
    
    Returns:
        float: Angle in range (-π, π), sign indicates direction
               Magnitude represents rotation amount
               
    Note:
        For 180° rotations, the axis/angle representation may
        jump discontinuously between equivalent representations.
    """

@property
def degrees(self):
    """
    Get rotation angle in degrees.
    
    Returns:
        float: Angle in degrees, converted from radians
    """

@property
def radians(self):
    """Alias for angle property."""

# Create quaternion with known axis and angle
original_axis = [1, 1, 0]  # Will be normalized
original_angle = 60        # degrees
q = Quaternion(axis=original_axis, degrees=original_angle)

# Extract axis and angle
extracted_axis = q.axis
extracted_angle_deg = q.degrees
extracted_angle_rad = q.radians

print(f"Original axis: {np.array(original_axis) / np.linalg.norm(original_axis)}")
print(f"Extracted axis: {extracted_axis}")
print(f"Original angle: {original_angle}°")
print(f"Extracted angle: {extracted_angle_deg:.1f}°")

# Handle null rotation case
identity_q = Quaternion()  # Identity quaternion
null_axis = identity_q.axis  # [0, 0, 0]
null_angle = identity_q.degrees  # 0.0

# Custom undefined axis for null rotations
custom_axis = identity_q.get_axis(undefined=[1, 0, 0])

@property
def polar_unit_vector(self):
    """
    Get the unit vector component of polar decomposition.
    
    Returns:
        numpy.ndarray: Unit vector direction [x, y, z]
    
    Raises:
        ZeroDivisionError: If quaternion is pure real (no unique unit vector)
    """

@property
def polar_angle(self):
    """
    Get the angle component of polar decomposition.
    
    Returns:
        float: Polar angle in radians
    """

@property
def polar_decomposition(self):
    """
    Get complete polar decomposition.
    
    Decomposes quaternion as: q = |q| * exp(unit_vector * angle)
    
    Returns:
        tuple: (unit_vector, angle)
               unit_vector: numpy array [x, y, z]
               angle: float in radians
    
    Raises:
        ZeroDivisionError: For pure real quaternions
    """

# Create non-trivial quaternion
q = Quaternion(axis=[1, 1, 1], degrees=120)

try:
    # Get polar decomposition
    unit_vec, polar_angle = q.polar_decomposition
    print(f"Unit vector: {unit_vec}")
    print(f"Polar angle: {np.degrees(polar_angle):.1f}°")
    
    # Reconstruct quaternion (approximately)
    reconstructed = q.norm * Quaternion.exp(
        Quaternion(vector=unit_vec * polar_angle)
    )
    print(f"Original: {q}")
    print(f"Reconstructed: {reconstructed}")
    
except ZeroDivisionError:
    print("Cannot decompose pure real quaternion")

def _rotate_quaternion(self, q):
    """
    Rotate a quaternion vector using stored rotation.
    
    Parameters:
        q: Quaternion with vector part to rotate (scalar part ignored)
    
    Returns:
        Quaternion: Rotated quaternion using formula: self * q * self.conjugate
    """

def _wrap_angle(self, theta):
    """
    Wrap angle to range (-π, π].
    
    Parameters:
        theta: Angle in radians
    
    Returns:
        float: Wrapped angle, with odd multiples of π mapped to +π
    """