featherstone 0.1.0

//! Spatial vector algebra for articulated body dynamics
//!
//! Implements 6D spatial vectors (Plücker coordinates) following Featherstone's
//! "Rigid Body Dynamics Algorithms" (2008), Chapter 2.
//!
//! Spatial vectors unify linear and angular quantities:
//! - **Motion vectors** (twists): [ω; v] — angular velocity on top, linear on bottom
//! - **Force vectors** (wrenches): [n; f] — torque on top, force on bottom
//!
//! The spatial cross products relate them:
//! - `v ×` (motion cross): for differentiating motion vectors
//! - `v ×*` (force cross): for transforming forces, = -(v ×)ᵀ

use nalgebra::{Matrix3, Matrix6, Vector3, Vector6};

// ============================================================================
// Spatial Vector (6D)
// ============================================================================

/// 6D spatial vector — used for both motion and force vectors.
///
/// Convention (Featherstone):
/// - Top 3 elements: angular component (ω or τ)
/// - Bottom 3 elements: linear component (v or f)
#[derive(Clone, Debug, Default)]
pub struct SpatialVector {
    /// Internal 6D vector: [angular(3); linear(3)]
    pub data: Vector6<f32>,
}

impl SpatialVector {
    /// Create from angular and linear components
    pub fn new(angular: Vector3<f32>, linear: Vector3<f32>) -> Self {
        Self {
            data: Vector6::new(
                angular.x, angular.y, angular.z, linear.x, linear.y, linear.z,
            ),
        }
    }

    /// Create zero spatial vector
    pub fn zero() -> Self {
        Self {
            data: Vector6::zeros(),
        }
    }

    /// Create from raw 6D vector
    pub fn from_vector(v: Vector6<f32>) -> Self {
        Self { data: v }
    }

    /// Angular component (top 3)
    pub fn angular(&self) -> Vector3<f32> {
        Vector3::new(self.data[0], self.data[1], self.data[2])
    }

    /// Linear component (bottom 3)
    pub fn linear(&self) -> Vector3<f32> {
        Vector3::new(self.data[3], self.data[4], self.data[5])
    }

    /// Set angular component
    pub fn set_angular(&mut self, w: Vector3<f32>) {
        self.data[0] = w.x;
        self.data[1] = w.y;
        self.data[2] = w.z;
    }

    /// Set linear component
    pub fn set_linear(&mut self, v: Vector3<f32>) {
        self.data[3] = v.x;
        self.data[4] = v.y;
        self.data[5] = v.z;
    }

    /// Spatial cross product for motion vectors: v × (motion cross)
    ///
    /// ```text
    /// [ω]   [ω×   0 ] [ω']   [ω × ω'        ]
    /// [v] × [v×  ω× ] [v'] = [v × ω' + ω × v']
    /// ```
    pub fn cross_motion(&self, other: &SpatialVector) -> SpatialVector {
        let w = self.angular();
        let v = self.linear();
        let w2 = other.angular();
        let v2 = other.linear();

        SpatialVector::new(w.cross(&w2), v.cross(&w2) + w.cross(&v2))
    }

    /// Spatial cross product for force vectors: v ×* (force cross)
    ///
    /// ```text
    /// [ω]     [ω×  v×] [n]   [ω × n + v × f]
    /// [v] ×*  [0   ω×] [f] = [ω × f         ]
    /// ```
    ///
    /// Duality: v ×* f = -(v ×)ᵀ f
    pub fn cross_force(&self, other: &SpatialVector) -> SpatialVector {
        let w = self.angular();
        let v = self.linear();
        let n = other.angular();
        let f = other.linear();

        SpatialVector::new(w.cross(&n) + v.cross(&f), w.cross(&f))
    }

    /// Dot product of two spatial vectors
    pub fn dot(&self, other: &SpatialVector) -> f32 {
        self.data.dot(&other.data)
    }

    /// Squared norm
    pub fn norm_squared(&self) -> f32 {
        self.data.norm_squared()
    }

    /// Norm
    pub fn norm(&self) -> f32 {
        self.data.norm()
    }

    /// Scale by scalar
    pub fn scale(&self, s: f32) -> SpatialVector {
        SpatialVector::from_vector(self.data * s)
    }

    /// Add two spatial vectors
    pub fn add(&self, other: &SpatialVector) -> SpatialVector {
        SpatialVector::from_vector(self.data + other.data)
    }

    /// Subtract two spatial vectors
    pub fn sub(&self, other: &SpatialVector) -> SpatialVector {
        SpatialVector::from_vector(self.data - other.data)
    }

    /// Negate
    pub fn neg(&self) -> SpatialVector {
        SpatialVector::from_vector(-self.data)
    }

    /// Build the 6×6 spatial cross-product matrix for motion vectors
    ///
    /// Returns the matrix [v×] such that [v×] * u = v.cross_motion(u)
    pub fn cross_motion_matrix(&self) -> Matrix6<f32> {
        let w = self.angular();
        let v = self.linear();
        let wx = skew(&w);
        let vx = skew(&v);
        let z = Matrix3::zeros();

        // [ω×   0 ]
        // [v×  ω× ]
        compose_spatial_matrix(&wx, &z, &vx, &wx)
    }

