featherstone 0.1.0

//! Collision geometry types for articulated body contact detection.
//!
//! Shapes are attached to body links and used by the contact detection pipeline
//! ([`crate::contact`]) to generate [`crate::contact::ContactManifold`]s.
//!
//! # Supported shapes
//!
//! | Shape | Description | Contact method |
//! |-------|-------------|----------------|
//! | [`ColliderShape::Sphere`] | Sphere by radius | Analytical |
//! | [`ColliderShape::Box`] | Axis-aligned box by half-extents | SAT / Sutherland-Hodgman |
//! | [`ColliderShape::Capsule`] | Cylinder + hemispheres along Y | Analytical (segment distance) |
//! | [`ColliderShape::Cylinder`] | Cylinder along Z | GJK/EPA |
//! | [`ColliderShape::ConvexHull`] | Arbitrary convex vertices + faces | GJK/EPA |
//! | [`ColliderShape::DecomposedMesh`] | Concave mesh split into convex hulls | Per-hull GJK/EPA |
//!
//! # Example
//!
//! ```rust
//! use featherstone::collider::ColliderShape;
//! use nalgebra::Vector3;
//!
//! let sphere = ColliderShape::Sphere { radius: 0.1 };
//! let cube = ColliderShape::Box { half_extents: Vector3::new(0.5, 0.5, 0.5) };
//! ```

use nalgebra::Vector3;

/// A convex hull represented by vertices and faces.
#[derive(Clone, Debug)]
pub struct ConvexHull {
    /// Vertices in local frame
    pub vertices: Vec<Vector3<f32>>,
    /// Faces as ordered vertex index lists
    pub faces: Vec<Vec<usize>>,
    /// Precomputed AABB min
    pub aabb_min: Vector3<f32>,
    /// Precomputed AABB max
    pub aabb_max: Vector3<f32>,
}

impl ConvexHull {
    /// Create a convex hull from vertices and faces, precomputing AABB.
    pub fn new(vertices: Vec<Vector3<f32>>, faces: Vec<Vec<usize>>) -> Self {
        let mut aabb_min = Vector3::new(f32::MAX, f32::MAX, f32::MAX);
        let mut aabb_max = Vector3::new(f32::MIN, f32::MIN, f32::MIN);
        for v in &vertices {
            aabb_min.x = aabb_min.x.min(v.x);
            aabb_min.y = aabb_min.y.min(v.y);
            aabb_min.z = aabb_min.z.min(v.z);
            aabb_max.x = aabb_max.x.max(v.x);
            aabb_max.y = aabb_max.y.max(v.y);
            aabb_max.z = aabb_max.z.max(v.z);
        }
        if vertices.is_empty() {
            aabb_min = Vector3::zeros();
            aabb_max = Vector3::zeros();
        }
        Self { vertices, faces, aabb_min, aabb_max }
    }

    /// Number of vertices.
    pub fn vertex_count(&self) -> usize { self.vertices.len() }

    /// Number of faces.
    pub fn face_count(&self) -> usize { self.faces.len() }

    /// AABB as (min, max).
    pub fn aabb(&self) -> (Vector3<f32>, Vector3<f32>) { (self.aabb_min, self.aabb_max) }

    /// Support function: find the vertex farthest along direction `d`.
    /// Returns (index, vertex). Used by GJK.
    /// Returns (0, origin) for empty hulls.
    pub fn support(&self, direction: &Vector3<f32>) -> (usize, Vector3<f32>) {
        if self.vertices.is_empty() {
            return (0, Vector3::zeros());
        }
        let mut best_idx = 0;
        let mut best_dot = f32::MIN;
        for (i, v) in self.vertices.iter().enumerate() {
            let d = v.dot(direction);
            if d > best_dot {
                best_dot = d;
                best_idx = i;
            }
        }
        (best_idx, self.vertices[best_idx])
    }

