alice-physics 3.0.1

//! SDF Contact Manifold Generation
//!
//! Multi-point contact manifold from SDF surface sampling.
//! Replaces single-point contacts with stable N-point manifolds
//! for robust stacking, sliding, and resting behavior.
//!
//! # Algorithm
//!
//! 1. Detect initial penetration via SDF distance query
//! 2. Sample SDF surface in a local tangent frame around the contact
//! 3. Build manifold from deepest-penetrating samples
//! 4. Reduce to 4-point manifold (maximizes contact area)
//!
//! Author: Moroya Sakamoto

#[cfg(feature = "std")]
use crate::collider::Contact;
#[cfg(feature = "std")]
use crate::math::{Fix128, Vec3Fix};
#[cfg(feature = "std")]
use crate::sdf_collider::SdfCollider;

// ============================================================================
// Manifold Configuration
// ============================================================================

/// Configuration for SDF manifold generation
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct ManifoldConfig {
    /// Number of sample points in each tangent direction (total = samples^2)
    pub samples_per_axis: usize,
    /// Sampling radius around the initial contact point
    pub sample_radius: f32,
    /// Maximum number of contacts in the final manifold. Reducing a larger
    /// candidate set picks at most 4 points (deepest, furthest, then the two
    /// that maximise the contact area), so values above 4 only matter when
    /// there are fewer candidates than that. `0` yields an empty manifold.
    pub max_contacts: usize,
    /// Minimum penetration depth to include in manifold
    pub min_depth: f32,
}

impl Default for ManifoldConfig {
    fn default() -> Self {
        Self {
            samples_per_axis: 5,
            sample_radius: 0.5,
            max_contacts: 4,
            min_depth: 0.001,
        }
    }
}

// ============================================================================
// Contact Manifold
// ============================================================================

/// Multi-point contact manifold from SDF surface
#[cfg(feature = "std")]
#[derive(Clone, Debug)]
pub struct SdfManifold {
    /// Contact points (up to `max_contacts`)
    pub contacts: Vec<Contact>,
    /// Average contact normal
    pub normal: Vec3Fix,
    /// Average penetration depth
    pub avg_depth: Fix128,
}

#[cfg(feature = "std")]
impl SdfManifold {
    /// Empty manifold
    #[must_use]
    pub const fn empty() -> Self {
        Self {
            contacts: Vec::new(),
            normal: Vec3Fix::ZERO,
            avg_depth: Fix128::ZERO,
        }
    }

    /// Number of contact points
    #[inline]
    #[must_use]
    pub fn len(&self) -> usize {
        self.contacts.len()
    }

    /// Check if manifold has no contacts
    #[inline]
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.contacts.is_empty()
    }

    /// Get the deepest contact
    #[must_use]
    pub fn deepest(&self) -> Option<&Contact> {
        self.contacts.iter().max_by(|a, b| a.depth.cmp(&b.depth))
    }
}

// ============================================================================
// Manifold Generation
// ============================================================================

/// Build a tangent frame from a normal vector.
///
/// Returns (tangent1, tangent2) orthogonal to the normal.
#[cfg(feature = "std")]
#[inline]
fn build_tangent_frame(normal: Vec3Fix) -> (Vec3Fix, Vec3Fix) {
    let (nx, _ny, _nz) = normal.to_f32();

    // Choose axis least aligned with normal for stable cross product
    let up = if nx.abs() < 0.9 {
        Vec3Fix::UNIT_X
    } else {
        Vec3Fix::UNIT_Y
    };

    let t1 = normal.cross(up).normalize();
    let t2 = normal.cross(t1).normalize();
    (t1, t2)
}