    /// Build the 6×6 spatial cross-product matrix for force vectors
    ///
    /// Returns the matrix [v×*] such that [v×*] * f = v.cross_force(f)
    /// Note: [v×*] = -[v×]ᵀ
    pub fn cross_force_matrix(&self) -> Matrix6<f32> {
        let w = self.angular();
        let v = self.linear();
        let wx = skew(&w);
        let vx = skew(&v);
        let z = Matrix3::zeros();

        // [ω×  v×]
        // [0   ω×]
        compose_spatial_matrix(&wx, &vx, &z, &wx)
    }
}

// Operator implementations for ergonomic arithmetic
impl std::ops::Add for &SpatialVector {
    type Output = SpatialVector;
    fn add(self, rhs: &SpatialVector) -> SpatialVector {
        SpatialVector::add(self, rhs)
    }
}

impl std::ops::Sub for &SpatialVector {
    type Output = SpatialVector;
    fn sub(self, rhs: &SpatialVector) -> SpatialVector {
        SpatialVector::sub(self, rhs)
    }
}

impl std::ops::Neg for &SpatialVector {
    type Output = SpatialVector;
    fn neg(self) -> SpatialVector {
        SpatialVector::neg(self)
    }
}

impl std::ops::Mul<f32> for &SpatialVector {
    type Output = SpatialVector;
    fn mul(self, rhs: f32) -> SpatialVector {
        self.scale(rhs)
    }
}

// ============================================================================
// Spatial Inertia (6×6)
// ============================================================================

/// 6×6 spatial inertia matrix for a rigid body.
///
/// Represents the inertia of a rigid body in spatial (Plücker) coordinates:
///
/// ```text
/// I_sp = [ I + m*c×*c×ᵀ   m*c× ]
///        [ m*c×ᵀ           m*1  ]
/// ```
///
/// Where:
/// - m: mass
/// - c: center of mass offset from body frame origin
/// - I: rotational inertia about body frame origin (3×3)
#[derive(Clone, Debug)]
pub struct SpatialInertia {
    /// Internal 6×6 matrix
    pub data: Matrix6<f32>,
    /// Mass (cached for quick access)
    pub mass: f32,
}

impl Default for SpatialInertia {
    fn default() -> Self {
        Self {
            data: Matrix6::zeros(),
            mass: 0.0,
        }
    }
}

impl SpatialInertia {
    /// Create spatial inertia from mass, center of mass offset, and 3×3 rotational inertia.
    ///
    /// # Arguments
    /// - `mass`: Body mass (kg)
    /// - `com`: Center of mass offset from body frame origin
    /// - `inertia`: 3×3 rotational inertia tensor about the body frame origin
    pub fn from_mass_inertia(mass: f32, com: Vector3<f32>, inertia: Matrix3<f32>) -> Self {
        let cx = skew(&com);
        let cx_t = cx.transpose();
        let m_eye = Matrix3::identity() * mass;

        // I_sp = [ I + m*cx*cxᵀ   m*cx ]
        //        [ m*cxᵀ           m*1  ]
        let top_left = inertia + cx * cx_t * mass;
        let top_right = cx * mass;
        let bottom_left = cx_t * mass;
        let bottom_right = m_eye;

        Self {
            data: compose_spatial_matrix(&top_left, &top_right, &bottom_left, &bottom_right),
            mass,
        }
    }

    /// Create spatial inertia for a body with COM at the body frame origin.
    ///
    /// Simplified case where c = 0:
    /// ```text
    /// I_sp = [ I   0   ]
    ///        [ 0   m*1 ]
    /// ```
    pub fn from_mass_inertia_at_com(mass: f32, inertia: Matrix3<f32>) -> Self {
        Self::from_mass_inertia(mass, Vector3::zeros(), inertia)
    }

    /// Create spatial inertia for a uniform sphere
    pub fn sphere(mass: f32, radius: f32) -> Self {
        let i = 2.0 / 5.0 * mass * radius * radius;
        let inertia = Matrix3::from_diagonal(&Vector3::new(i, i, i));
        Self::from_mass_inertia_at_com(mass, inertia)
    }

    /// Create spatial inertia for a uniform box
    pub fn cuboid(mass: f32, half_extents: Vector3<f32>) -> Self {
        let x2 = (2.0 * half_extents.x).powi(2);
        let y2 = (2.0 * half_extents.y).powi(2);
        let z2 = (2.0 * half_extents.z).powi(2);
        let inertia = Matrix3::from_diagonal(&Vector3::new(
            mass / 12.0 * (y2 + z2),
            mass / 12.0 * (x2 + z2),
            mass / 12.0 * (x2 + y2),
        ));
        Self::from_mass_inertia_at_com(mass, inertia)
    }

    /// Create spatial inertia for a uniform cylinder (along Z axis)
    pub fn cylinder(mass: f32, radius: f32, half_height: f32) -> Self {
        let r2 = radius * radius;
        let h2 = (2.0 * half_height).powi(2);
        let iz = 0.5 * mass * r2;
        let ixy = mass / 12.0 * (3.0 * r2 + h2);
        let inertia = Matrix3::from_diagonal(&Vector3::new(ixy, ixy, iz));
        Self::from_mass_inertia_at_com(mass, inertia)
    }

    /// Zero spatial inertia (for fixed/massless bodies)
    pub fn zero() -> Self {
        Self::default()
    }

    /// Multiply spatial inertia by a motion vector to get a force vector: f = I * v
    pub fn mul_vector(&self, v: &SpatialVector) -> SpatialVector {
        SpatialVector::from_vector(self.data * v.data)
    }

