symtropy-physics 0.2.0

// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
// SPDX-License-Identifier: AGPL-3.0-or-later
// Commercial licensing: see COMMERCIAL_LICENSE.md at repository root
//! Multi-point contact manifold generation via contact clustering.
//!
//! After EPA finds the primary contact (normal + depth), this module generates
//! additional contact points by perturbing the query direction around the
//! contact normal and calling each shape's support function.
//!
//! # Why this matters
//! With a single contact point, a box resting on a flat plane has only one
//! impulse site. The box can pivot around that point — unstable stacking.
//! With 4 corner contacts, each corner carries part of the weight, and the
//! box is fully constrained. This is the core fix for stable stacking.
//!
//! # Algorithm
//! 1. Primary contact: `p0 = support_A(n) + pos_a`, `depth_0 = depth` (from EPA)
//! 2. Build an orthonormal tangent frame in the contact plane (D−1 vectors ⊥ n)
//! 3. For each cardinal+diagonal perturbation direction `normalize(n + ε·t)`:
//!    - Query `wA = support_A(dir) + pos_a`, `wB = support_B(-dir) + pos_b`
//!    - Measure penetration along the EPA normal: `sep = (wA - wB) · n`
//!    - Accept if `sep ≥ depth − DEPTH_TOLERANCE` and point is not a duplicate
//! 4. Reduce to at most 4 points via maximum-area selection (Catto 2011)
//!
//! # References
//! - Erin Catto, "Iterative Dynamics with Temporal Coherence", GDC 2006
//! - Erin Catto, "Box2D Lite", contact reduction (best-4 algorithm)

use arrayvec::ArrayVec;
use nalgebra::SVector;
use symtropy_math::Shape;

use crate::body::BodyHandle;
use crate::contact::{ContactManifold, ContactPoint, MAX_CONTACTS};

/// Angular perturbation magnitude: 0.5 rad ≈ 29° off-normal.
/// Large enough to find all 4 corners of a box face resting on a plane.
const PERTURB_EPS: f64 = 0.5;

/// A secondary contact is accepted if its penetration depth along the primary
/// normal is at least `primary_depth - DEPTH_TOLERANCE`.
const DEPTH_TOLERANCE: f64 = 0.02;

/// Deduplicate candidates closer than this (in world units).
const DEDUP_DIST: f64 = 0.001;

/// Generate a multi-point contact manifold for a GJK+EPA collision.
///
/// `normal` and `depth` come from the EPA result. Up to `MAX_CONTACTS` contact
/// points are generated by querying support in directions perturbed around `normal`.
pub fn generate_contact_manifold<const D: usize>(
    shape_a: &dyn Shape<D>,
    pos_a: &SVector<f64, D>,
    shape_b: &dyn Shape<D>,
    pos_b: &SVector<f64, D>,
    normal: SVector<f64, D>,
    depth: f64,
    body_a: BodyHandle,
    body_b: BodyHandle,
) -> ContactManifold<D> {
    // Primary contact: support of A in the contact normal direction
    let p0 = shape_a.support(&normal) + pos_a;
    let mut candidates: ArrayVec<(SVector<f64, D>, f64), 16> = ArrayVec::new();
    candidates.push((p0, depth));

    if depth > 1e-10 {
        let tangents = orthonormal_complement(&normal);
        let perturbs = build_perturbations(&normal, &tangents);

        for dir in &perturbs {
            let wA = shape_a.support(dir) + pos_a;
            let wB = shape_b.support(&(-*dir)) + pos_b;
            // Penetration along the primary contact normal
            let sep = (wA - wB).dot(&normal);

            if sep < depth - DEPTH_TOLERANCE {
                continue; // not deep enough to be a real contact
            }

            // Dedup: skip if too close to an existing candidate
            if candidates.iter().any(|(q, _)| (wA - q).norm() < DEDUP_DIST) {
                continue;
            }

            if candidates.len() < candidates.capacity() {
                candidates.push((wA, sep));
            }
        }
    }

    let max_pts = MAX_CONTACTS.min(4);
    let points = best_n(&candidates, max_pts);
    ContactManifold { body_a, body_b, normal, points }
}

// ---------------------------------------------------------------------------
// Tangent frame construction
// ---------------------------------------------------------------------------

