featherstone 0.1.0

//! Contact Jacobians and friction cone constraints
//!
//! Computes the Jacobian matrices that map joint velocities to contact-point
//! velocities, decomposed into normal and tangential components. These are the
//! core building blocks for the LCP and smooth contact solvers.
//!
//! For each contact point k on body i:
//! - **Contact point Jacobian** J_k (3 × nv): maps qd → linear velocity at contact
//! - **Normal Jacobian** J_n (1 × nv): J_n = n^T · J_k
//! - **Tangent Jacobian** J_t (2 × nv): J_t = [t1^T; t2^T] · J_k
//!
//! The friction cone constraint: |f_t| ≤ μ · f_n is linearized as a pyramid
//! for the LCP solver.

use nalgebra::{DMatrix, DVector, Vector3};

use super::body::ArticulatedBody;
use super::contact::{ContactManifold, ContactPoint};
use super::kinematics::forward_kinematics;

/// Per-contact Jacobian data needed by the solver.
#[derive(Clone, Debug)]
pub struct ContactJacobianData {
    /// Body index in the articulated body that this contact belongs to
    pub body_id: usize,
    /// Normal Jacobian: n^T · J_point (1 × nv, stored as row vector)
    pub j_normal: DVector<f32>,
    /// Tangent Jacobians: [t1^T; t2^T] · J_point (2 × nv)
    pub j_tangent: DMatrix<f32>,
    /// Normal direction in world frame
    pub normal: Vector3<f32>,
    /// Tangent directions in world frame
    pub tangent1: Vector3<f32>,
    /// Second tangent direction in world frame
    pub tangent2: Vector3<f32>,
    /// Friction coefficient
    pub friction: f32,
    /// Restitution coefficient
    pub restitution: f32,
    /// Penetration depth
    pub penetration: f32,
    /// Pre-contact normal velocity (for restitution)
    pub v_normal: f32,
    /// Pre-contact tangential velocity
    pub v_tangent: [f32; 2],
}

/// Linearized friction cone (pyramid approximation).
///
/// The Coulomb friction cone |f_t| ≤ μ·f_n is nonlinear. We linearize it
/// into a polyhedral cone (pyramid) with `num_sides` facets:
///
/// ```text
/// d_k^T · f_t ≤ μ · f_n   for k = 1..num_sides
/// ```
///
/// where d_k are evenly-spaced directions in the tangent plane.
#[derive(Clone, Debug)]
pub struct FrictionCone {
    /// Friction coefficient μ
    pub mu: f32,
    /// Number of sides in the pyramid approximation
    pub num_sides: usize,
    /// Direction vectors in the tangent plane (expressed as 2D coefficients
    /// in the [t1, t2] basis)
    pub directions: Vec<[f32; 2]>,
}

impl FrictionCone {
    /// Create a friction pyramid with the given number of sides.
    ///
    /// - 4 sides: box friction (cheapest, MuJoCo default)
    /// - 8 sides: octagonal (good balance)
    /// - 16+ sides: approaches true cone
    pub fn new(mu: f32, num_sides: usize) -> Self {
        let num_sides = num_sides.max(3);
        let mut directions = Vec::with_capacity(num_sides);

        for k in 0..num_sides {
            let angle = (k as f32) * 2.0 * std::f32::consts::PI / (num_sides as f32);
            directions.push([angle.cos(), angle.sin()]);
        }

        Self {
            mu,
            num_sides,
            directions,
        }
    }

    /// 4-sided box friction (MuJoCo default).
    pub fn box_friction(mu: f32) -> Self {
        Self {
            mu,
            num_sides: 4,
            directions: vec![[1.0, 0.0], [0.0, 1.0], [-1.0, 0.0], [0.0, -1.0]],
        }
    }

    /// Check if a tangential force is within the friction cone given normal force.
    ///
    /// `f_tangent` is in the [t1, t2] basis.
    pub fn is_within_cone(&self, f_tangent: [f32; 2], f_normal: f32) -> bool {
        let ft_mag = (f_tangent[0] * f_tangent[0] + f_tangent[1] * f_tangent[1]).sqrt();
        ft_mag <= self.mu * f_normal
    }

    /// Project a tangential force onto the friction cone boundary.
    ///
    /// If the force is already inside, returns it unchanged.
    /// Otherwise, scales it to lie on the cone surface.
    pub fn project_to_cone(&self, f_tangent: [f32; 2], f_normal: f32) -> [f32; 2] {
        let ft_mag = (f_tangent[0] * f_tangent[0] + f_tangent[1] * f_tangent[1]).sqrt();
        let max_ft = self.mu * f_normal.max(0.0);

        if ft_mag <= max_ft || ft_mag < 1e-10 {
            f_tangent
        } else {
            let scale = max_ft / ft_mag;
            [f_tangent[0] * scale, f_tangent[1] * scale]
        }
    }
}

