symtropy_math/
convex_hull.rs

1// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
2// SPDX-License-Identifier: AGPL-3.0-or-later
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!(!vertices.is_empty(), "convex hull needs at least one vertex");
25
26        // Compute centroid
27        let mut center = SVector::zeros();
28        for v in &vertices {
29            center += v;
30        }
31        center /= vertices.len() as f64;
32
33        // Compute bounding radius
34        let radius = vertices
35            .iter()
36            .map(|v| (v - center).norm())
37            .fold(0.0f64, f64::max);
38
39        Self {
40            vertices,
41            center,
42            radius,
43        }
44    }
45
46    /// Number of vertices.
47    pub fn num_vertices(&self) -> usize {
48        self.vertices.len()
49    }
50
51    /// Create an axis-aligned hyperbox centered at origin with given half-extents.
52    pub fn hyperbox(half_extents: [f64; D]) -> Self {
53        // 2^D vertices
54        let n = 1 << D;
55        let mut vertices = Vec::with_capacity(n);
56        for mask in 0..n {
57            let mut v = SVector::zeros();
58            for axis in 0..D {
59                v[axis] = if (mask >> axis) & 1 == 0 {
60                    -half_extents[axis]
61                } else {
62                    half_extents[axis]
63                };
64            }
65            vertices.push(v);
66        }
67        Self::new(vertices)
68    }
69
70    /// Unit hypercube centered at origin: side length 2, half-extent 1 per axis.
71    pub fn unit_cube() -> Self {
72        Self::hyperbox([1.0; D])
73    }
74}
75
76impl<const D: usize> Shape<D> for ConvexHull<D> {
77    fn support(&self, direction: &SVector<f64, D>) -> SVector<f64, D> {
78        let mut best = self.vertices[0];
79        let mut best_dot = direction.dot(&best);
80        for v in &self.vertices[1..] {
81            let d = direction.dot(v);
82            if d > best_dot {
83                best_dot = d;
84                best = *v;
85            }
86        }
87        best
88    }
89
90    fn bounding_sphere(&self) -> (Point<D>, f64) {
91        (Point(self.center), self.radius)
92    }
93
94    fn as_any(&self) -> &dyn std::any::Any { self }
95}
96
97#[cfg(test)]
98mod tests {
99    use super::*;
100
101    #[test]
102    fn unit_cube_2d() {
103        let cube = ConvexHull::<2>::unit_cube();
104        assert_eq!(cube.num_vertices(), 4); // 2^2 = 4 vertices
105    }
106
107    #[test]
108    fn unit_cube_3d() {
109        let cube = ConvexHull::<3>::unit_cube();
110        assert_eq!(cube.num_vertices(), 8); // 2^3 = 8
111    }
112
113    #[test]
114    fn unit_cube_4d() {
115        let cube = ConvexHull::<4>::unit_cube();
116        assert_eq!(cube.num_vertices(), 16); // 2^4 = 16 (tesseract)
117    }
118
119    #[test]
120    fn support_returns_correct_vertex() {
121        let cube = ConvexHull::<3>::unit_cube();
122        let dir = SVector::from([1.0, 1.0, 1.0]);
123        let sp = cube.support(&dir);
124        // Should be the (1,1,1) vertex
125        assert!((sp[0] - 1.0).abs() < 1e-12);
126        assert!((sp[1] - 1.0).abs() < 1e-12);
127        assert!((sp[2] - 1.0).abs() < 1e-12);
128    }
129
130    #[test]
131    fn support_negative_direction() {
132        let cube = ConvexHull::<3>::unit_cube();
133        let dir = SVector::from([-1.0, 0.0, 0.0]);
134        let sp = cube.support(&dir);
135        assert!((sp[0] + 1.0).abs() < 1e-12); // -1 in x
136    }
137
138    #[test]
139    fn bounding_sphere_contains_all_vertices() {
140        let cube = ConvexHull::<4>::unit_cube();
141        let (center, radius) = cube.bounding_sphere();
142        for v in &cube.vertices {
143            let dist = Point(*v).distance(&center);
144            assert!(dist <= radius + 1e-10);
145        }
146    }
147
148    #[test]
149    fn custom_hyperbox() {
150        let box3 = ConvexHull::<3>::hyperbox([2.0, 3.0, 4.0]);
151        let dir = SVector::from([1.0, 0.0, 0.0]);
152        let sp = box3.support(&dir);
153        assert!((sp[0] - 2.0).abs() < 1e-12);
154    }
155
156    #[test]
157    fn triangle_support() {
158        let tri = ConvexHull::<2>::new(vec![
159            SVector::from([0.0, 0.0]),
160            SVector::from([1.0, 0.0]),
161            SVector::from([0.5, 1.0]),
162        ]);
163        let dir = SVector::from([0.0, 1.0]);
164        let sp = tri.support(&dir);
165        assert!((sp[1] - 1.0).abs() < 1e-12);
166    }
167}
symtropy_math/convex_hull.rs

symtropy_math/
convex_hull.rs