/// Build D−1 orthonormal vectors perpendicular to `n` via Gram-Schmidt.
///
/// Returns up to 3 tangent vectors (sufficient for D ≤ 4).
fn orthonormal_complement<const D: usize>(
    n: &SVector<f64, D>,
) -> ArrayVec<SVector<f64, D>, 3> {
    let mut tangents: ArrayVec<SVector<f64, D>, 3> = ArrayVec::new();
    let needed = D.saturating_sub(1).min(3);

    for axis in 0..D {
        if tangents.len() >= needed {
            break;
        }
        let mut v = SVector::<f64, D>::zeros();
        v[axis] = 1.0;

        // Gram-Schmidt: subtract projection onto n and existing tangents
        v -= n * n.dot(&v);
        for t in tangents.iter() {
            v -= t * t.dot(&v);
        }

        let len = v.norm();
        if len > 1e-8 {
            tangents.push(v / len);
        }
    }
    tangents
}

/// Generate perturbation directions around `normal`.
///
/// - Cardinal: `normalize(n ± ε·t)` for each tangent `t`
/// - Diagonal: `normalize(n + ε·(t0 ± t1) / √2)` (3D only, covers box corners)
fn build_perturbations<const D: usize>(
    normal: &SVector<f64, D>,
    tangents: &[SVector<f64, D>],
) -> ArrayVec<SVector<f64, D>, 8> {
    let mut dirs: ArrayVec<SVector<f64, D>, 8> = ArrayVec::new();
    let eps = PERTURB_EPS;

    // Cardinal: ± each tangent
    for t in tangents {
        for &sign in &[1.0_f64, -1.0] {
            let d = normal + t * (sign * eps);
            let len = d.norm();
            if len > 1e-15 && dirs.len() < dirs.capacity() {
                dirs.push(d / len);
            }
        }
    }

    // Diagonal (only when ≥2 tangents, covers box corners)
    if tangents.len() >= 2 && dirs.len() + 4 <= dirs.capacity() {
        let t0 = tangents[0];
        let t1 = tangents[1];
        let inv_sqrt2 = 1.0 / 2.0_f64.sqrt();
        for &s0 in &[1.0_f64, -1.0] {
            for &s1 in &[1.0_f64, -1.0] {
                if dirs.len() >= dirs.capacity() {
                    break;
                }
                let d = normal + (t0 * s0 + t1 * s1) * (eps * inv_sqrt2);
                let len = d.norm();
                if len > 1e-15 {
                    dirs.push(d / len);
                }
            }
        }
    }

    dirs
}

// ---------------------------------------------------------------------------
// Best-4 contact point selection (Catto 2011)
// ---------------------------------------------------------------------------

/// Reduce `candidates` to at most `max_count` points maximising contact area.
///
/// Algorithm:
/// 1. Keep the deepest point (most penetration).
/// 2. Keep the point farthest from #1 (maximise patch extent).
/// 3. Keep the point farthest from the line #1-#2 (maximise triangle area).
/// 4. Keep the point farthest from the centroid of #1-#2-#3 (maximise quadrilateral area).
fn best_n<const D: usize>(
    candidates: &[(SVector<f64, D>, f64)],
    max_count: usize,
) -> ArrayVec<ContactPoint<D>, MAX_CONTACTS> {
    let mut out: ArrayVec<ContactPoint<D>, MAX_CONTACTS> = ArrayVec::new();
    if candidates.is_empty() || max_count == 0 {
        return out;
    }

    let n = candidates.len().min(max_count);
    if n == 0 {
        return out;
    }

    // Helper to push a candidate
    macro_rules! push_cand {
        ($i:expr) => {
            if out.len() < MAX_CONTACTS {
                out.push(ContactPoint {
                    position: candidates[$i].0,
                    depth: candidates[$i].1,
                    lambda: 0.0,
                });
            }
        };
    }

    // 1. Deepest point
    let i0 = (0..candidates.len())
        .max_by(|&a, &b| candidates[a].1.total_cmp(&candidates[b].1))
        .unwrap();
    push_cand!(i0);

    if n <= 1 || candidates.len() <= 1 {
        return out;
    }

    // 2. Farthest from p0
    let p0 = candidates[i0].0;
    let i1 = (0..candidates.len())
        .filter(|&i| i != i0)
        .max_by(|&a, &b| {
            (candidates[a].0 - p0)
                .norm_squared()
                .total_cmp(&(candidates[b].0 - p0).norm_squared())
        })
        .unwrap();
    push_cand!(i1);

    if n <= 2 || candidates.len() <= 2 {
        return out;
    }

    // 3. Farthest from line p0-p1 (maximise triangle area)
    let p1 = candidates[i1].0;
    let edge = p1 - p0;
    let edge_len_sq = edge.norm_squared();

    let i2 = (0..candidates.len())
        .filter(|&i| i != i0 && i != i1)
        .max_by(|&a, &b| {
            perp_dist_sq(&candidates[a].0, &p0, &edge, edge_len_sq)
                .total_cmp(&perp_dist_sq(&candidates[b].0, &p0, &edge, edge_len_sq))
        })
        .unwrap();
    push_cand!(i2);

    if n <= 3 || candidates.len() <= 3 {
        return out;
    }

    // 4. Farthest from centroid of the three existing points
    let p2 = candidates[i2].0;
    let centroid = (p0 + p1 + p2) / 3.0;

    let i3_opt = (0..candidates.len())
        .filter(|&i| i != i0 && i != i1 && i != i2)
        .max_by(|&a, &b| {
            (candidates[a].0 - centroid)
                .norm_squared()
                .total_cmp(&(candidates[b].0 - centroid).norm_squared())
        });
    if let Some(i3) = i3_opt {
        push_cand!(i3);
    }

    out
}