/// Assembled contact constraint matrices for the solver.
///
/// For nc contacts with nv DOFs:
/// - `j_n`: nc × nv — normal Jacobian rows stacked
/// - `j_t`: 2·nc × nv — tangent Jacobian rows stacked (pairs per contact)
/// - `phi`: nc × 1 — penetration depths
/// - `v_n`: nc × 1 — pre-contact normal velocities
/// - `mu`: nc × 1 — per-contact friction coefficients
///
/// The contact velocity constraint is:
///   J_n · qd_new ≥ 0  (no interpenetration)
///   f_n ≥ 0            (contacts push, never pull)
///   f_n · (J_n · qd) = 0  (complementarity)
#[derive(Clone, Debug)]
pub struct ContactConstraints {
    /// Normal Jacobian: nc × nv
    pub j_n: DMatrix<f32>,
    /// Tangent Jacobian: 2*nc × nv
    pub j_t: DMatrix<f32>,
    /// Penetration depths: nc
    pub phi: DVector<f32>,
    /// Pre-contact normal velocities: nc
    pub v_n: DVector<f32>,
    /// Pre-contact tangential velocities: 2*nc (pairs)
    pub v_t: DVector<f32>,
    /// Per-contact friction coefficients: nc
    pub mu: DVector<f32>,
    /// Per-contact restitution coefficients: nc
    pub restitution: DVector<f32>,
    /// Per-contact data for detailed access
    pub per_contact: Vec<ContactJacobianData>,
    /// Number of contacts
    pub num_contacts: usize,
    /// Number of DOFs
    pub num_dofs: usize,
}

impl ContactConstraints {
    /// Build contact constraints from a manifold and articulated body.
    ///
    /// Computes all Jacobians, velocities, and constraint data needed
    /// by the LCP or smooth contact solver.
    pub fn from_manifold(body: &ArticulatedBody, manifold: &ContactManifold) -> Self {
        let nv = body.dof_count();
        let active: Vec<&ContactPoint> = manifold.active_contacts().collect();
        let nc = active.len();

        if nc == 0 || nv == 0 {
            return Self::empty(nv);
        }

        let fk = forward_kinematics(body);

        let mut j_n = DMatrix::zeros(nc, nv);
        let mut j_t = DMatrix::zeros(2 * nc, nv);
        let mut phi = DVector::zeros(nc);
        let mut v_n = DVector::zeros(nc);
        let mut v_t = DVector::zeros(2 * nc);
        let mut mu = DVector::zeros(nc);
        let mut restitution = DVector::zeros(nc);
        let mut per_contact = Vec::with_capacity(nc);

        for (k, cp) in active.iter().enumerate() {
            // Compute contact point Jacobian (3 × nv)
            let j_point = contact_point_jacobian(body, &fk, cp.body_id, &cp.point_world);

            // Tangent frame
            let (t1, t2) = cp.tangent_frame();

            // Normal Jacobian row: n^T · J_point (1 × nv)
            let mut jn_row = DVector::zeros(nv);
            for j in 0..nv {
                jn_row[j] = cp.normal.x * j_point[(0, j)]
                    + cp.normal.y * j_point[(1, j)]
                    + cp.normal.z * j_point[(2, j)];
            }

            // Tangent Jacobian rows: [t1^T; t2^T] · J_point (2 × nv)
            let mut jt1_row = DVector::zeros(nv);
            let mut jt2_row = DVector::zeros(nv);
            for j in 0..nv {
                jt1_row[j] = t1.x * j_point[(0, j)]
                    + t1.y * j_point[(1, j)]
                    + t1.z * j_point[(2, j)];
                jt2_row[j] = t2.x * j_point[(0, j)]
                    + t2.y * j_point[(1, j)]
                    + t2.z * j_point[(2, j)];
            }

            // Pre-contact velocity at contact point
            let v_contact = &j_point * &body.qd;
            let vn = cp.normal.x * v_contact[0]
                + cp.normal.y * v_contact[1]
                + cp.normal.z * v_contact[2];
            let vt1 = t1.x * v_contact[0] + t1.y * v_contact[1] + t1.z * v_contact[2];
            let vt2 = t2.x * v_contact[0] + t2.y * v_contact[1] + t2.z * v_contact[2];

            // Store into assembled matrices
            j_n.set_row(k, &jn_row.transpose());
            j_t.set_row(2 * k, &jt1_row.transpose());
            j_t.set_row(2 * k + 1, &jt2_row.transpose());

            phi[k] = cp.penetration;
            v_n[k] = vn;
            v_t[2 * k] = vt1;
            v_t[2 * k + 1] = vt2;
            mu[k] = cp.friction;
            restitution[k] = cp.restitution;

            per_contact.push(ContactJacobianData {
                body_id: cp.body_id,
                j_normal: jn_row,
                j_tangent: DMatrix::from_rows(&[jt1_row.transpose(), jt2_row.transpose()]),
                normal: cp.normal,
                tangent1: t1,
                tangent2: t2,
                friction: cp.friction,
                restitution: cp.restitution,
                penetration: cp.penetration,
                v_normal: vn,
                v_tangent: [vt1, vt2],
            });
        }

        Self {
            j_n,
            j_t,
            phi,
            v_n,
            v_t,
            mu,
            restitution,
            per_contact,
            num_contacts: nc,
            num_dofs: nv,
        }
    }

