1#![deny(missing_docs)]
2
3use 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#[derive(Clone)]
23pub struct Convex<V: VectorSpace> {
24 pub rad: V::Scalar,
26 pub points: Box<[V]>,
28}
29
30impl<V: VectorSpace> Convex<V> {
31 pub const fn new(rad: V::Scalar, points: Box<[V]>) -> Self {
33 Self { rad, points }
34 }
35
36 pub fn capsule(rad: V::Scalar, start: V, end: V) -> Self {
38 Self {
39 rad,
40 points: [start, end].into(),
41 }
42 }
43
44 pub fn point(pos: V) -> Self {
46 Self {
47 rad: V::Scalar::zero(),
48 points: [pos].into(),
49 }
50 }
51
52 pub fn line(start: V, end: V) -> Self {
54 Self {
55 rad: V::Scalar::zero(),
56 points: [start, end].into(),
57 }
58 }
59
60 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}