Skip to main content

collide_convex/
lib.rs

1#![deny(missing_docs)]
2
3//! This crate contains a convex collider, which implements the collide trait from the `collide` crate.
4
5use collide::{Collider, CollisionInfo, Transform, Transformable};
6use inner_space::{InnerSpace, VectorSpace};
7use scalars::{One, Real, Sqrt, Zero};
8
9fn epsilon<V: VectorSpace>() -> V::Scalar
10where
11    V::Scalar: Real,
12{
13    let two = V::Scalar::one() + V::Scalar::one();
14    let thousand = {
15        let t = two * two * two * two * two;
16        t * t
17    };
18    V::Scalar::one() / (thousand * thousand)
19}
20
21/// The convex collider defined as a convex hull around multiple spheres having the same radius.
22#[derive(Clone)]
23pub struct Convex<V: VectorSpace> {
24    /// The radius of the spheres.
25    pub rad: V::Scalar,
26    /// The positions of the spheres.
27    pub points: Box<[V]>,
28}
29
30impl<V: VectorSpace> Convex<V> {
31    /// Creates a new convex collider.
32    pub const fn new(rad: V::Scalar, points: Box<[V]>) -> Self {
33        Self { rad, points }
34    }
35
36    /// Creates a new convex collider representing a capsule.
37    pub fn capsule(rad: V::Scalar, start: V, end: V) -> Self {
38        Self {
39            rad,
40            points: [start, end].into(),
41        }
42    }
43
44    /// Creates a new convex collider representing a point.
45    pub fn point(pos: V) -> Self {
46        Self {
47            rad: V::Scalar::zero(),
48            points: [pos].into(),
49        }
50    }
51
52    /// Creates a new convex collider representing a line.
53    pub fn line(start: V, end: V) -> Self {
54        Self {
55            rad: V::Scalar::zero(),
56            points: [start, end].into(),
57        }
58    }
59
60    /// Creates a new convex collider representing a sphere.
61    pub fn sphere(pos: V, rad: V::Scalar) -> Self {
62        Self {
63            rad,
64            points: [pos].into(),
65        }
66    }
67}
68
69pub(crate) fn origin_offset<V: Copy + InnerSpace>(vertices: &[V]) -> V {
70    let Some(&first) = vertices.first() else {
71        return V::zero();
72    };
73
74    let mut ortho: Vec<V> = Vec::new();
75    for &vertex in &vertices[1..] {
76        let mut edge = vertex - first;
77        for basis in &ortho {
78            let mag2 = basis.magnitude2();
79            if !mag2.is_zero() {
80                edge = edge - *basis * (edge.dot(basis) / mag2);
81            }
82        }
83        if !edge.is_zero() {
84            ortho.push(edge);
85        }
86    }
87
88    let mut result = first;
89    for basis in &ortho {
90        let mag2 = basis.magnitude2();
91        if !mag2.is_zero() {
92            result = result - *basis * (result.dot(basis) / mag2);
93        }
94    }
95    result
96}
97
98fn is_projection_inside<V: Copy + InnerSpace>(offset: V, simplex: &[V]) -> bool {
99    if simplex.len() <= 1 {
100        return true;
101    }
102    let n = simplex.len();
103    for i in 0..n {
104        let face: Vec<V> = (0..n).filter(|&j| j != i).map(|j| simplex[j]).collect();
105        let face_offset = origin_offset(&face);
106        let toward_vertex = simplex[i] - face_offset;
107        let toward_projection = offset - face_offset;
108        if toward_vertex.dot(&toward_projection) < V::Scalar::zero() {
109            return false;
110        }
111    }
112    true
113}
114
115pub(crate) fn closest_subsimplex<V: Copy + InnerSpace>(points: &[V]) -> (Vec<V>, V) {
116    let n = points.len();
117    let last_bit = 1u32 << (n - 1);
118    let mut best: Option<(u32, V, V::Scalar)> = None;
119
120    for mask in 1u32..(1u32 << n) {
121        if mask & last_bit == 0 {
122            continue;
123        }
124
125        let subset: Vec<V> = (0..n)
126            .filter(|&i| mask & (1 << i) != 0)
127            .map(|i| points[i])
128            .collect();
129
130        let offset = origin_offset(&subset);
131
132        if !is_projection_inside(offset, &subset) {
133            continue;
134        }
135
136        let mag2 = offset.magnitude2();
137        if best.as_ref().is_some_and(|(_, _, m)| mag2 >= *m) {
138            continue;
139        }
140
141        best = Some((mask, offset, mag2));
142    }
143
144    let (mask, offset, _) =
145        best.unwrap_or_else(|| (last_bit, points[n - 1], points[n - 1].magnitude2()));
146    let subset = (0..n)
147        .filter(|&i| mask & (1 << i) != 0)
148        .map(|i| points[i])
149        .collect();
150    (subset, offset)
151}
152
153fn epa<V: Copy + InnerSpace>(initial_simplex: &[V], support: impl Fn(V) -> V) -> V
154where
155    V::Scalar: Real,
156{
157    if initial_simplex.len() <= 2 {
158        return epa_closest_initial_face(initial_simplex);
159    }
160
161    let mut vertices: Vec<V> = initial_simplex.to_vec();
162    let vertex_count = vertices.len();
163
164    let mut faces: Vec<Vec<usize>> = (0..vertex_count)
165        .map(|skip| (0..vertex_count).filter(|&i| i != skip).collect())
166        .collect();
167
168    let mut result = epa_closest_initial_face(initial_simplex);
169
170    for _ in 0..64 {
171        let Some((_, offset, distance)) = epa_find_closest_face(&vertices, &faces) else {
172            break;
173        };
174
175        if distance.is_zero() {
176            continue;
177        }
178
179        result = offset;
180
181        let new_point = support(offset);
182
183        let new_projection = new_point.dot(&offset);
184        let tolerance = epsilon::<V>().sqrt();
185        if new_projection - distance <= tolerance * distance {
186            break;
187        }
188
189        let eps = epsilon::<V>();
190        if vertices
191            .iter()
192            .any(|&vertex| (vertex - new_point).magnitude2() <= eps)
193        {
194            break;
195        }
196
197        let new_index = vertices.len();
198        vertices.push(new_point);
199
200        let visible: Vec<bool> = faces
201            .iter()
202            .map(|face| {
203                let points: Vec<V> = face.iter().map(|&i| vertices[i]).collect();
204                let face_offset = origin_offset(&points);
205                if face_offset.magnitude2().is_zero() {
206                    return false;
207                }
208                (new_point - vertices[face[0]]).dot(&face_offset) > V::Scalar::zero()
209            })
210            .collect();
211
212        let mut ridges: Vec<Vec<usize>> = Vec::new();
213        for (face_index, face) in faces.iter().enumerate() {
214            if !visible[face_index] {
215                continue;
216            }
217            for skip in 0..face.len() {
218                let mut ridge: Vec<usize> = face
219                    .iter()
220                    .enumerate()
221                    .filter(|&(j, _)| j != skip)
222                    .map(|(_, &vertex)| vertex)
223                    .collect();
224                ridge.sort_unstable();
225                ridges.push(ridge);
226            }
227        }
228
229        let horizon: Vec<Vec<usize>> = ridges
230            .iter()
231            .filter(|ridge| ridges.iter().filter(|other| other == ridge).count() == 1)
232            .cloned()
233            .collect();
234
235        faces = faces
236            .into_iter()
237            .enumerate()
238            .filter(|(index, _)| !visible[*index])
239            .map(|(_, face)| face)
240            .collect();
241
242        for ridge in horizon {
243            let mut face = ridge;
244            face.push(new_index);
245            faces.push(face);
246        }
247
248        if faces.is_empty() {
249            break;
250        }
251    }
252
253    result
254}
255
256fn epa_find_closest_face<V: Copy + InnerSpace>(
257    vertices: &[V],
258    faces: &[Vec<usize>],
259) -> Option<(usize, V, V::Scalar)> {
260    let mut best: Option<(usize, V, V::Scalar)> = None;
261
262    for (index, face) in faces.iter().enumerate() {
263        let points: Vec<V> = face.iter().map(|&i| vertices[i]).collect();
264        let offset = origin_offset(&points);
265
266        if !is_projection_inside(offset, &points) {
267            continue;
268        }
269
270        let distance = offset.magnitude2();
271        if distance.is_zero() {
272            continue;
273        }
274        if best.as_ref().is_some_and(|(_, _, d)| distance >= *d) {
275            continue;
276        }
277
278        best = Some((index, offset, distance));
279    }
280
281    best
282}
283
284fn epa_closest_initial_face<V: Copy + InnerSpace>(simplex: &[V]) -> V {
285    let mut best: Option<(V::Scalar, V)> = None;
286    for skip in 0..simplex.len() {
287        let face: Vec<V> = (0..simplex.len())
288            .filter(|&j| j != skip)
289            .map(|j| simplex[j])
290            .collect();
291        if face.is_empty() {
292            continue;
293        }
294        let offset = origin_offset(&face);
295        let magnitude = offset.magnitude2();
296        if magnitude.is_zero() {
297            continue;
298        }
299        if best.as_ref().is_some_and(|(m, _)| magnitude >= *m) {
300            continue;
301        }
302        best = Some((magnitude, offset));
303    }
304    best.map_or_else(V::zero, |(_, offset)| offset)
305}
306
307enum Side {
308    Inside,
309    Outside,
310}
311
312impl<V: Copy + InnerSpace> Convex<V>
313where
314    V::Scalar: Real,
315{
316    fn support(&self, other: &Self, dir: V) -> V {
317        let mut found: Option<V> = None;
318        for &point in &self.points {
319            for &other_point in &other.points {
320                let diff = other_point - point;
321                if found.is_some_and(|found| diff.dot(&dir) <= found.dot(&dir)) {
322                    continue;
323                }
324                found = Some(diff);
325            }
326        }
327
328        found.unwrap_or_else(V::zero)
329    }
330
331    fn offset(&self, other: &Self) -> (V, Side) {
332        let isd = other.points[0] - self.points[0];
333        if isd.is_zero() {
334            return (V::zero(), Side::Outside);
335        }
336        let initial = self.support(other, isd);
337        let mut simplex = vec![initial];
338        let mut dir = -initial;
339
340        for _ in 0..64 {
341            let new_point = self.support(other, dir);
342
343            if simplex.contains(&new_point) {
344                break;
345            }
346
347            simplex.push(new_point);
348            let (sub, offset) = closest_subsimplex(&simplex);
349
350            let inside = if offset.is_zero() {
351                true
352            } else if sub.len() == simplex.len() {
353                let scale = simplex
354                    .iter()
355                    .copied()
356                    .map(|v| v.magnitude2())
357                    .fold(V::Scalar::zero(), |a, b| if a > b { a } else { b });
358                let eps = epsilon::<V>();
359                offset.magnitude2() <= scale * eps * eps
360            } else {
361                false
362            };
363            if inside {
364                let mut expanded = sub;
365                'expand: for &self_point in &self.points {
366                    for &other_point in &other.points {
367                        let diff = other_point - self_point;
368                        if expanded.contains(&diff) {
369                            continue;
370                        }
371                        expanded.push(diff);
372                        let (new_sub, new_offset) = closest_subsimplex(&expanded);
373                        if new_offset.is_zero() {
374                            expanded = new_sub;
375                            if expanded.len() > 2 {
376                                break 'expand;
377                            }
378                        } else {
379                            expanded.pop();
380                        }
381                    }
382                }
383                let result = epa(&expanded, |direction| self.support(other, direction));
384                return (result, Side::Inside);
385            }
386
387            simplex = sub;
388            dir = -offset;
389        }
390
391        (origin_offset(&simplex), Side::Outside)
392    }
393}
394
395impl<V: Copy + InnerSpace, T: Transform<V>> Transformable<T> for Convex<V> {
396    fn transformed(&self, transform: &T) -> Self {
397        Self {
398            rad: self.rad,
399            points: self
400                .points
401                .iter()
402                .map(|&p| transform.apply_point(p))
403                .collect(),
404        }
405    }
406}
407
408#[cfg(feature = "sphere")]
409mod bounding_sphere;
410
411#[cfg(feature = "ray")]
412mod ray;
413
414impl<V: Copy + InnerSpace> Collider for Convex<V>
415where
416    V::Scalar: Real,
417{
418    type Vector = V;
419
420    fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Self::Vector>> {
421        let (offset, side) = self.offset(other);
422        let mag2 = offset.magnitude2();
423        let rad = self.rad + other.rad;
424        match side {
425            Side::Inside => {
426                if mag2.is_zero() {
427                    return Some(CollisionInfo {
428                        self_contact: V::zero(),
429                        other_contact: V::zero(),
430                        vector: V::zero(),
431                    });
432                }
433                let mag = mag2.sqrt();
434                let dir = offset / mag;
435                Some(CollisionInfo {
436                    self_contact: V::zero(),
437                    other_contact: V::zero(),
438                    vector: dir * (mag + rad),
439                })
440            }
441            Side::Outside => {
442                if mag2 > rad * rad {
443                    return None;
444                }
445                if mag2.is_zero() {
446                    return Some(CollisionInfo {
447                        self_contact: V::zero(),
448                        other_contact: V::zero(),
449                        vector: V::zero(),
450                    });
451                }
452                let mag = mag2.sqrt();
453                let dir = offset / mag;
454                Some(CollisionInfo {
455                    self_contact: V::zero(),
456                    other_contact: V::zero(),
457                    vector: dir * (mag - rad),
458                })
459            }
460        }
461    }
462}