/// Generate multi-point contact manifold from SDF.
///
/// Samples the SDF surface around an initial contact point to build
/// a stable manifold for constraint solving.
#[cfg(feature = "std")]
#[must_use]
pub fn generate_sdf_manifold(
    center: Vec3Fix,
    radius: Fix128,
    sdf: &SdfCollider,
    config: &ManifoldConfig,
) -> SdfManifold {
    // 1. Initial probe: check if there's any contact at all
    let (lx, ly, lz) = sdf.world_to_local(center);
    let dist = sdf.field.distance(lx, ly, lz);
    let scale = sdf.scale_f32;
    let world_dist = dist * scale;
    let radius_f32 = radius.to_f32();

    if radius_f32 - world_dist <= 0.0 {
        return SdfManifold::empty();
    }

    // 2. Get initial normal and build tangent frame
    let (nx, ny, nz) = sdf.field.normal(lx, ly, lz);
    let world_normal = sdf.local_normal_to_world(nx, ny, nz);
    let (t1, t2) = build_tangent_frame(world_normal);

    // 3. Sample grid around contact point
    let n = config.samples_per_axis;
    // Grid centred on the contact point: offsets run from `-sample_radius` to
    // `+sample_radius` symmetrically for every `n` (for odd `n` this is the
    // classic `(i - n/2) * radius / (n/2)`; an even `n` used to be shifted by
    // half a step toward -tangent).
    let half = (n.saturating_sub(1)) as f32 * 0.5;
    let step = if half > 0.0 {
        config.sample_radius / half
    } else {
        config.sample_radius
    };
    let mut candidates: Vec<(Contact, f32)> = Vec::with_capacity(n * n);

    for i in 0..n {
        for j in 0..n {
            let di = (i as f32 - half) * step;
            let dj = (j as f32 - half) * step;

            let offset = t1 * Fix128::from_f32(di) + t2 * Fix128::from_f32(dj);
            let sample_pos = center + offset;

            let (slx, sly, slz) = sdf.world_to_local(sample_pos);
            let sample_dist = sdf.field.distance(slx, sly, slz) * scale;
            let penetration = radius_f32 - sample_dist;

            if penetration > config.min_depth {
                let (snx, sny, snz) = sdf.field.normal(slx, sly, slz);
                let sample_normal = sdf.local_normal_to_world(snx, sny, snz);
                let depth = Fix128::from_f32(penetration);

                let point_a = sample_pos - sample_normal * radius;
                let point_b = sample_pos - sample_normal * Fix128::from_f32(sample_dist);

                candidates.push((
                    Contact {
                        depth,
                        normal: sample_normal,
                        point_a,
                        point_b,
                    },
                    penetration,
                ));
            }
        }
    }

    if candidates.is_empty() {
        return SdfManifold::empty();
    }

    // 4. Reduce to max_contacts using area-maximizing selection
    let contacts = reduce_manifold(&candidates, config.max_contacts);
    if contacts.is_empty() {
        return SdfManifold::empty();
    }

    // 5. Compute average normal and depth
    let mut sum_normal = Vec3Fix::ZERO;
    let mut sum_depth = Fix128::ZERO;
    for c in &contacts {
        sum_normal = sum_normal + c.normal;
        sum_depth = sum_depth + c.depth;
    }
    let count = Fix128::from_int(contacts.len() as i64);
    let avg_normal = sum_normal.normalize();
    let avg_depth = if contacts.is_empty() {
        Fix128::ZERO
    } else {
        sum_depth / count
    };

    SdfManifold {
        contacts,
        normal: avg_normal,
        avg_depth,
    }
}

