featherstone 0.1.0

# Contact Dynamics

featherstone provides three contact solving approaches: LCP (rigid), smooth (differentiable), and Newton (stacking).

## Ground Contacts

The simplest case: a robot interacting with a flat ground plane.

```rust
use featherstone::prelude::*;
use featherstone::contact::ground_plane_contacts;
use featherstone::contact_jacobian::ContactConstraints;

// Detect contacts with ground at y=0
let manifold = ground_plane_contacts(
    &body,
    0.0,                    // ground height
    Vector3::y(),           // ground normal (up)
    0.5,                    // friction coefficient
    0.0,                    // restitution (0 = inelastic)
);

if !manifold.contacts.is_empty() {
    // Build constraint matrices
    let constraints = ContactConstraints::from_manifold(&body, &manifold);

    // Solve for contact impulses
    let config = LcpSolverConfig::default();
    let result = solve_contact_lcp(&body, &constraints, dt, &config);

    // Apply impulse as velocity correction: dqd = M^{-1} * tau_contact
    let m = featherstone::crba::crba_mass_matrix(&body);
    if let Some(m_inv) = m.try_inverse() {
        body.qd += m_inv * &result.tau_contact;
    }
}
```

## LCP Solver (Rigid Contacts)

Projected Gauss-Seidel with warm-starting. Best for rigid contacts where penetration must be zero.

```rust
let config = LcpSolverConfig {
    max_iterations: 100,
    tolerance: 1e-6,
    baumgarte: 0.1,           // penetration correction strength
    restitution_threshold: 0.01,
    warm_start: 0.9,          // reuse previous frame's impulses
    ..Default::default()
};
```

**Key properties:**
- Normal impulse >= 0 (contacts push, never pull)
- Friction cone: `||f_t|| <= mu * f_n`
- Complementarity: `lambda * constraint = 0`
- Convergence: monotonic residual decay

## Smooth Contacts (Differentiable)

Spring-damper penalty model. Best for gradient-based optimization and RL.

```rust
use featherstone::smooth_contact::{smooth_contact_forces, SmoothContactConfig};

let config = SmoothContactConfig::default();  // stiffness=1e5, damping=1e3
// Or: SmoothContactConfig::stiff() for rigid-like behavior
// Or: SmoothContactConfig::soft() for compliant contacts

let (tau_contact, energy) = smooth_contact_forces(&body, &constraints, &config);
```

**Trade-off vs LCP:**
- Smooth: differentiable, no iteration, slight penetration allowed
- LCP: exact non-penetration, not differentiable, iterative

## Simulation Loop with Contacts

The recommended contact simulation pattern:

```rust
let dt = 0.001;
let integrator = IntegratorConfig { dt, ..Default::default() };
let lcp_config = LcpSolverConfig::default();

loop {
    // Phase 1: Compute accelerations
    featherstone::integrator::compute_accelerations(&mut body, &integrator);

    // Phase 2: Update velocities
    featherstone::integrator::update_velocities(&mut body, &integrator);

    // Phase 3: Detect and resolve contacts (between velocity and position update)
    let manifold = ground_plane_contacts(&body, 0.0, Vector3::y(), 0.5, 0.0);
    if !manifold.contacts.is_empty() {
        let constraints = ContactConstraints::from_manifold(&body, &manifold);
        let result = solve_contact_lcp(&body, &constraints, dt, &lcp_config);
        let m = featherstone::crba::crba_mass_matrix(&body);
        if let Some(m_inv) = m.try_inverse() {
            body.qd += m_inv * &result.tau_contact;
        }
    }

    // Phase 4: Update positions
    featherstone::integrator::update_positions(&mut body, &integrator);
}
```

## Collision Shapes

Contact detection supports:

| Shape | Detection | Notes |
|-------|-----------|-------|
| Sphere-Sphere | Analytical | Fastest |
| Sphere-Box | Analytical | Clamped closest point |
| Box-Box | SAT + Sutherland-Hodgman | 15-axis SAT, polygon clipping |
| Capsule-Capsule | Segment distance | Analytical |
| Capsule-Sphere | Segment-point | Analytical |
| Capsule-Box | Multi-point sphere-box | 3 test points |
| ConvexHull-* | GJK/EPA | General convex shapes |