    /// Decompose a potentially concave mesh into convex sub-hulls.
    pub fn decompose(vertices: Vec<Vector3<f32>>, faces: Vec<Vec<usize>>, max_hulls: usize) -> Vec<ConvexHull> {
        if vertices.len() < 4 || max_hulls <= 1 {
            return vec![ConvexHull::new(vertices, faces)];
        }

        let mut min = Vector3::new(f32::MAX, f32::MAX, f32::MAX);
        let mut max = Vector3::new(f32::MIN, f32::MIN, f32::MIN);
        for v in &vertices {
            min.x = min.x.min(v.x); min.y = min.y.min(v.y); min.z = min.z.min(v.z);
            max.x = max.x.max(v.x); max.y = max.y.max(v.y); max.z = max.z.max(v.z);
        }

        let extent = max - min;
        let (axis, mid) = if extent.x >= extent.y && extent.x >= extent.z {
            (0, (min.x + max.x) * 0.5)
        } else if extent.y >= extent.z {
            (1, (min.y + max.y) * 0.5)
        } else {
            (2, (min.z + max.z) * 0.5)
        };

        let mut left_verts = Vec::new();
        let mut right_verts = Vec::new();
        for v in &vertices {
            if v[axis] <= mid {
                left_verts.push(*v);
            } else {
                right_verts.push(*v);
            }
        }

        if left_verts.len() < 4 || right_verts.len() < 4 {
            return vec![ConvexHull::new(vertices, faces)];
        }

        let half = max_hulls / 2;
        let mut result = ConvexHull::decompose(left_verts, vec![], half.max(1));
        result.extend(ConvexHull::decompose(right_verts, vec![], (max_hulls - half).max(1)));
        result
    }
}

/// Collision shape stored per body link.
#[derive(Clone, Debug)]
pub enum ColliderShape {
    /// Sphere defined by its radius.
    Sphere {
        /// Sphere radius in meters.
        radius: f32,
    },
    /// Axis-aligned box defined by half-extents along each axis.
    Box {
        /// Half-extents (half width, half height, half depth).
        half_extents: Vector3<f32>,
    },
    /// Capsule (cylinder with hemispherical caps) along the Y axis.
    Capsule {
        /// Half the height of the cylindrical portion.
        half_height: f32,
        /// Radius of the capsule (cylinder and hemisphere).
        radius: f32,
    },
    /// Cylinder along the Z axis.
    Cylinder {
        /// Half the height of the cylinder.
        half_height: f32,
        /// Radius of the cylinder.
        radius: f32,
    },
    /// Arbitrary convex hull defined by vertices and faces.
    ConvexHull {
        /// The convex hull geometry.
        hull: ConvexHull,
    },
    /// Concave mesh decomposed into multiple convex sub-hulls.
    DecomposedMesh {
        /// The convex sub-hulls forming the decomposition.
        hulls: Vec<ConvexHull>,
    },
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn convex_hull_support_returns_farthest_vertex() {
        let hull = ConvexHull::new(
            vec![
                Vector3::new(0.0, 0.0, 0.0),
                Vector3::new(1.0, 0.0, 0.0),
                Vector3::new(0.0, 1.0, 0.0),
                Vector3::new(0.0, 0.0, 1.0),
            ],
            vec![vec![0, 1, 2], vec![0, 1, 3]],
        );
        let (idx, pt) = hull.support(&Vector3::new(1.0, 0.0, 0.0));
        assert_eq!(idx, 1);
        assert!((pt.x - 1.0).abs() < 1e-6);
    }

    #[test]
    fn convex_hull_support_empty_returns_origin() {
        let hull = ConvexHull::new(vec![], vec![]);
        let (idx, pt) = hull.support(&Vector3::new(1.0, 0.0, 0.0));
        assert_eq!(idx, 0);
        assert!(pt.norm() < 1e-6);
    }

    #[test]
    fn convex_hull_aabb_bounds_all_vertices() {
        let hull = ConvexHull::new(
            vec![
                Vector3::new(-1.0, -2.0, -3.0),
                Vector3::new(4.0, 5.0, 6.0),
                Vector3::new(0.0, 0.0, 0.0),
            ],
            vec![],
        );
        let (min, max) = hull.aabb();
        assert!(min.x <= -1.0 && min.y <= -2.0 && min.z <= -3.0);
        assert!(max.x >= 4.0 && max.y >= 5.0 && max.z >= 6.0);
    }