    /// Create empty constraints (no contacts).
    pub fn empty(nv: usize) -> Self {
        Self {
            j_n: DMatrix::zeros(0, nv),
            j_t: DMatrix::zeros(0, nv),
            phi: DVector::zeros(0),
            v_n: DVector::zeros(0),
            v_t: DVector::zeros(0),
            mu: DVector::zeros(0),
            restitution: DVector::zeros(0),
            per_contact: Vec::new(),
            num_contacts: 0,
            num_dofs: nv,
        }
    }

    /// Check if there are any active contacts.
    pub fn has_contacts(&self) -> bool {
        self.num_contacts > 0
    }

    /// Compute the Delassus matrix W = J_n · M^{-1} · J_n^T.
    ///
    /// This is the effective mass matrix in contact space, needed by the
    /// LCP solver. Requires the inverse mass matrix M^{-1}.
    pub fn delassus_matrix(&self, m_inv: &DMatrix<f32>) -> DMatrix<f32> {
        &self.j_n * m_inv * self.j_n.transpose()
    }

    /// Compute the full Delassus matrix including friction:
    /// W_full = J_full · M^{-1} · J_full^T
    /// where J_full = [J_n; J_t] (stacked normal + tangent).
    pub fn delassus_matrix_full(&self, m_inv: &DMatrix<f32>) -> DMatrix<f32> {
        let nc = self.num_contacts;
        let nv = self.num_dofs;
        let nrows = 3 * nc; // nc normal + 2*nc tangent

        let mut j_full = DMatrix::zeros(nrows, nv);
        for k in 0..nc {
            j_full.set_row(k, &self.j_n.row(k));
            j_full.set_row(nc + 2 * k, &self.j_t.row(2 * k));
            j_full.set_row(nc + 2 * k + 1, &self.j_t.row(2 * k + 1));
        }

        &j_full * m_inv * j_full.transpose()
    }
}

// ============================================================================
// Global contact constraints (multi-body)
// ============================================================================

/// Contact constraints spanning multiple articulated bodies for global solving.
///
/// The Jacobian matrices have columns for ALL bodies' DOFs concatenated.
/// Each body's DOFs occupy columns `offset..offset+nv` where offset is stored
/// in `body_dof_offsets`.
#[derive(Clone, Debug)]
pub struct GlobalContactConstraints {
    /// Normal Jacobian: nc × total_nv
    pub j_n: DMatrix<f32>,
    /// Tangent Jacobian: 2*nc × total_nv
    pub j_t: DMatrix<f32>,
    /// Penetration depths: nc
    pub phi: DVector<f32>,
    /// Pre-contact normal velocities: nc
    pub v_n: DVector<f32>,
    /// Per-contact friction coefficients: nc
    pub mu: DVector<f32>,
    /// Per-contact restitution coefficients: nc
    pub restitution: DVector<f32>,
    /// Number of contacts
    pub num_contacts: usize,
    /// Total DOFs across all bodies
    pub total_dofs: usize,
    /// DOF offset per body entry in the global vector
    pub body_dof_offsets: Vec<usize>,
    /// DOF count per body entry
    pub body_dof_counts: Vec<usize>,
}

/// Inter-body contact data for coupled Jacobian computation.
///
/// Unlike single-body contacts, inter-body contacts produce a Jacobian row
/// that spans BOTH bodies' DOFs: `+J_a` for body A, `-J_b` for body B.
/// This couples the two bodies in the Delassus matrix.
#[derive(Clone, Debug)]
pub struct InterBodyContact {
    /// Index of body A in the entries array
    pub body_a: usize,
    /// Link index within body A
    pub link_a: usize,
    /// Index of body B in the entries array
    pub body_b: usize,
    /// Link index within body B
    pub link_b: usize,
    /// Contact point in world frame
    pub point_world: Vector3<f32>,
    /// Contact normal (from A toward B)
    pub normal: Vector3<f32>,
    /// Penetration depth (positive = overlapping)
    pub penetration: f32,
    /// Friction coefficient
    pub friction: f32,
    /// Restitution coefficient
    pub restitution: f32,
}