/// Squared perpendicular distance from point `p` to the infinite line through
/// `origin` in direction `edge` (with precomputed `edge.norm_sq()`).
#[inline]
fn perp_dist_sq<const D: usize>(
    p: &SVector<f64, D>,
    origin: &SVector<f64, D>,
    edge: &SVector<f64, D>,
    edge_len_sq: f64,
) -> f64 {
    let v = p - origin;
    if edge_len_sq < 1e-30 {
        return v.norm_squared();
    }
    let proj = edge * (v.dot(edge) / edge_len_sq);
    (v - proj).norm_squared()
}

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;
    use crate::body::BodyHandle;
    use symtropy_math::{HyperBox, Point, Sphere, Transform};

    fn vec3(x: f64, y: f64, z: f64) -> SVector<f64, 3> {
        SVector::from([x, y, z])
    }

    #[test]
    fn sphere_sphere_gives_single_contact() {
        // Two unit spheres just touching: primary contact only, no useful secondaries
        let sa = Sphere::unit();
        let sb = Sphere::unit();
        let pos_a = vec3(0.0, 0.0, 0.0);
        let pos_b = vec3(1.8, 0.0, 0.0);
        let normal = vec3(1.0, 0.0, 0.0); // from A to B
        let depth = 0.2;

        let m = generate_contact_manifold(
            &sa, &pos_a, &sb, &pos_b, normal, depth,
            BodyHandle(0), BodyHandle(1),
        );
        // Sphere support is continuous — perturbed queries converge to same point
        // We expect at least 1 contact
        assert!(!m.points.is_empty());
        // Primary contact depth should match
        assert!((m.depth() - depth).abs() < DEPTH_TOLERANCE + 0.01);
    }

    #[test]
    fn box_box_flat_face_generates_multiple_contacts() {
        // 1×1×1 HyperBox resting on top of another 1×1×1 box.
        // Top face of lower box should produce ≥2 contact points.
        let box_a = HyperBox::new([0.5, 0.5, 0.5]);
        let box_b = HyperBox::new([0.5, 0.5, 0.5]);
        let pos_a = vec3(0.0, 0.0, 0.0);
        let pos_b = vec3(0.0, 0.9, 0.0); // slight overlap
        let normal = vec3(0.0, -1.0, 0.0); // A pushes down into B
        let depth = 0.1;

        let m = generate_contact_manifold(
            &box_a, &pos_a, &box_b, &pos_b, normal, depth,
            BodyHandle(0), BodyHandle(1),
        );
        // Flat box-box contact should have more than 1 contact point
        assert!(m.points.len() >= 2,
            "expected ≥2 contact points for flat face, got {}", m.points.len());
    }

    #[test]
    fn all_contacts_have_positive_depth() {
        let box_a = HyperBox::new([0.5, 0.5, 0.5]);
        let box_b = HyperBox::new([0.5, 0.5, 0.5]);
        let pos_a = vec3(0.0, 0.0, 0.0);
        let pos_b = vec3(0.0, 0.9, 0.0);
        let normal = vec3(0.0, -1.0, 0.0);
        let depth = 0.1;

        let m = generate_contact_manifold(
            &box_a, &pos_a, &box_b, &pos_b, normal, depth,
            BodyHandle(0), BodyHandle(1),
        );
        for pt in &m.points {
            assert!(pt.depth > 0.0, "all contact points must have positive depth");
        }
    }

    #[test]
    fn max_four_contacts_returned() {
        let box_a = HyperBox::new([0.5, 0.5, 0.5]);
        let box_b = HyperBox::new([0.5, 0.5, 0.5]);
        let pos_a = vec3(0.0, 0.0, 0.0);
        let pos_b = vec3(0.0, 0.9, 0.0);
        let normal = vec3(0.0, -1.0, 0.0);
        let depth = 0.1;

        let m = generate_contact_manifold(
            &box_a, &pos_a, &box_b, &pos_b, normal, depth,
            BodyHandle(0), BodyHandle(1),
        );
        assert!(m.points.len() <= 4, "at most 4 contact points");
    }

    #[test]
    fn contact_normal_matches_epa_normal() {
        let sa = Sphere::unit();
        let sb = Sphere::unit();
        let pos_a = vec3(0.0, 0.0, 0.0);
        let pos_b = vec3(1.8, 0.0, 0.0);
        let normal = vec3(1.0, 0.0, 0.0);
        let depth = 0.2;

        let m = generate_contact_manifold(
            &sa, &pos_a, &sb, &pos_b, normal, depth,
            BodyHandle(0), BodyHandle(1),
        );
        // Normal is passed through unchanged
        assert!((m.normal - normal).norm() < 1e-10);
    }

    #[test]
    fn orthonormal_complement_3d_produces_two_orthogonal_tangents() {
        let n = vec3(0.0, 1.0, 0.0);
        let ts = orthonormal_complement(&n);
        assert_eq!(ts.len(), 2);
        for t in &ts {
            assert!(n.dot(t).abs() < 1e-10, "tangent must be perpendicular to n");
            assert!((t.norm() - 1.0).abs() < 1e-10, "tangent must be unit length");
        }
        assert!(ts[0].dot(&ts[1]).abs() < 1e-10, "tangents must be orthogonal");
    }

    #[test]
    fn orthonormal_complement_2d_produces_one_tangent() {
        let n = SVector::<f64, 2>::from([0.0, 1.0]);
        let ts = orthonormal_complement(&n);
        assert_eq!(ts.len(), 1);
        assert!(n.dot(&ts[0]).abs() < 1e-10);
    }

    #[test]
    fn orthonormal_complement_4d_produces_three_tangents() {
        let n = SVector::<f64, 4>::from([0.0, 1.0, 0.0, 0.0]);
        let ts = orthonormal_complement(&n);
        assert_eq!(ts.len(), 3);
        for t in &ts {
            assert!(n.dot(t).abs() < 1e-10);
        }
    }

    #[test]
    fn best_n_with_four_candidates_returns_four() {
        let pts: Vec<(SVector<f64, 3>, f64)> = vec![
            (vec3(1.0, 0.0, 0.0), 0.1),
            (vec3(-1.0, 0.0, 0.0), 0.1),
            (vec3(0.0, 0.0, 1.0), 0.1),
            (vec3(0.0, 0.0, -1.0), 0.1),
        ];
        let result = best_n(&pts, 4);
        assert_eq!(result.len(), 4);
    }

    #[test]
    fn best_n_returns_deepest_first() {
        let pts: Vec<(SVector<f64, 3>, f64)> = vec![
            (vec3(0.0, 0.0, 0.0), 0.05),
            (vec3(1.0, 0.0, 0.0), 0.2),   // deepest
            (vec3(0.0, 0.0, 1.0), 0.1),
        ];
        let result = best_n(&pts, 4);
        assert!((result[0].depth - 0.2).abs() < 1e-12, "first result must be deepest");
    }

    #[test]
    fn lambda_initialized_to_zero() {
        let box_a = HyperBox::new([0.5, 0.5, 0.5]);
        let box_b = HyperBox::new([0.5, 0.5, 0.5]);
        let pos_a = vec3(0.0, 0.0, 0.0);
        let pos_b = vec3(0.0, 0.9, 0.0);
        let normal = vec3(0.0, -1.0, 0.0);
        let depth = 0.1;

        let m = generate_contact_manifold(
            &box_a, &pos_a, &box_b, &pos_b, normal, depth,
            BodyHandle(0), BodyHandle(1),
        );
        for pt in &m.points {
            assert_eq!(pt.lambda, 0.0, "lambda must start at zero");
        }
    }
}