    #[test]
    fn convex_hull_decompose_small_input_returns_single() {
        let verts = vec![
            Vector3::new(0.0, 0.0, 0.0),
            Vector3::new(1.0, 0.0, 0.0),
            Vector3::new(0.0, 1.0, 0.0),
        ];
        let hulls = ConvexHull::decompose(verts, vec![], 4);
        assert_eq!(hulls.len(), 1); // < 4 vertices → single hull
    }

    #[test]
    fn convex_hull_decompose_splits_large_input() {
        let mut verts = Vec::new();
        for i in 0..20 {
            verts.push(Vector3::new(i as f32, (i * i) as f32 % 10.0, 0.0));
        }
        let hulls = ConvexHull::decompose(verts, vec![], 4);
        assert!(hulls.len() > 1, "should split into multiple hulls");
        assert!(hulls.len() <= 4, "should not exceed max_hulls");
    }

    #[test]
    fn collider_shape_sphere_has_positive_radius() {
        let shape = ColliderShape::Sphere { radius: 0.5 };
        if let ColliderShape::Sphere { radius } = shape {
            assert!(radius > 0.0);
        }
    }

    #[test]
    fn collider_shape_box_half_extents_positive() {
        let shape = ColliderShape::Box {
            half_extents: Vector3::new(0.5, 0.5, 0.5),
        };
        if let ColliderShape::Box { half_extents } = shape {
            assert!(half_extents.x > 0.0);
            assert!(half_extents.y > 0.0);
            assert!(half_extents.z > 0.0);
        }
    }

    #[test]
    fn collider_shape_capsule_construction() {
        let shape = ColliderShape::Capsule { half_height: 0.3, radius: 0.1 };
        if let ColliderShape::Capsule { half_height, radius } = shape {
            assert!(half_height > 0.0);
            assert!(radius > 0.0);
        } else {
            panic!("should be Capsule variant");
        }
    }

    #[test]
    fn collider_shape_cylinder_construction() {
        let shape = ColliderShape::Cylinder { half_height: 0.5, radius: 0.2 };
        if let ColliderShape::Cylinder { half_height, radius } = shape {
            assert!(half_height > 0.0);
            assert!(radius > 0.0);
        } else {
            panic!("should be Cylinder variant");
        }
    }

    #[test]
    fn collider_shape_convex_hull_construction() {
        let hull = ConvexHull::new(
            vec![Vector3::new(0.0, 0.0, 0.0), Vector3::new(1.0, 0.0, 0.0),
                 Vector3::new(0.0, 1.0, 0.0), Vector3::new(0.0, 0.0, 1.0)],
            vec![vec![0, 1, 2]],
        );
        let shape = ColliderShape::ConvexHull { hull: hull.clone() };
        if let ColliderShape::ConvexHull { hull: h } = shape {
            assert_eq!(h.vertex_count(), 4);
            assert_eq!(h.face_count(), 1);
        } else {
            panic!("should be ConvexHull variant");
        }
    }

    #[test]
    fn collider_shape_decomposed_mesh_construction() {
        let hull1 = ConvexHull::new(
            vec![Vector3::zeros(), Vector3::x(), Vector3::y(), Vector3::z()],
            vec![],
        );
        let hull2 = ConvexHull::new(
            vec![Vector3::x(), Vector3::new(2.0, 0.0, 0.0), Vector3::new(1.0, 1.0, 0.0), Vector3::new(1.0, 0.0, 1.0)],
            vec![],
        );
        let shape = ColliderShape::DecomposedMesh { hulls: vec![hull1, hull2] };
        if let ColliderShape::DecomposedMesh { hulls } = shape {
            assert_eq!(hulls.len(), 2);
        } else {
            panic!("should be DecomposedMesh variant");
        }
    }

    #[test]
    fn collider_shape_is_clone_and_debug() {
        let shape = ColliderShape::Sphere { radius: 0.1 };
        let cloned = shape.clone();
        let dbg = format!("{:?}", cloned);
        assert!(dbg.contains("Sphere"), "debug should mention Sphere");
    }