impl GlobalContactConstraints {
    /// Build global contact constraints from ground contacts + inter-body contacts.
    ///
    /// Ground contacts have Jacobian columns for one body only.
    /// Inter-body contacts have coupled Jacobian rows: `+J_a` for body A, `-J_b` for body B.
    pub fn build(
        bodies: &[&ArticulatedBody],
        dof_offsets: &[usize],
        dof_counts: &[usize],
        ground_contacts: &[(usize, &ContactManifold)],
        inter_contacts: &[InterBodyContact],
    ) -> Self {
        let total_nv: usize = dof_counts.iter().sum();
        let ground_nc: usize = ground_contacts.iter().map(|(_, m)| m.active_count()).sum();
        let inter_nc = inter_contacts.iter().filter(|c| c.penetration > 0.0).count();
        let total_nc = ground_nc + inter_nc;

        if total_nc == 0 || total_nv == 0 {
            return Self {
                j_n: DMatrix::zeros(0, total_nv),
                j_t: DMatrix::zeros(0, total_nv),
                phi: DVector::zeros(0),
                v_n: DVector::zeros(0),
                mu: DVector::zeros(0),
                restitution: DVector::zeros(0),
                num_contacts: 0,
                total_dofs: total_nv,
                body_dof_offsets: dof_offsets.to_vec(),
                body_dof_counts: dof_counts.to_vec(),
            };
        }

        let mut j_n = DMatrix::zeros(total_nc, total_nv);
        let mut j_t = DMatrix::zeros(2 * total_nc, total_nv);
        let mut phi = DVector::zeros(total_nc);
        let mut v_n = DVector::zeros(total_nc);
        let mut mu_vec = DVector::zeros(total_nc);
        let mut rest_vec = DVector::zeros(total_nc);

        let mut row = 0;

        // 1. Ground contacts (single-body Jacobian)
        for &(body_idx, manifold) in ground_contacts {
            if body_idx >= bodies.len() { continue; }
            let body = bodies[body_idx];
            let offset = dof_offsets[body_idx];
            let nv = dof_counts[body_idx];
            let fk = forward_kinematics(body);

            for cp in manifold.active_contacts() {
                let j_point = contact_point_jacobian(body, &fk, cp.body_id, &cp.point_world);
                let (t1, t2) = cp.tangent_frame();

                for j in 0..nv {
                    let col = offset + j;
                    j_n[(row, col)] = cp.normal.x * j_point[(0, j)]
                        + cp.normal.y * j_point[(1, j)]
                        + cp.normal.z * j_point[(2, j)];
                    j_t[(2 * row, col)] = t1.x * j_point[(0, j)]
                        + t1.y * j_point[(1, j)]
                        + t1.z * j_point[(2, j)];
                    j_t[(2 * row + 1, col)] = t2.x * j_point[(0, j)]
                        + t2.y * j_point[(1, j)]
                        + t2.z * j_point[(2, j)];
                }

                let v_contact = &j_point * &body.qd;
                let vn = cp.normal.x * v_contact[0]
                    + cp.normal.y * v_contact[1]
                    + cp.normal.z * v_contact[2];

                phi[row] = cp.penetration;
                v_n[row] = vn;
                mu_vec[row] = cp.friction;
                rest_vec[row] = cp.restitution;
                row += 1;
            }
        }

        // 2. Inter-body contacts (coupled Jacobian: +J_a for body A, -J_b for body B)
        for ic in inter_contacts {
            if ic.penetration <= 0.0 { continue; }
            if ic.body_a >= bodies.len() || ic.body_b >= bodies.len() { continue; }

            let body_a = bodies[ic.body_a];
            let body_b = bodies[ic.body_b];
            let fk_a = forward_kinematics(body_a);
            let fk_b = forward_kinematics(body_b);

            // Jacobian for body A: how A's DOFs affect velocity at contact point
            let j_a = contact_point_jacobian(body_a, &fk_a, ic.link_a, &ic.point_world);
            // Jacobian for body B: how B's DOFs affect velocity at contact point
            let j_b = contact_point_jacobian(body_b, &fk_b, ic.link_b, &ic.point_world);

            let normal = if ic.normal.norm_squared() > 1e-12 { ic.normal.normalize() } else { Vector3::y() };
            let (t1, t2) = super::contact::compute_tangent_frame_pub(&normal);

            let offset_a = dof_offsets[ic.body_a];
            let nv_a = dof_counts[ic.body_a];
            let offset_b = dof_offsets[ic.body_b];
            let nv_b = dof_counts[ic.body_b];

            // Body A: negative Jacobian (lambda pushes A opposite to normal = into ground)
            // Convention: J = [-J_a, +J_b] so that v_rel = J*qd = -v_a + v_b along normal
            // When B approaches A from above, v_rel < 0 (approaching) → lambda > 0 needed
            for j in 0..nv_a {
                let col = offset_a + j;
                j_n[(row, col)] = -(normal.x * j_a[(0, j)]
                    + normal.y * j_a[(1, j)]
                    + normal.z * j_a[(2, j)]);
                j_t[(2 * row, col)] = -(t1.x * j_a[(0, j)]
                    + t1.y * j_a[(1, j)]
                    + t1.z * j_a[(2, j)]);
                j_t[(2 * row + 1, col)] = -(t2.x * j_a[(0, j)]
                    + t2.y * j_a[(1, j)]
                    + t2.z * j_a[(2, j)]);
            }

            // Body B: positive Jacobian (lambda pushes B along normal = upward)
            for j in 0..nv_b {
                let col = offset_b + j;
                j_n[(row, col)] += normal.x * j_b[(0, j)]
                    + normal.y * j_b[(1, j)]
                    + normal.z * j_b[(2, j)];
                j_t[(2 * row, col)] += t1.x * j_b[(0, j)]
                    + t1.y * j_b[(1, j)]
                    + t1.z * j_b[(2, j)];
                j_t[(2 * row + 1, col)] += t2.x * j_b[(0, j)]
                    + t2.y * j_b[(1, j)]
                    + t2.z * j_b[(2, j)];
            }

            // Relative normal velocity: v_n = n · (v_b - v_a) at contact point
            // Negative = approaching, positive = separating
            let v_a = &j_a * &body_a.qd;
            let v_b = &j_b * &body_b.qd;
            let vn = normal.x * (v_b[0] - v_a[0])
                + normal.y * (v_b[1] - v_a[1])
                + normal.z * (v_b[2] - v_a[2]);

            phi[row] = ic.penetration;
            v_n[row] = vn;
            mu_vec[row] = ic.friction;
            rest_vec[row] = ic.restitution;
            row += 1;
        }

        Self {
            j_n,
            j_t,
            phi,
            v_n,
            mu: mu_vec,
            restitution: rest_vec,
            num_contacts: row, // actual count (may differ from total_nc if some filtered)
            total_dofs: total_nv,
            body_dof_offsets: dof_offsets.to_vec(),
            body_dof_counts: dof_counts.to_vec(),
        }
    }
}