/// Reduce contact candidates to N points maximizing contact area.
///
/// Algorithm:
/// 1. Keep the deepest contact
/// 2. Keep the point furthest from it
/// 3. Keep the point maximizing triangle area
/// 4. Keep the point maximizing quadrilateral area
#[cfg(feature = "std")]
fn reduce_manifold(candidates: &[(Contact, f32)], max_contacts: usize) -> Vec<Contact> {
    if max_contacts == 0 {
        return Vec::new();
    }
    if candidates.len() <= max_contacts {
        return candidates.iter().map(|(c, _)| *c).collect();
    }

    let mut selected: Vec<usize> = Vec::with_capacity(max_contacts);

    // 1. Deepest point
    let deepest_idx = candidates
        .iter()
        .enumerate()
        .max_by(|(_, a), (_, b)| a.1.partial_cmp(&b.1).unwrap_or(core::cmp::Ordering::Equal))
        .map_or(0, |(i, _)| i);
    selected.push(deepest_idx);

    // 2. Furthest from deepest
    if max_contacts >= 2 {
        let p0 = candidates[deepest_idx].0.point_b;
        let mut best_dist = Fix128::ZERO;
        let mut best_idx = candidates
            .iter()
            .enumerate()
            .find(|(i, _)| !selected.contains(i))
            .map_or(0, |(i, _)| i);
        for (i, (c, _)) in candidates.iter().enumerate() {
            if selected.contains(&i) {
                continue;
            }
            let d = (c.point_b - p0).length_squared();
            if d > best_dist {
                best_dist = d;
                best_idx = i;
            }
        }
        selected.push(best_idx);
    }

    // 3. Maximize triangle area
    if max_contacts >= 3 && candidates.len() >= 3 {
        let p0 = candidates[selected[0]].0.point_b;
        let p1 = candidates[selected[1]].0.point_b;
        let edge = p1 - p0;
        let mut best_area = Fix128::ZERO;
        let mut best_idx = candidates
            .iter()
            .enumerate()
            .find(|(i, _)| !selected.contains(i))
            .map_or(0, |(i, _)| i);
        for (i, (c, _)) in candidates.iter().enumerate() {
            if selected.contains(&i) {
                continue;
            }
            let v = c.point_b - p0;
            let cross = edge.cross(v);
            let area = cross.length_squared();
            if area > best_area {
                best_area = area;
                best_idx = i;
            }
        }
        selected.push(best_idx);
    }

    // 4. Maximize quadrilateral area
    if max_contacts >= 4 && candidates.len() >= 4 {
        let p0 = candidates[selected[0]].0.point_b;
        let p1 = candidates[selected[1]].0.point_b;
        let p2 = candidates[selected[2]].0.point_b;
        let tri_normal = (p1 - p0).cross(p2 - p0);
        let tri_n2 = tri_normal.length_squared();
        let mut best_score = Fix128::ZERO;
        let mut best_idx = candidates
            .iter()
            .enumerate()
            .find(|(i, _)| !selected.contains(i))
            .map_or(0, |(i, _)| i);
        for (i, (c, _)) in candidates.iter().enumerate() {
            if selected.contains(&i) {
                continue;
            }
            let score = if tri_n2.is_zero() {
                // The first three points are collinear: no plane to measure
                // an area in, so take the point furthest from them.
                (c.point_b - p0).length_squared()
            } else {
                // Twice the area of the convex hull of {p0, p1, p2, c} in the
                // triangle's plane, scaled by |n|. For a convex quadrilateral
                // with diagonals d1, d2 it is (d1 x d2) . n; the three ways to
                // pair the points into diagonals are tried and the largest
                // wins (the non-convex pairings give less). When one point
                // lies inside the triangle of the other three the hull is that
                // triangle, so the four triangles (the original one, n . n,
                // and the three that contain `c`) are candidates too.
                let d = c.point_b;
                let q1 = (p2 - p0).cross(d - p1).dot(tri_normal).abs();
                let q2 = (d - p0).cross(p2 - p1).dot(tri_normal).abs();
                let q3 = (p1 - p0).cross(d - p2).dot(tri_normal).abs();
                let t1 = (p1 - p0).cross(d - p0).dot(tri_normal).abs();
                let t2 = (p2 - p0).cross(d - p0).dot(tri_normal).abs();
                let t3 = (p2 - p1).cross(d - p1).dot(tri_normal).abs();
                let mut hull = tri_n2;
                for q in [q1, q2, q3, t1, t2, t3] {
                    if q > hull {
                        hull = q;
                    }
                }
                hull
            };
            if score > best_score {
                best_score = score;
                best_idx = i;
            }
        }
        selected.push(best_idx);
    }

    selected.iter().map(|&i| candidates[i].0).collect()
}

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