    #[test]
    fn convex_hull_aabb_contains_all_vertices() {
        let verts = vec![
            Vector3::new(-1.0, 2.0, 3.0),
            Vector3::new(4.0, -5.0, 0.0),
            Vector3::new(0.0, 0.0, 7.0),
            Vector3::new(2.0, 1.0, -2.0),
        ];
        let hull = ConvexHull::new(verts.clone(), vec![]);
        let (aabb_min, aabb_max) = hull.aabb();
        for v in &verts {
            assert!(v.x >= aabb_min.x - 1e-6 && v.x <= aabb_max.x + 1e-6,
                "vertex x={} out of AABB [{}, {}]", v.x, aabb_min.x, aabb_max.x);
            assert!(v.y >= aabb_min.y - 1e-6 && v.y <= aabb_max.y + 1e-6,
                "vertex y out of AABB");
            assert!(v.z >= aabb_min.z - 1e-6 && v.z <= aabb_max.z + 1e-6,
                "vertex z out of AABB");
        }
    }

    #[test]
    fn convex_hull_decompose_single_hull_when_too_few_verts() {
        let verts = vec![Vector3::zeros(), Vector3::x(), Vector3::y()]; // 3 < 4
        let hulls = ConvexHull::decompose(verts, vec![], 4);
        assert_eq!(hulls.len(), 1, "should not decompose with < 4 vertices");
    }

    #[test]
    fn convex_hull_support_direction_negative() {
        let hull = ConvexHull::new(
            vec![
                Vector3::new(-2.0, 0.0, 0.0),
                Vector3::new(1.0, 0.0, 0.0),
                Vector3::new(0.0, 3.0, 0.0),
                Vector3::new(0.0, 0.0, 1.0),
            ],
            vec![],
        );
        let (idx, pt) = hull.support(&Vector3::new(-1.0, 0.0, 0.0));
        assert_eq!(idx, 0, "should find the most negative-x vertex");
        assert!((pt.x - (-2.0)).abs() < 1e-6);
    }

    #[test]
    fn intent_sphere_support_returns_surface_point() {
        // For a Sphere, model support as a ConvexHull of surface samples.
        // The support function in a given direction should return a point at radius distance.
        let radius = 0.5_f32;
        let n = 26; // sample directions on axes and diagonals
        let dirs: Vec<Vector3<f32>> = vec![
            Vector3::x(), -Vector3::x(),
            Vector3::y(), -Vector3::y(),
            Vector3::z(), -Vector3::z(),
        ];
        // Build a convex hull from sphere surface points in those directions
        let verts: Vec<Vector3<f32>> = dirs.iter().map(|d| d * radius).collect();
        let hull = ConvexHull::new(verts, vec![]);

        for dir in &dirs {
            let (_idx, pt) = hull.support(dir);
            let dist = pt.norm();
            assert!(
                (dist - radius).abs() < 1e-5,
                "Support point distance {} should equal radius {} for dir {:?}",
                dist, radius, dir
            );
        }
    }

    #[test]
    fn intent_box_support_returns_vertex() {
        // For a Box shape, the support in each axis direction should return half_extent.
        let half = Vector3::new(0.5, 0.3, 0.7);
        // A box has 8 vertices at all sign combinations of half extents
        let mut verts = Vec::new();
        for sx in &[-1.0_f32, 1.0] {
            for sy in &[-1.0_f32, 1.0] {
                for sz in &[-1.0_f32, 1.0] {
                    verts.push(Vector3::new(sx * half.x, sy * half.y, sz * half.z));
                }
            }
        }
        let hull = ConvexHull::new(verts, vec![]);

        // +X direction: support should have x == half.x
        let (_idx, pt) = hull.support(&Vector3::x());
        assert!((pt.x - half.x).abs() < 1e-6,
            "Box support +X: expected x={}, got x={}", half.x, pt.x);

        // +Y direction: support should have y == half.y
        let (_idx, pt) = hull.support(&Vector3::y());
        assert!((pt.y - half.y).abs() < 1e-6,
            "Box support +Y: expected y={}, got y={}", half.y, pt.y);

        // +Z direction: support should have z == half.z
        let (_idx, pt) = hull.support(&Vector3::z());
        assert!((pt.z - half.z).abs() < 1e-6,
            "Box support +Z: expected z={}, got z={}", half.z, pt.z);
    }