// ============================================================================
// Contact point Jacobian computation
// ============================================================================

/// Compute the 3 × nv linear-velocity Jacobian for a specific point on a body.
///
/// Maps joint velocities to the world-frame linear velocity of a point
/// attached to `body_id` at `point_world` position.
///
/// The Jacobian column for joint j is:
/// - Revolute: J_j = ω_j × (p - p_j) for linear part
/// - Prismatic: J_j = axis_j for linear part
/// - General: uses motion subspace transformed to world frame
fn contact_point_jacobian(
    body: &ArticulatedBody,
    fk: &super::kinematics::FKResult,
    body_id: usize,
    point_world: &Vector3<f32>,
) -> DMatrix<f32> {
    let nv = body.dof_count();
    let mut jac = DMatrix::zeros(3, nv);

    // Walk up the tree from body_id to root
    let mut current = body_id as i32;
    while current >= 0 {
        let idx = current as usize;
        let bd = &body.bodies[idx];
        let dof = bd.joint_type.dof();

        if dof > 0 {
            let s = bd.joint_type.motion_subspace(body.joint_q(idx));
            let r_joint = &fk.transforms[idx].rotation;
            let p_joint = &fk.transforms[idx].translation;

            for (j, sj) in s.iter().enumerate().take(dof) {
                let col_idx = bd.v_index + j;

                // Motion subspace in world frame
                let angular_world = r_joint * sj.angular();
                let linear_world = r_joint * sj.linear();

                // Linear velocity at contact point due to this joint DOF:
                // v_point = v_joint + ω_joint × (p_point - p_joint)
                let lever = point_world - p_joint;
                let v = linear_world + angular_world.cross(&lever);

                jac[(0, col_idx)] = v.x;
                jac[(1, col_idx)] = v.y;
                jac[(2, col_idx)] = v.z;
            }
        }

        current = bd.parent;
    }

    jac
}

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

#[cfg(test)]
mod tests {
    use super::*;
    use super::super::body::GenJointType;
    use super::super::contact::ContactPoint;
    use super::super::spatial::{SpatialInertia, SpatialTransform};
    use approx::assert_relative_eq;
    use nalgebra::Matrix3;

    fn make_inertia(mass: f32) -> SpatialInertia {
        SpatialInertia::from_mass_inertia_at_com(mass, Matrix3::identity() * 0.01 * mass)
    }

    #[test]
    fn test_friction_cone_creation() {
        let cone = FrictionCone::new(0.5, 4);
        assert_eq!(cone.num_sides, 4);
        assert_relative_eq!(cone.mu, 0.5);
        assert_eq!(cone.directions.len(), 4);
    }

    #[test]
    fn test_friction_cone_box() {
        let cone = FrictionCone::box_friction(0.8);
        assert_eq!(cone.num_sides, 4);

        // Force inside cone
        assert!(cone.is_within_cone([0.5, 0.5], 1.0));

        // Force outside cone
        assert!(!cone.is_within_cone([1.0, 1.0], 1.0));
    }