    /// Add two spatial inertias (for composite rigid body)
    pub fn add(&self, other: &SpatialInertia) -> SpatialInertia {
        SpatialInertia {
            data: self.data + other.data,
            mass: self.mass + other.mass,
        }
    }

    /// Subtract spatial inertia
    pub fn sub(&self, other: &SpatialInertia) -> SpatialInertia {
        SpatialInertia {
            data: self.data - other.data,
            mass: self.mass - other.mass,
        }
    }

    /// Check if this inertia is physically valid (symmetric, positive semi-definite)
    pub fn is_valid(&self) -> bool {
        // Check symmetry
        let diff = (self.data - self.data.transpose()).norm();
        if diff > 1e-6 {
            return false;
        }
        // Check mass is non-negative
        if self.mass < 0.0 {
            return false;
        }
        // Check positive semi-definite via eigenvalues
        let eigenvalues = self.data.symmetric_eigenvalues();
        eigenvalues.iter().all(|&ev| ev >= -1e-8)
    }
}

// ============================================================================
// Spatial Transform (Plücker Transform)
// ============================================================================

/// Spatial coordinate transform (Plücker transform).
///
/// Transforms spatial vectors between coordinate frames:
///
/// ```text
/// X = [ R     0   ]     (for motion vectors)
///     [ -r×R  R   ]
///
/// X* = [ R    -r×R ]    (for force vectors, = X⁻ᵀ)
///      [ 0     R   ]
/// ```
///
/// Where R is a 3×3 rotation and r is a 3D translation.
#[derive(Clone, Debug)]
pub struct SpatialTransform {
    /// 3×3 rotation matrix
    pub rotation: Matrix3<f32>,
    /// 3D translation vector
    pub translation: Vector3<f32>,
}

impl Default for SpatialTransform {
    fn default() -> Self {
        Self {
            rotation: Matrix3::identity(),
            translation: Vector3::zeros(),
        }
    }
}

impl SpatialTransform {
    /// Create identity transform
    pub fn identity() -> Self {
        Self::default()
    }

    /// Create from rotation matrix and translation
    pub fn from_rotation_translation(rotation: Matrix3<f32>, translation: Vector3<f32>) -> Self {
        Self {
            rotation,
            translation,
        }
    }

    /// Create from axis-angle rotation and translation
    pub fn from_axis_angle_translation(axis: Vector3<f32>, angle: f32, translation: Vector3<f32>) -> Self {
        let rotation = rotation_from_axis_angle(&axis, angle);
        Self {
            rotation,
            translation,
        }
    }

    /// Create pure rotation (no translation)
    pub fn from_rotation(rotation: Matrix3<f32>) -> Self {
        Self {
            rotation,
            translation: Vector3::zeros(),
        }
    }

    /// Create pure translation (no rotation)
    pub fn from_translation(translation: Vector3<f32>) -> Self {
        Self {
            rotation: Matrix3::identity(),
            translation,
        }
    }

    /// Create rotation around X axis
    pub fn rot_x(angle: f32) -> Self {
        Self::from_axis_angle_translation(Vector3::x(), angle, Vector3::zeros())
    }

    /// Create rotation around Y axis
    pub fn rot_y(angle: f32) -> Self {
        Self::from_axis_angle_translation(Vector3::y(), angle, Vector3::zeros())
    }

    /// Create rotation around Z axis
    pub fn rot_z(angle: f32) -> Self {
        Self::from_axis_angle_translation(Vector3::z(), angle, Vector3::zeros())
    }

    /// Apply transform to a motion vector: v' = X * v
    ///
    /// ```text
    /// [ω']   [ R     0 ] [ω]   [R*ω              ]
    /// [v'] = [-r×R   R ] [v] = [-r×R*ω + R*v     ]
    /// ```
    pub fn apply_motion(&self, v: &SpatialVector) -> SpatialVector {
        let w = v.angular();
        let vel = v.linear();
        let rx = skew(&self.translation);

        let new_angular = self.rotation * w;
        let new_linear = -rx * (self.rotation * w) + self.rotation * vel;

        SpatialVector::new(new_angular, new_linear)
    }

    /// Apply transform to a force vector: f' = X* f = X⁻ᵀ f
    ///
    /// ```text
    /// [n']   [ R   -r×R] [n]   [R*n - r×R*f]
    /// [f'] = [ 0    R  ] [f] = [R*f         ]
    /// ```
    pub fn apply_force(&self, f: &SpatialVector) -> SpatialVector {
        let n = f.angular();
        let force = f.linear();
        let rx = skew(&self.translation);

        let new_angular = self.rotation * n - rx * (self.rotation * force);
        let new_linear = self.rotation * force;

        SpatialVector::new(new_angular, new_linear)
    }

    /// Transform a spatial inertia to a new frame.
    ///
    /// Uses the identity: I' = X^{*} I X^{-1}
    /// where X^{*} is the force transform and X^{-1} is the inverse motion transform.
    pub fn apply_inertia(&self, inertia: &SpatialInertia) -> SpatialInertia {
        let x_inv = self.inverse();
        let x_inv_motion = x_inv.to_matrix_motion();
        let x_force = self.to_matrix_force();
        // I' = X^* · I · X^{-1}  (force transform on left, inverse motion on right)
        // This computes I_A = X_{B→A}^{-T} * I_B * X_{B→A}^{-1} when called on X_{A→B}
        let new_data = x_force * inertia.data * x_inv_motion;
        // Symmetrize to remove floating-point drift
        let sym = (new_data + new_data.transpose()) * 0.5;
        SpatialInertia {
            data: sym,
            mass: inertia.mass,
        }
    }