#[cfg(all(test, feature = "std"))]
mod tests {
    use super::*;
    use crate::math::QuatFix;
    use crate::sdf_collider::ClosureSdf;

    fn ground_plane() -> ClosureSdf {
        ClosureSdf::new(|_x, y, _z| y, |_x, _y, _z| (0.0, 1.0, 0.0))
    }

    fn unit_sphere() -> ClosureSdf {
        ClosureSdf::new(
            |x, y, z| z.mul_add(z, x.mul_add(x, y * y)).sqrt() - 1.0,
            |x, y, z| {
                let len = z.mul_add(z, x.mul_add(x, y * y)).sqrt();
                if len < 1e-10 {
                    (0.0, 1.0, 0.0)
                } else {
                    (x / len, y / len, z / len)
                }
            },
        )
    }

    #[test]
    fn test_manifold_ground_plane() {
        let sdf =
            SdfCollider::new_static(Box::new(ground_plane()), Vec3Fix::ZERO, QuatFix::IDENTITY);

        // Sphere penetrating ground
        let center = Vec3Fix::from_f32(0.0, 0.3, 0.0);
        let radius = Fix128::ONE;
        let config = ManifoldConfig::default();

        let manifold = generate_sdf_manifold(center, radius, &sdf, &config);
        assert!(!manifold.is_empty(), "Should generate contacts");
        assert!(
            manifold.len() <= config.max_contacts,
            "Should not exceed max contacts"
        );
    }

    #[test]
    fn test_manifold_no_contact() {
        let sdf =
            SdfCollider::new_static(Box::new(ground_plane()), Vec3Fix::ZERO, QuatFix::IDENTITY);

        // Sphere above ground
        let center = Vec3Fix::from_f32(0.0, 5.0, 0.0);
        let radius = Fix128::ONE;
        let config = ManifoldConfig::default();

        let manifold = generate_sdf_manifold(center, radius, &sdf, &config);
        assert!(manifold.is_empty(), "Should have no contacts");
    }

    #[test]
    fn test_manifold_sphere_sdf() {
        let sdf =
            SdfCollider::new_static(Box::new(unit_sphere()), Vec3Fix::ZERO, QuatFix::IDENTITY);

        // Small sphere inside SDF sphere
        let center = Vec3Fix::from_f32(0.5, 0.0, 0.0);
        let radius = Fix128::from_f32(0.3);
        let config = ManifoldConfig {
            samples_per_axis: 3,
            sample_radius: 0.2,
            ..Default::default()
        };

        let manifold = generate_sdf_manifold(center, radius, &sdf, &config);
        assert!(
            !manifold.is_empty(),
            "Should generate contacts inside sphere"
        );
    }

    #[test]
    fn test_tangent_frame() {
        let normal = Vec3Fix::UNIT_Y;
        let (t1, t2) = build_tangent_frame(normal);

        // t1 and t2 should be orthogonal to normal
        let d1 = normal.dot(t1).to_f32();
        let d2 = normal.dot(t2).to_f32();
        assert!(d1.abs() < 0.01, "t1 should be orthogonal to normal");
        assert!(d2.abs() < 0.01, "t2 should be orthogonal to normal");
    }

    fn cand(x: f32, y: f32, depth: f32) -> (Contact, f32) {
        (
            Contact {
                depth: Fix128::from_f32(depth),
                normal: Vec3Fix::UNIT_Y,
                point_a: Vec3Fix::from_f32(x, y, 0.0),
                point_b: Vec3Fix::from_f32(x, y, 0.0),
            },
            depth,
        )
    }