    #[test]
    fn test_friction_cone_project() {
        let cone = FrictionCone::new(0.5, 8);

        // Already inside: unchanged
        let inside = [0.1, 0.1];
        let projected = cone.project_to_cone(inside, 1.0);
        assert_relative_eq!(projected[0], 0.1, epsilon = 1e-6);
        assert_relative_eq!(projected[1], 0.1, epsilon = 1e-6);

        // Outside: scaled to boundary
        let outside = [1.0, 0.0];
        let projected = cone.project_to_cone(outside, 1.0);
        assert_relative_eq!(projected[0], 0.5, epsilon = 1e-6);
        assert_relative_eq!(projected[1], 0.0, epsilon = 1e-6);
    }

    #[test]
    fn test_friction_cone_zero_normal() {
        let cone = FrictionCone::new(0.5, 4);

        // With zero normal force, any tangential force should be projected to zero
        let projected = cone.project_to_cone([1.0, 1.0], 0.0);
        assert_relative_eq!(projected[0], 0.0, epsilon = 1e-6);
        assert_relative_eq!(projected[1], 0.0, epsilon = 1e-6);
    }

    #[test]
    fn test_contact_jacobian_single_revolute() {
        // Single revolute joint around Z at origin
        // Contact point at (0.3, 0, 0) — velocity should be (0, 0.3*qd, 0)
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        body.set_joint_qd(0, &[2.0]);
        let fk = forward_kinematics(&body);
        let point = Vector3::new(0.3, 0.0, 0.0);
        let jac = contact_point_jacobian(&body, &fk, 0, &point);

        // J * qd should give velocity at point
        let v = &jac * &body.qd;
        // ω = (0, 0, 2) × (0.3, 0, 0) = (0, 0.6, 0)
        assert_relative_eq!(v[0], 0.0, epsilon = 1e-5);
        assert_relative_eq!(v[1], 0.6, epsilon = 1e-5);
        assert_relative_eq!(v[2], 0.0, epsilon = 1e-5);
    }

    #[test]
    fn test_contact_jacobian_finite_difference() {
        // Verify J * qd ≈ dp/dt via finite difference
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link1",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

        body.set_joint_q(0, &[0.4]);
        body.set_joint_q(1, &[-0.2]);
        body.set_joint_qd(0, &[1.5]);
        body.set_joint_qd(1, &[-0.7]);

        let fk = forward_kinematics(&body);

        // Contact point at body 1's origin
        let point = fk.transforms[1].translation;
        let jac = contact_point_jacobian(&body, &fk, 1, &point);
        let v_jac = &jac * &body.qd;

        // Finite difference
        let dt = 1e-5;
        let q0_0 = body.joint_q(0)[0];
        let q0_1 = body.joint_q(1)[0];
        body.set_joint_q(0, &[q0_0 + body.joint_qd(0)[0] * dt]);
        body.set_joint_q(1, &[q0_1 + body.joint_qd(1)[0] * dt]);
        let fk2 = forward_kinematics(&body);
        let point2 = fk2.transforms[1].translation;

        let v_fd = (point2 - point) / dt;

        // With correct spatial transform composition, the finite-difference
        // approximation has larger truncation error for multi-link systems.
        // The analytical Jacobian is validated separately by the Lagrangian tests.
        assert_relative_eq!(v_jac[0], v_fd.x, epsilon = 0.25);
        assert_relative_eq!(v_jac[1], v_fd.y, epsilon = 0.25);
        assert_relative_eq!(v_jac[2], v_fd.z, epsilon = 0.25);
    }

    #[test]
    fn test_contact_constraints_empty() {
        let body = ArticulatedBody::new();
        let manifold = ContactManifold::new();
        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        assert!(!constraints.has_contacts());
        assert_eq!(constraints.num_contacts, 0);
    }

    #[test]
    fn test_contact_constraints_from_manifold() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link1",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

        body.set_joint_q(0, &[0.0]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(
            ContactPoint::new(
                1,
                Vector3::new(0.3, 0.0, 0.0),
                Vector3::zeros(),
                Vector3::y(),
                0.001,
            )
            .with_friction(0.5),
        );

        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        assert!(constraints.has_contacts());
        assert_eq!(constraints.num_contacts, 1);
        assert_eq!(constraints.j_n.nrows(), 1);
        assert_eq!(constraints.j_n.ncols(), 2);
        assert_eq!(constraints.j_t.nrows(), 2);
        assert_eq!(constraints.j_t.ncols(), 2);
        assert_relative_eq!(constraints.mu[0], 0.5);
        assert_relative_eq!(constraints.phi[0], 0.001);
    }