    /// Inverse transform: X⁻¹
    pub fn inverse(&self) -> SpatialTransform {
        let r_inv = self.rotation.transpose();
        SpatialTransform {
            rotation: r_inv,
            translation: -(r_inv * self.translation),
        }
    }

    /// Compose two transforms: X_result = X_self * X_other
    ///
    /// Applies other first, then self.
    pub fn compose(&self, other: &SpatialTransform) -> SpatialTransform {
        SpatialTransform {
            rotation: self.rotation * other.rotation,
            translation: self.rotation * other.translation + self.translation,
        }
    }

    /// Build the 6×6 motion transform matrix
    pub fn to_matrix_motion(&self) -> Matrix6<f32> {
        let rx = skew(&self.translation);
        let z = Matrix3::zeros();
        let neg_rx_r = -rx * self.rotation;

        compose_spatial_matrix(&self.rotation, &z, &neg_rx_r, &self.rotation)
    }

    /// Build the 6×6 force transform matrix (= X⁻ᵀ)
    pub fn to_matrix_force(&self) -> Matrix6<f32> {
        let rx = skew(&self.translation);
        let z = Matrix3::zeros();
        let neg_rx_r = -rx * self.rotation;

        compose_spatial_matrix(&self.rotation, &neg_rx_r, &z, &self.rotation)
    }
}

// ============================================================================
// Helper Functions
// ============================================================================

/// Skew-symmetric matrix from a 3D vector: `[v]x` such that `[v]x * u = v x u`.
pub fn skew(v: &Vector3<f32>) -> Matrix3<f32> {
    Matrix3::new(
        0.0, -v.z, v.y, //
        v.z, 0.0, -v.x, //
        -v.y, v.x, 0.0, //
    )
}

/// Compose a 6×6 matrix from four 3×3 blocks
///
/// ```text
/// [ A  B ]
/// [ C  D ]
/// ```
fn compose_spatial_matrix(
    a: &Matrix3<f32>,
    b: &Matrix3<f32>,
    c: &Matrix3<f32>,
    d: &Matrix3<f32>,
) -> Matrix6<f32> {
    let mut m = Matrix6::zeros();

    // Top-left (A)
    for i in 0..3 {
        for j in 0..3 {
            m[(i, j)] = a[(i, j)];
        }
    }
    // Top-right (B)
    for i in 0..3 {
        for j in 0..3 {
            m[(i, j + 3)] = b[(i, j)];
        }
    }
    // Bottom-left (C)
    for i in 0..3 {
        for j in 0..3 {
            m[(i + 3, j)] = c[(i, j)];
        }
    }
    // Bottom-right (D)
    for i in 0..3 {
        for j in 0..3 {
            m[(i + 3, j + 3)] = d[(i, j)];
        }
    }

    m
}

/// Rotation matrix from axis-angle (Rodrigues' formula)
fn rotation_from_axis_angle(axis: &Vector3<f32>, angle: f32) -> Matrix3<f32> {
    if angle.abs() < 1e-12 {
        return Matrix3::identity();
    }
    if axis.norm_squared() < 1e-12 {
        return Matrix3::identity();
    }
    let k = axis.normalize();
    let kx = skew(&k);
    let (s, c) = angle.sin_cos();

    Matrix3::identity() + kx * s + kx * kx * (1.0 - c)
}