    /// First three picks are A (deepest), B (furthest from A) and C (largest
    /// triangle with AB); the fourth must maximise the hull area of the four.
    /// A = (0,0), B = (8,0), C = (0,5). Each case lists a decoy first (a point
    /// inside triangle ABC, hull area 20) so a scoring that cannot see the
    /// candidate's hull falls back to the decoy by tie.
    fn fourth_pick(extra: &[(f32, f32)]) -> (f32, f32) {
        let mut c = vec![
            cand(0.0, 0.0, 0.5),
            cand(8.0, 0.0, 0.1),
            cand(0.0, 5.0, 0.1),
        ];
        for &(x, y) in extra {
            c.push(cand(x, y, 0.1));
        }
        let r = reduce_manifold(&c, 4);
        assert_eq!(r.len(), 4);
        let (x, y, _) = r[3].point_b.to_f32();
        (x, y)
    }

    #[test]
    fn reduce_fourth_point_maximises_hull_area_for_every_pairing() {
        // quad A-B-D-C (hull 31 vs decoy 20)
        assert_eq!(fourth_pick(&[(1.0, 1.0), (6.0, 4.0)]), (6.0, 4.0));
        // quad B-C-D-A (D left of AC)
        assert_eq!(fourth_pick(&[(1.0, 1.0), (-3.0, 2.0)]), (-3.0, 2.0));
        // quad A-D-B-C (D below AB)
        assert_eq!(fourth_pick(&[(1.0, 1.0), (3.0, -2.0)]), (3.0, -2.0));
        // A inside triangle BCD: hull is that triangle (39.5) and beats the
        // convex quad with D1 = (6, 3.5) (29)
        assert_eq!(fourth_pick(&[(6.0, 3.5), (-3.0, -3.0)]), (-3.0, -3.0));
    }

    #[test]
    fn reduce_ties_among_interior_points_go_to_the_earliest() {
        // Both extras lie inside triangle ABC: the hull stays the triangle, so the
        // 4th point is the first candidate (deterministic tie-break), not the one
        // with the larger partial areas.
        assert_eq!(fourth_pick(&[(1.0, 1.0), (2.0, 1.5)]), (1.0, 1.0));
        assert_eq!(fourth_pick(&[(2.0, 1.5), (1.0, 1.0)]), (2.0, 1.5));
    }

    #[test]
    fn reduce_collinear_first_three_picks_the_furthest() {
        // A = (0,0) deepest, B = (8,0) furthest, C = (4,0) collinear: no plane,
        // so the fourth is the point furthest from A.
        let c = vec![
            cand(0.0, 0.0, 0.5),
            cand(8.0, 0.0, 0.1),
            cand(4.0, 0.0, 0.1),
            cand(2.0, 0.0, 0.1),
            cand(6.0, 0.0, 0.1),
        ];
        let r = reduce_manifold(&c, 4);
        assert_eq!(r.len(), 4);
        let xs: Vec<f32> = r.iter().map(|c| c.point_b.to_f32().0).collect();
        assert_eq!(xs[0], 0.0);
        assert_eq!(xs[1], 8.0);
        assert_eq!(xs[3], 6.0);
    }

    #[test]
    fn test_reduce_manifold() {
        let contacts: Vec<(Contact, f32)> = (0..10)
            .map(|i| {
                let angle = (i as f32) * 0.628; // ~2pi/10
                (
                    Contact {
                        depth: Fix128::from_f32((i as f32).mul_add(0.01, 0.1)),
                        normal: Vec3Fix::UNIT_Y,
                        point_a: Vec3Fix::from_f32(
                            crate::det_math::cos(angle),
                            0.0,
                            crate::det_math::sin(angle),
                        ),
                        point_b: Vec3Fix::from_f32(
                            crate::det_math::cos(angle),
                            -0.1,
                            crate::det_math::sin(angle),
                        ),
                    },
                    (i as f32).mul_add(0.01, 0.1),
                )
            })
            .collect();

        let reduced = reduce_manifold(&contacts, 4);
        assert_eq!(reduced.len(), 4, "Should reduce to 4 contacts");
    }
}