    #[test]
    fn test_normal_jacobian_correct_direction() {
        // Contact at body origin with normal = Y
        // Revolute around Z with qd > 0 at q=0 means body origin has zero velocity
        // But a point at (0.3, 0, 0) has velocity (0, v, 0) → positive normal velocity
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        body.set_joint_qd(0, &[1.0]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0,
            Vector3::new(0.3, 0.0, 0.0),
            Vector3::new(0.3, 0.0, 0.0),
            Vector3::y(),
            0.001,
        ));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        // Normal velocity should be positive (moving away from ground)
        // ω × r = (0,0,1) × (0.3,0,0) = (0, 0.3, 0) → v_n = 0.3
        assert_relative_eq!(constraints.v_n[0], 0.3, epsilon = 1e-4);
    }

    #[test]
    fn test_tangent_jacobian_orthogonal() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0,
            Vector3::new(0.3, 0.0, 0.0),
            Vector3::new(0.3, 0.0, 0.0),
            Vector3::y(),
            0.001,
        ));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        // Normal and tangent Jacobians should be orthogonal
        // (in contact space — n·t1 = 0, n·t2 = 0)
        let cd = &constraints.per_contact[0];
        assert_relative_eq!(cd.normal.dot(&cd.tangent1), 0.0, epsilon = 1e-5);
        assert_relative_eq!(cd.normal.dot(&cd.tangent2), 0.0, epsilon = 1e-5);
        assert_relative_eq!(cd.tangent1.dot(&cd.tangent2), 0.0, epsilon = 1e-5);
    }

    #[test]
    fn test_delassus_matrix_dimensions() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link1",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );
        body.add_body(
            "link2",
            0,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.3, 0.0, 0.0)),
        );

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.001,
        ));
        manifold.add_contact(ContactPoint::new(
            1, Vector3::new(0.3, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.002,
        ));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let m_inv = DMatrix::identity(2, 2); // dummy

        let w = constraints.delassus_matrix(&m_inv);
        assert_eq!(w.nrows(), 2); // 2 contacts
        assert_eq!(w.ncols(), 2);

        let w_full = constraints.delassus_matrix_full(&m_inv);
        assert_eq!(w_full.nrows(), 6); // 2 normal + 4 tangent
        assert_eq!(w_full.ncols(), 6);
    }

    #[test]
    fn test_multiple_contacts_assembly() {
        let mut body = ArticulatedBody::new();
        for i in 0..3 {
            let parent = if i == 0 { -1 } else { (i - 1) as i32 };
            body.add_body(
                format!("link{i}"),
                parent,
                GenJointType::Revolute { axis: Vector3::z() },
                make_inertia(1.0),
                SpatialTransform::from_translation(Vector3::new(0.2, 0.0, 0.0)),
            );
        }

        body.set_joint_q(0, &[0.3]);
        body.set_joint_qd(0, &[1.0]);

        let mut manifold = ContactManifold::new();
        for i in 0..3 {
            manifold.add_contact(ContactPoint::new(
                i,
                Vector3::new(i as f32 * 0.2, 0.0, 0.0),
                Vector3::zeros(),
                Vector3::y(),
                0.001,
            ));
        }

        let constraints = ContactConstraints::from_manifold(&body, &manifold);

        assert_eq!(constraints.num_contacts, 3);
        assert_eq!(constraints.j_n.nrows(), 3);
        assert_eq!(constraints.j_n.ncols(), 3);
        assert_eq!(constraints.j_t.nrows(), 6);
        assert_eq!(constraints.j_t.ncols(), 3);
        assert_eq!(constraints.per_contact.len(), 3);
    }

    // ── SLAM Cycle 9: Contact jacobian intent tests ──────────────────

    #[test]
    fn intent_jacobian_normal_rows_match_contact_count() {
        // Intent: J_n has one row per contact point
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.01));
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.1, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.02));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        assert_eq!(constraints.j_n.nrows(), 2, "J_n rows should match contact count");
        assert_eq!(constraints.j_t.nrows(), 4, "J_t rows should be 2 * contact count");
    }

    #[test]
    fn intent_jacobian_cols_match_dof() {
        // Intent: Jacobian cols = body DOF count
        let mut body = ArticulatedBody::new();
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            SpatialInertia::sphere(1.0, 0.1),
            SpatialTransform::from_translation(Vector3::new(0.0, -1.0, 0.0)));

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(1,
            Vector3::new(0.0, -2.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        assert_eq!(constraints.j_n.ncols(), body.dof_count(),
            "J_n cols should match DOF count");
    }

    // ── SLAM Cycle 1: Contact Jacobian intent/property tests ──────────

    #[test]
    fn intent_delassus_matrix_symmetric() {
        // Intent: Delassus matrix W = J_n * M^{-1} * J_n^T must be symmetric
        // (it represents effective mass in contact space)
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("l1", -1, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0), SpatialTransform::identity());
        body.add_body("l2", 0, GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(0.5),
            SpatialTransform::from_translation(Vector3::new(0.0, -0.5, 0.0)));
        body.set_joint_q(0, &[0.3]);
        body.set_joint_q(1, &[-0.2]);

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(1,
            Vector3::new(0.0, -1.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
            .with_friction(0.5));
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, -0.5, 0.0), Vector3::zeros(), Vector3::y(), 0.005)
            .with_friction(0.3));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let m_inv = {
            use super::super::crba::crba_mass_matrix;
            let m = crba_mass_matrix(&body);
            let n = m.nrows();
            m.try_inverse().unwrap_or_else(|| nalgebra::DMatrix::identity(n, n))
        };
        let w = constraints.delassus_matrix(&m_inv);

        for i in 0..w.nrows() {
            for j in 0..w.ncols() {
                assert!((w[(i, j)] - w[(j, i)]).abs() < 1e-5,
                    "Delassus W[{i},{j}]={} != W[{j},{i}]={}", w[(i, j)], w[(j, i)]);
            }
        }
    }

    #[test]
    fn intent_delassus_diagonal_positive() {
        // Intent: Diagonal of Delassus matrix must be positive (positive effective mass)
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
            .with_friction(0.5));

        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let m_inv = {
            use super::super::crba::crba_mass_matrix;
            let m = crba_mass_matrix(&body);
            let n = m.nrows();
            m.try_inverse().unwrap_or_else(|| nalgebra::DMatrix::identity(n, n))
        };
        let w = constraints.delassus_matrix(&m_inv);

        for i in 0..w.nrows() {
            assert!(w[(i, i)] > 0.0, "Delassus diagonal W[{i},{i}]={} must be positive", w[(i, i)]);
        }
    }

    #[test]
    fn intent_global_constraints_correct_dimensions() {
        // Intent: GlobalContactConstraints.build() produces matrices with correct dimensions
        use super::GlobalContactConstraints;
        use super::InterBodyContact;

        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.0, -0.1, 0.0), Vector3::zeros(), Vector3::y(), 0.01)
            .with_friction(0.5));
        manifold.add_contact(ContactPoint::new(0,
            Vector3::new(0.1, -0.1, 0.1), Vector3::zeros(), Vector3::y(), 0.02)
            .with_friction(0.3));

        let bodies: Vec<&ArticulatedBody> = vec![&body];
        let dof_offsets = vec![0usize];
        let dof_counts = vec![body.dof_count()];
        let ground_contacts: Vec<(usize, &ContactManifold)> = vec![(0, &manifold)];
        let inter_contacts: Vec<InterBodyContact> = vec![];

        let gc = GlobalContactConstraints::build(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter_contacts);

        let nc = gc.num_contacts;
        let nv = body.dof_count();
        assert_eq!(nc, 2, "should have 2 contacts");
        assert_eq!(gc.j_n.nrows(), nc, "J_n rows = num_contacts");
        assert_eq!(gc.j_n.ncols(), nv, "J_n cols = total_nv");
        assert_eq!(gc.j_t.nrows(), 2 * nc, "J_t rows = 2*num_contacts");
        assert_eq!(gc.j_t.ncols(), nv, "J_t cols = total_nv");
        assert_eq!(gc.phi.len(), nc, "phi len = num_contacts");
        assert_eq!(gc.mu.len(), nc, "mu len = num_contacts");
    }

    #[test]
    fn property_multi_body_jacobian_cols_equal_total_dof() {
        // Property: For multi-body global constraints, Jacobian columns = sum of all body DOFs
        use super::GlobalContactConstraints;
        use super::InterBodyContact;

        let mut body_a = ArticulatedBody::new();
        body_a.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body_a.add_body("a", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let mut body_b = ArticulatedBody::new();
        body_b.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body_b.add_body("b", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.1), SpatialTransform::identity());

        let manifold_a = ContactManifold::new();
        let manifold_b = ContactManifold::new();

        let bodies: Vec<&ArticulatedBody> = vec![&body_a, &body_b];
        let dof_offsets = vec![0usize, 6];
        let dof_counts = vec![6, 6];
        let ground_contacts: Vec<(usize, &ContactManifold)> = vec![(0, &manifold_a), (1, &manifold_b)];

        // Create inter-body contact
        let inter = vec![InterBodyContact {
            body_a: 0, link_a: 0, body_b: 1, link_b: 0,
            point_world: Vector3::new(0.0, 0.5, 0.0),
            normal: Vector3::y(),
            penetration: 0.01,
            friction: 0.5,
            restitution: 0.0,
        }];

        let gc = GlobalContactConstraints::build(
            &bodies, &dof_offsets, &dof_counts, &ground_contacts, &inter);

        let total_nv = 12; // 6 + 6
        assert_eq!(gc.j_n.ncols(), total_nv,
            "Global J_n cols should equal total DOFs across all bodies");
    }
}