    #[test]
    fn intent_convex_hull_aabb_contains_all_vertices() {
        // After construction, the AABB must contain every vertex (with diverse coords).
        let verts = vec![
            Vector3::new(3.5, -1.2, 0.7),
            Vector3::new(-2.1, 4.8, -0.3),
            Vector3::new(0.0, 0.0, 5.5),
            Vector3::new(1.1, -3.3, -4.4),
            Vector3::new(-0.5, 2.2, 1.9),
        ];
        let hull = ConvexHull::new(verts.clone(), vec![]);
        let (aabb_min, aabb_max) = hull.aabb();

        for (vi, v) in verts.iter().enumerate() {
            assert!(v.x >= aabb_min.x - 1e-6 && v.x <= aabb_max.x + 1e-6,
                "vertex {} x={} outside AABB [{}, {}]", vi, v.x, aabb_min.x, aabb_max.x);
            assert!(v.y >= aabb_min.y - 1e-6 && v.y <= aabb_max.y + 1e-6,
                "vertex {} y={} outside AABB [{}, {}]", vi, v.y, aabb_min.y, aabb_max.y);
            assert!(v.z >= aabb_min.z - 1e-6 && v.z <= aabb_max.z + 1e-6,
                "vertex {} z={} outside AABB [{}, {}]", vi, v.z, aabb_min.z, aabb_max.z);
        }
    }

    // ── SLAM Cycle 10: Collider proptest and edge cases ──────────────

    use proptest::prelude::*;

    proptest! {
        #[test]
        fn prop_aabb_contains_all_vertices(
            x1 in -10.0f32..10.0, y1 in -10.0f32..10.0, z1 in -10.0f32..10.0,
            x2 in -10.0f32..10.0, y2 in -10.0f32..10.0, z2 in -10.0f32..10.0,
        ) {
            let verts = vec![
                Vector3::new(x1, y1, z1),
                Vector3::new(x2, y2, z2),
                Vector3::new(0.0, 0.0, 0.0),
            ];
            let hull = ConvexHull::new(verts.clone(), vec![]);
            let (amin, amax) = hull.aabb();
            for v in &verts {
                prop_assert!(v.x >= amin.x - 1e-6 && v.x <= amax.x + 1e-6);
                prop_assert!(v.y >= amin.y - 1e-6 && v.y <= amax.y + 1e-6);
                prop_assert!(v.z >= amin.z - 1e-6 && v.z <= amax.z + 1e-6);
            }
        }
    }

    #[test]
    fn edge_empty_convex_hull_support() {
        let hull = ConvexHull::new(vec![], vec![]);
        let (idx, pt) = hull.support(&Vector3::x());
        assert_eq!(idx, 0);
        assert!(pt.norm() < 1e-6, "empty hull support should return origin");
    }

    #[test]
    fn edge_single_vertex_hull() {
        let hull = ConvexHull::new(vec![Vector3::new(3.0, 4.0, 5.0)], vec![]);
        let (idx, pt) = hull.support(&Vector3::x());
        assert_eq!(idx, 0);
        assert!((pt - Vector3::new(3.0, 4.0, 5.0)).norm() < 1e-6);
    }

    #[test]
    fn property_decompose_preserves_vertex_count() {
        // ConvexHull decomposition should not lose vertices —
        // total vertices across all hulls >= original vertices.
        let mut verts = Vec::new();
        for i in 0..30 {
            let f = i as f32;
            verts.push(Vector3::new(
                f * 0.5,
                (f * 1.3).sin() * 3.0,
                (f * 0.7).cos() * 2.0,
            ));
        }
        let original_count = verts.len();

        let hulls = ConvexHull::decompose(verts, vec![], 8);
        let total_verts: usize = hulls.iter().map(|h| h.vertex_count()).sum();

        assert!(
            total_verts >= original_count,
            "Decomposition lost vertices: original={}, total across {} hulls={}",
            original_count, hulls.len(), total_verts
        );
    }
}