symtropy_math/
convex_hull.rs

1// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
2// SPDX-License-Identifier: Apache-2.0 OR MIT
3// Commercial licensing: see COMMERCIAL_LICENSE.md at repository root
4use crate::point::Point;
5use crate::shape::Shape;
6use nalgebra::SVector;
7
8/// D-dimensional convex hull defined by a set of vertices.
9///
10/// The support function iterates over all vertices, which is O(n) per query.
11/// For large vertex counts, use a spatial acceleration structure. For physics
12/// colliders with <100 vertices, linear scan is faster due to cache locality.
13#[derive(Clone, Debug)]
14pub struct ConvexHull<const D: usize> {
15    pub vertices: Vec<SVector<f64, D>>,
16    /// Cached bounding sphere for broadphase.
17    center: SVector<f64, D>,
18    radius: f64,
19}
20
21impl<const D: usize> ConvexHull<D> {
22    /// Create from a set of vertices.
23    pub fn new(vertices: Vec<SVector<f64, D>>) -> Self {
24        assert!(
25            !vertices.is_empty(),
26            "convex hull needs at least one vertex"
27        );
28
29        // Compute centroid
30        let mut center = SVector::zeros();
31        for v in &vertices {
32            center += v;
33        }
34        center /= vertices.len() as f64;
35
36        // Compute bounding radius
37        let radius = vertices
38            .iter()
39            .map(|v| (v - center).norm())
40            .fold(0.0f64, f64::max);
41
42        Self {
43            vertices,
44            center,
45            radius,
46        }
47    }
48
49    /// Number of vertices.
50    pub fn num_vertices(&self) -> usize {
51        self.vertices.len()
52    }
53
54    /// Create an axis-aligned hyperbox centered at origin with given half-extents.
55    pub fn hyperbox(half_extents: [f64; D]) -> Self {
56        // 2^D vertices
57        let n = 1 << D;
58        let mut vertices = Vec::with_capacity(n);
59        for mask in 0..n {
60            let mut v = SVector::zeros();
61            for axis in 0..D {
62                v[axis] = if (mask >> axis) & 1 == 0 {
63                    -half_extents[axis]
64                } else {
65                    half_extents[axis]
66                };
67            }
68            vertices.push(v);
69        }
70        Self::new(vertices)
71    }
72
73    /// Unit hypercube centered at origin: side length 2, half-extent 1 per axis.
74    pub fn unit_cube() -> Self {
75        Self::hyperbox([1.0; D])
76    }
77}
78
79impl<const D: usize> Shape<D> for ConvexHull<D> {
80    fn support(&self, direction: &SVector<f64, D>) -> SVector<f64, D> {
81        let mut best = self.vertices[0];
82        let mut best_dot = direction.dot(&best);
83        for v in &self.vertices[1..] {
84            let d = direction.dot(v);
85            if d > best_dot {
86                best_dot = d;
87                best = *v;
88            }
89        }
90        best
91    }
92
93    fn bounding_sphere(&self) -> (Point<D>, f64) {
94        (Point(self.center), self.radius)
95    }
96
97    fn as_any(&self) -> &dyn std::any::Any {
98        self
99    }
100
101    fn clone_box(&self) -> Box<dyn Shape<D>> {
102        Box::new(self.clone())
103    }
104}
105
106#[cfg(test)]
107mod tests {
108    use super::*;
109
110    #[test]
111    fn unit_cube_2d() {
112        let cube = ConvexHull::<2>::unit_cube();
113        assert_eq!(cube.num_vertices(), 4); // 2^2 = 4 vertices
114    }
115
116    #[test]
117    fn unit_cube_3d() {
118        let cube = ConvexHull::<3>::unit_cube();
119        assert_eq!(cube.num_vertices(), 8); // 2^3 = 8
120    }
121
122    #[test]
123    fn unit_cube_4d() {
124        let cube = ConvexHull::<4>::unit_cube();
125        assert_eq!(cube.num_vertices(), 16); // 2^4 = 16 (tesseract)
126    }
127
128    #[test]
129    fn support_returns_correct_vertex() {
130        let cube = ConvexHull::<3>::unit_cube();
131        let dir = SVector::from([1.0, 1.0, 1.0]);
132        let sp = cube.support(&dir);
133        // Should be the (1,1,1) vertex
134        assert!((sp[0] - 1.0).abs() < 1e-12);
135        assert!((sp[1] - 1.0).abs() < 1e-12);
136        assert!((sp[2] - 1.0).abs() < 1e-12);
137    }
138
139    #[test]
140    fn support_negative_direction() {
141        let cube = ConvexHull::<3>::unit_cube();
142        let dir = SVector::from([-1.0, 0.0, 0.0]);
143        let sp = cube.support(&dir);
144        assert!((sp[0] + 1.0).abs() < 1e-12); // -1 in x
145    }
146
147    #[test]
148    fn bounding_sphere_contains_all_vertices() {
149        let cube = ConvexHull::<4>::unit_cube();
150        let (center, radius) = cube.bounding_sphere();
151        for v in &cube.vertices {
152            let dist = Point(*v).distance(&center);
153            assert!(dist <= radius + 1e-10);
154        }
155    }
156
157    #[test]
158    fn custom_hyperbox() {
159        let box3 = ConvexHull::<3>::hyperbox([2.0, 3.0, 4.0]);
160        let dir = SVector::from([1.0, 0.0, 0.0]);
161        let sp = box3.support(&dir);
162        assert!((sp[0] - 2.0).abs() < 1e-12);
163    }
164
165    #[test]
166    fn triangle_support() {
167        let tri = ConvexHull::<2>::new(vec![
168            SVector::from([0.0, 0.0]),
169            SVector::from([1.0, 0.0]),
170            SVector::from([0.5, 1.0]),
171        ]);
172        let dir = SVector::from([0.0, 1.0]);
173        let sp = tri.support(&dir);
174        assert!((sp[1] - 1.0).abs() < 1e-12);
175    }
176}
symtropy_math/convex_hull.rs

symtropy_math/
convex_hull.rs