// ============================================================================
// Tests
// ============================================================================

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;
    use std::f32::consts::PI;

    #[test]
    fn test_spatial_vector_create() {
        let v = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        assert_eq!(v.angular(), Vector3::new(1.0, 2.0, 3.0));
        assert_eq!(v.linear(), Vector3::new(4.0, 5.0, 6.0));
    }

    #[test]
    fn test_spatial_vector_zero() {
        let v = SpatialVector::zero();
        assert_eq!(v.data, Vector6::zeros());
    }

    #[test]
    fn test_spatial_vector_arithmetic() {
        let a = SpatialVector::new(Vector3::new(1.0, 0.0, 0.0), Vector3::new(0.0, 1.0, 0.0));
        let b = SpatialVector::new(Vector3::new(0.0, 1.0, 0.0), Vector3::new(0.0, 0.0, 1.0));

        let sum = &a + &b;
        assert_eq!(sum.angular(), Vector3::new(1.0, 1.0, 0.0));
        assert_eq!(sum.linear(), Vector3::new(0.0, 1.0, 1.0));

        let diff = &a - &b;
        assert_eq!(diff.angular(), Vector3::new(1.0, -1.0, 0.0));
        assert_eq!(diff.linear(), Vector3::new(0.0, 1.0, -1.0));

        let scaled = &a * 3.0;
        assert_eq!(scaled.angular(), Vector3::new(3.0, 0.0, 0.0));
        assert_eq!(scaled.linear(), Vector3::new(0.0, 3.0, 0.0));

        let neg = -&a;
        assert_eq!(neg.angular(), Vector3::new(-1.0, 0.0, 0.0));
    }

    #[test]
    fn test_spatial_vector_dot() {
        let a = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        let b = SpatialVector::new(Vector3::new(1.0, 1.0, 1.0), Vector3::new(1.0, 1.0, 1.0));
        // dot = 1+2+3+4+5+6 = 21
        assert_relative_eq!(a.dot(&b), 21.0, epsilon = 1e-10);
    }

    #[test]
    fn test_cross_motion_zero() {
        let v = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        let z = SpatialVector::zero();
        let result = v.cross_motion(&z);
        assert!(result.norm() < 1e-10, "Cross with zero should be zero");
    }

    #[test]
    fn test_cross_motion_self_zero() {
        // v × v should be zero for angular part (ω × ω = 0)
        let v = SpatialVector::new(Vector3::new(1.0, 0.0, 0.0), Vector3::zeros());
        let result = v.cross_motion(&v);
        assert!(result.angular().norm() < 1e-10, "ω × ω should be zero");
    }

    #[test]
    fn test_cross_force_duality() {
        // Duality: v ×* f = -(v ×)ᵀ f
        let v = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        let f = SpatialVector::new(Vector3::new(0.5, 1.5, 2.5), Vector3::new(3.5, 4.5, 5.5));

        let cross_f = v.cross_force(&f);
        let cross_m_mat = v.cross_motion_matrix();
        let neg_transpose = -cross_m_mat.transpose() * f.data;

        for i in 0..6 {
            assert_relative_eq!(cross_f.data[i], neg_transpose[i], epsilon = 1e-6);
        }
    }

    #[test]
    fn test_cross_motion_matrix_consistency() {
        let v = SpatialVector::new(Vector3::new(1.0, -0.5, 0.3), Vector3::new(0.2, 0.7, -0.4));
        let u = SpatialVector::new(Vector3::new(0.5, 1.5, -1.0), Vector3::new(-0.3, 0.8, 0.6));

        let direct = v.cross_motion(&u);
        let via_matrix = SpatialVector::from_vector(v.cross_motion_matrix() * u.data);

        for i in 0..6 {
            assert_relative_eq!(direct.data[i], via_matrix.data[i], epsilon = 1e-6);
        }
    }

    #[test]
    fn test_cross_force_matrix_consistency() {
        let v = SpatialVector::new(Vector3::new(1.0, -0.5, 0.3), Vector3::new(0.2, 0.7, -0.4));
        let f = SpatialVector::new(Vector3::new(0.5, 1.5, -1.0), Vector3::new(-0.3, 0.8, 0.6));

        let direct = v.cross_force(&f);
        let via_matrix = SpatialVector::from_vector(v.cross_force_matrix() * f.data);

        for i in 0..6 {
            assert_relative_eq!(direct.data[i], via_matrix.data[i], epsilon = 1e-6);
        }
    }

    // ========================================================================
    // Spatial Inertia Tests
    // ========================================================================

    #[test]
    fn test_spatial_inertia_sphere() {
        let mass = 2.0_f32;
        let radius = 0.5_f32;
        let si = SpatialInertia::sphere(mass, radius);

        assert_relative_eq!(si.mass, 2.0);
        // I = 2/5 * m * r² = 2/5 * 2 * 0.25 = 0.2
        let expected_i = 2.0 / 5.0 * mass * radius * radius;
        assert_relative_eq!(si.data[(0, 0)], expected_i, epsilon = 1e-6);
        assert_relative_eq!(si.data[(1, 1)], expected_i, epsilon = 1e-6);
        assert_relative_eq!(si.data[(2, 2)], expected_i, epsilon = 1e-6);
        // Mass in bottom-right
        assert_relative_eq!(si.data[(3, 3)], mass, epsilon = 1e-6);
        assert_relative_eq!(si.data[(4, 4)], mass, epsilon = 1e-6);
        assert_relative_eq!(si.data[(5, 5)], mass, epsilon = 1e-6);
        assert!(si.is_valid());
    }

    #[test]
    fn test_spatial_inertia_cuboid() {
        let mass = 3.0_f32;
        let half = Vector3::new(0.5, 0.3, 0.2);
        let si = SpatialInertia::cuboid(mass, half);

        // Ixx = m/12 * (ly² + lz²) = 3/12 * (0.6² + 0.4²) = 0.25*(0.36+0.16) = 0.13
        let lx = 1.0_f32;
        let ly = 0.6_f32;
        let lz = 0.4_f32;
        let ixx = mass / 12.0 * (ly * ly + lz * lz);
        let iyy = mass / 12.0 * (lx * lx + lz * lz);
        let izz = mass / 12.0 * (lx * lx + ly * ly);
        assert_relative_eq!(si.data[(0, 0)], ixx, epsilon = 1e-6);
        assert_relative_eq!(si.data[(1, 1)], iyy, epsilon = 1e-6);
        assert_relative_eq!(si.data[(2, 2)], izz, epsilon = 1e-6);
        assert!(si.is_valid());
    }

    #[test]
    fn test_spatial_inertia_cylinder() {
        let mass = 1.5_f32;
        let radius = 0.1_f32;
        let half_h = 0.3_f32;
        let si = SpatialInertia::cylinder(mass, radius, half_h);

        let r2 = radius * radius;
        let h2 = (2.0 * half_h).powi(2);
        let iz = 0.5 * mass * r2;
        let ixy = mass / 12.0 * (3.0 * r2 + h2);
        assert_relative_eq!(si.data[(0, 0)], ixy, epsilon = 1e-6);
        assert_relative_eq!(si.data[(2, 2)], iz, epsilon = 1e-6);
        assert!(si.is_valid());
    }

    #[test]
    fn test_spatial_inertia_symmetry() {
        let si = SpatialInertia::from_mass_inertia(
            2.0,
            Vector3::new(0.1, 0.2, 0.3),
            Matrix3::from_diagonal(&Vector3::new(0.5, 0.4, 0.3)),
        );

        for i in 0..6 {
            for j in 0..6 {
                assert_relative_eq!(si.data[(i, j)], si.data[(j, i)], epsilon = 1e-6);
            }
        }
        assert!(si.is_valid());
    }

    #[test]
    fn test_spatial_inertia_mul_vector() {
        // For a point mass at origin: I * [ω; 0] = [I*ω; 0]
        let si = SpatialInertia::from_mass_inertia_at_com(
            1.0,
            Matrix3::identity(),
        );
        let v = SpatialVector::new(Vector3::new(1.0, 0.0, 0.0), Vector3::zeros());
        let result = si.mul_vector(&v);
        assert_relative_eq!(result.angular().x, 1.0, epsilon = 1e-6);
    }

    #[test]
    fn test_spatial_inertia_add() {
        let a = SpatialInertia::sphere(1.0, 0.1);
        let b = SpatialInertia::sphere(2.0, 0.1);
        let sum = a.add(&b);
        assert_relative_eq!(sum.mass, 3.0, epsilon = 1e-10);
        assert!(sum.is_valid());
    }

    // ========================================================================
    // Spatial Transform Tests
    // ========================================================================

    #[test]
    fn test_transform_identity() {
        let x = SpatialTransform::identity();
        let v = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        let result = x.apply_motion(&v);

        for i in 0..6 {
            assert_relative_eq!(result.data[i], v.data[i], epsilon = 1e-10);
        }
    }

    #[test]
    fn test_transform_inverse_round_trip() {
        let x = SpatialTransform::from_axis_angle_translation(
            Vector3::new(1.0, 1.0, 0.0).normalize(),
            0.7,
            Vector3::new(0.5, -0.3, 0.1),
        );
        let x_inv = x.inverse();
        let composed = x.compose(&x_inv);

        // Should be identity
        let v = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        let result = composed.apply_motion(&v);

        for i in 0..6 {
            assert_relative_eq!(result.data[i], v.data[i], epsilon = 1e-5);
        }
    }

    #[test]
    fn test_transform_compose_associative() {
        let a = SpatialTransform::rot_x(0.3);
        let b = SpatialTransform::from_translation(Vector3::new(1.0, 0.0, 0.0));
        let c = SpatialTransform::rot_z(0.5);

        let ab_c = a.compose(&b).compose(&c);
        let a_bc = a.compose(&b.compose(&c));

        let v = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        let r1 = ab_c.apply_motion(&v);
        let r2 = a_bc.apply_motion(&v);

        for i in 0..6 {
            assert_relative_eq!(r1.data[i], r2.data[i], epsilon = 1e-5);
        }
    }

    #[test]
    fn test_transform_pure_rotation() {
        let x = SpatialTransform::rot_z(PI / 2.0);
        // Pure angular velocity around Z should stay around Z
        let v = SpatialVector::new(Vector3::new(0.0, 0.0, 1.0), Vector3::zeros());
        let result = x.apply_motion(&v);
        assert_relative_eq!(result.angular().z, 1.0, epsilon = 1e-6);
        assert!(result.linear().norm() < 1e-6);
    }

    #[test]
    fn test_transform_pure_translation() {
        let x = SpatialTransform::from_translation(Vector3::new(1.0, 0.0, 0.0));
        // Rotation around Z at origin, translated by (1,0,0):
        // linear velocity at new origin = ω × (-r) = (0,0,1) × (-1,0,0) = (0,1,0)... wait
        // Actually: v' = -r × R*ω + R*v = -(1,0,0)×(0,0,1) + 0 = (0*1-0*0, 0*0-(-1)*1, (-1)*0-0*0)
        // skew([1,0,0]) * [0,0,1] = [0*1-0*0, 0*0-1*1, 1*0-0*0] = [0, -1, 0]
        // -skew * Rω = -[0,-1,0] = [0,1,0]
        let v = SpatialVector::new(Vector3::new(0.0, 0.0, 1.0), Vector3::zeros());
        let result = x.apply_motion(&v);
        assert_relative_eq!(result.angular().z, 1.0, epsilon = 1e-6);
        assert_relative_eq!(result.linear().y, 1.0, epsilon = 1e-6);
    }

    #[test]
    fn test_transform_force_motion_duality() {
        // X* should equal X⁻ᵀ
        let x = SpatialTransform::from_axis_angle_translation(
            Vector3::z(),
            0.5,
            Vector3::new(0.3, 0.2, 0.1),
        );

        let x_motion_mat = x.to_matrix_motion();
        let x_force_mat = x.to_matrix_force();
        let x_inv_t = x_motion_mat
            .try_inverse()
            .expect("Motion matrix should be invertible")
            .transpose();

        for i in 0..6 {
            for j in 0..6 {
                assert_relative_eq!(x_force_mat[(i, j)], x_inv_t[(i, j)], epsilon = 1e-5);
            }
        }
    }

    #[test]
    fn test_transform_inertia() {
        let si = SpatialInertia::sphere(2.0, 0.3);
        let x = SpatialTransform::from_translation(Vector3::new(1.0, 0.0, 0.0));
        let si_transformed = x.apply_inertia(&si);

        // Mass should be preserved
        assert_relative_eq!(si_transformed.mass, si.mass, epsilon = 1e-10);
        // Should still be valid
        assert!(si_transformed.is_valid());
    }

    // ========================================================================
    // Skew matrix test
    // ========================================================================

    #[test]
    fn test_skew_cross_product() {
        let a = Vector3::new(1.0, 2.0, 3.0);
        let b = Vector3::new(4.0, 5.0, 6.0);
        let cross_direct = a.cross(&b);
        let cross_via_skew = skew(&a) * b;

        assert_relative_eq!(cross_direct.x, cross_via_skew.x, epsilon = 1e-10);
        assert_relative_eq!(cross_direct.y, cross_via_skew.y, epsilon = 1e-10);
        assert_relative_eq!(cross_direct.z, cross_via_skew.z, epsilon = 1e-10);
    }

    #[test]
    fn test_skew_antisymmetric() {
        let v = Vector3::new(1.0, 2.0, 3.0);
        let s = skew(&v);
        let diff = s + s.transpose();
        assert!(diff.norm() < 1e-10, "Skew matrix should be antisymmetric");
    }

    // ======================================================================
    // Property tests (proptest)
    // ======================================================================

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_transform_inverse_roundtrip(
            tx in -5.0f32..5.0, ty in -5.0f32..5.0, tz in -5.0f32..5.0,
            angle in -3.14f32..3.14,
        ) {
            let axis = Vector3::new(0.0, 0.0, 1.0);
            let rot = nalgebra::Rotation3::new(axis * angle);
            let x = SpatialTransform {
                rotation: *rot.matrix(),
                translation: Vector3::new(tx, ty, tz),
            };

            let x_inv = x.inverse();
            let identity = x.compose(&x_inv);

            // Should be close to identity
            let rot_diff = (identity.rotation - Matrix3::identity()).norm();
            let trans_diff = identity.translation.norm();
            prop_assert!(rot_diff < 1e-4, "Rotation not identity: diff={rot_diff}");
            prop_assert!(trans_diff < 1e-4, "Translation not zero: diff={trans_diff}");
        }

        #[test]
        fn prop_spatial_inertia_mass_preserved_under_transform(
            mass in 0.1f32..10.0,
            radius in 0.01f32..1.0,
            tx in -2.0f32..2.0,
        ) {
            let si = SpatialInertia::sphere(mass, radius);
            let x = SpatialTransform::from_translation(Vector3::new(tx, 0.0, 0.0));
            let si2 = x.apply_inertia(&si);

            prop_assert!((si2.mass - mass).abs() < 1e-5,
                "Mass should be preserved: {} vs {}", si2.mass, mass);
        }

        #[test]
        fn prop_cross_motion_self_is_zero(
            ax in -5.0f32..5.0, ay in -5.0f32..5.0, az in -5.0f32..5.0,
            lx in -5.0f32..5.0, ly in -5.0f32..5.0, lz in -5.0f32..5.0,
        ) {
            let v = SpatialVector::new(
                Vector3::new(ax, ay, az),
                Vector3::new(lx, ly, lz),
            );
            let result = v.cross_motion(&v);
            prop_assert!(result.data.norm() < 1e-4,
                "v × v should be zero, got norm={}", result.data.norm());
        }

        #[test]
        fn prop_skew_antisymmetric(
            x in -10.0f32..10.0, y in -10.0f32..10.0, z in -10.0f32..10.0,
        ) {
            let v = Vector3::new(x, y, z);
            let s = skew(&v);
            let diff = (s + s.transpose()).norm();
            prop_assert!(diff < 1e-10, "Skew should be antisymmetric: diff={diff}");
        }
    }

    #[test]
    fn test_set_angular_overwrites_top_three() {
        let mut sv = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        sv.set_angular(Vector3::new(10.0, 20.0, 30.0));
        assert_relative_eq!(sv.angular(), Vector3::new(10.0, 20.0, 30.0), epsilon = 1e-10);
        // Linear must be unchanged
        assert_relative_eq!(sv.linear(), Vector3::new(4.0, 5.0, 6.0), epsilon = 1e-10);
    }

    #[test]
    fn test_set_linear_overwrites_bottom_three() {
        let mut sv = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        sv.set_linear(Vector3::new(40.0, 50.0, 60.0));
        // Angular must be unchanged
        assert_relative_eq!(sv.angular(), Vector3::new(1.0, 2.0, 3.0), epsilon = 1e-10);
        assert_relative_eq!(sv.linear(), Vector3::new(40.0, 50.0, 60.0), epsilon = 1e-10);
    }

    #[test]
    fn test_spatial_vector_sub_correctness() {
        let a = SpatialVector::new(Vector3::new(3.0, 5.0, 7.0), Vector3::new(2.0, 4.0, 6.0));
        let b = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(1.0, 1.0, 1.0));
        let diff = a.sub(&b);
        assert_relative_eq!(diff.angular(), Vector3::new(2.0, 3.0, 4.0), epsilon = 1e-10);
        assert_relative_eq!(diff.linear(), Vector3::new(1.0, 3.0, 5.0), epsilon = 1e-10);
    }

    #[test]
    fn test_spatial_inertia_sub_correctness() {
        let a = SpatialInertia::sphere(5.0, 0.1);
        let b = SpatialInertia::sphere(2.0, 0.1);
        let diff = a.sub(&b);
        assert_relative_eq!(diff.mass, 3.0, epsilon = 1e-10);
    }

    #[test]
    fn test_spatial_inertia_zero_has_zero_mass_and_data() {
        let z = SpatialInertia::zero();
        assert_relative_eq!(z.mass, 0.0, epsilon = 1e-10);
        for i in 0..6 {
            for j in 0..6 {
                assert!(z.data[(i, j)].abs() < 1e-10,
                    "zero inertia should be all zeros at ({i},{j}), got {}", z.data[(i, j)]);
            }
        }
    }

    #[test]
    fn test_rot_y_90_degrees() {
        use std::f32::consts::FRAC_PI_2;
        let ry = SpatialTransform::rot_y(FRAC_PI_2);
        // Rotating X-axis unit vector by 90° around Y should give -Z
        let v = SpatialVector::new(Vector3::zeros(), Vector3::new(1.0, 0.0, 0.0));
        let result = ry.apply_motion(&v);
        assert_relative_eq!(result.linear().x, 0.0, epsilon = 1e-5);
        assert_relative_eq!(result.linear().y, 0.0, epsilon = 1e-5);
        assert_relative_eq!(result.linear().z, -1.0, epsilon = 1e-5);
    }

    #[test]
    fn test_rot_y_inverse_roundtrip() {
        let angle = 1.23_f32;
        let ry = SpatialTransform::rot_y(angle);
        let ry_inv = ry.inverse();
        let composed = ry.compose(&ry_inv);
        let v = SpatialVector::new(Vector3::new(1.0, 2.0, 3.0), Vector3::new(4.0, 5.0, 6.0));
        let result = composed.apply_motion(&v);
        for i in 0..6 {
            assert!((result.data[i] - v.data[i]).abs() < 1e-4,
                "rot_y inverse roundtrip failed at component {i}: got {} expected {}", result.data[i], v.data[i]);
        }
    }

    // ── SLAM Cycle 9: Determinism and boundary tests ─────────────────

    #[test]
    fn determinism_spatial_transform_compose() {
        let a = SpatialTransform::from_axis_angle_translation(
            Vector3::z(), 0.7, Vector3::new(1.0, 2.0, 3.0));
        let b = SpatialTransform::from_axis_angle_translation(
            Vector3::x(), -0.3, Vector3::new(-1.0, 0.5, 0.0));
        let c1 = a.compose(&b);
        let c2 = a.compose(&b);
        for i in 0..3 {
            for j in 0..3 {
                assert_eq!(c1.rotation[(i, j)], c2.rotation[(i, j)]);
            }
        }
        assert_eq!(c1.translation, c2.translation);
    }

    #[test]
    fn edge_spatial_inertia_large_mass_valid() {
        let si = SpatialInertia::sphere(1e6, 1.0);
        assert!((si.mass - 1e6).abs() < 1.0);
        assert!(si.is_valid());
    }

    #[test]
    fn edge_spatial_inertia_small_mass_valid() {
        let si = SpatialInertia::sphere(1e-4, 0.01);
        assert!((si.mass - 1e-4).abs() < 1e-6);
        assert!(si.is_valid());
    }

    #[test]
    fn edge_large_translation_finite_result() {
        let t = SpatialTransform::from_translation(Vector3::new(1e5, 1e5, 1e5));
        let v = SpatialVector::new(Vector3::new(1.0, 0.0, 0.0), Vector3::zeros());
        let result = t.apply_motion(&v);
        assert!(result.data.iter().all(|x| x.is_finite()));
    }

    proptest! {
        #[test]
        fn prop_kinetic_energy_non_negative(
            wx in -5.0f32..5.0, wy in -5.0f32..5.0, wz in -5.0f32..5.0,
            vx in -5.0f32..5.0, vy in -5.0f32..5.0, vz in -5.0f32..5.0,
        ) {
            let si = SpatialInertia::sphere(1.0, 0.1);
            let v = SpatialVector::new(
                Vector3::new(wx, wy, wz),
                Vector3::new(vx, vy, vz),
            );
            let ke = 0.5 * v.dot(&si.mul_vector(&v));
            prop_assert!(ke >= -1e-6, "KE={ke} must be non-negative");
        }
    }

    #[test]
    fn intent_spatial_inertia_sphere_mass_correct() {
        // SpatialInertia::sphere(mass, radius) should have .mass == mass
        let mass = 7.3_f32;
        let radius = 0.42_f32;
        let si = SpatialInertia::sphere(mass, radius);
        assert_relative_eq!(si.mass, mass, epsilon = 1e-10);
    }

    #[test]
    fn intent_spatial_transform_compose_associative() {
        // (A . B) . C should equal A . (B . C) when applied to a spatial vector
        let a = SpatialTransform::from_axis_angle_translation(
            Vector3::new(1.0, 0.0, 0.0), 0.6,
            Vector3::new(0.2, -0.5, 0.3),
        );
        let b = SpatialTransform::from_axis_angle_translation(
            Vector3::new(0.0, 1.0, 0.0), -0.4,
            Vector3::new(-0.1, 0.7, 0.0),
        );
        let c = SpatialTransform::from_axis_angle_translation(
            Vector3::new(0.0, 0.0, 1.0), 1.1,
            Vector3::new(0.0, 0.0, -0.8),
        );

        let ab_c = a.compose(&b).compose(&c);
        let a_bc = a.compose(&b.compose(&c));

        // Test that both orderings produce the same result on a test vector
        let v = SpatialVector::new(
            Vector3::new(1.5, -0.7, 2.3),
            Vector3::new(-0.4, 3.1, 0.6),
        );
        let r1 = ab_c.apply_motion(&v);
        let r2 = a_bc.apply_motion(&v);

        for i in 0..6 {
            assert_relative_eq!(r1.data[i], r2.data[i], epsilon = 1e-4,
            );
        }
    }
}