loam_physics/
narrowphase.rs

1use std::collections::HashMap;
2
3use crate::body::RigidBody;
4use crate::collider::ColliderKind;
5use crate::geometry::GeometryStore;
6use crate::integrator::PhysicsSpace;
7use crate::response::Contact;
8
9/// Always called with `a.kind()` matching the key's first component.
10pub type NarrowphaseFn<S> = fn(
11    a: &RigidBody<S>,
12    b: &RigidBody<S>,
13    geometry: &GeometryStore,
14    space: &S,
15) -> Option<Contact<S>>;
16
17fn kind_rank(kind: ColliderKind) -> u8 {
18    match kind {
19        ColliderKind::Sphere => 0,
20        ColliderKind::HalfSpace => 1,
21        ColliderKind::HalfSpace4D => 2,
22        ColliderKind::Box3 => 3,
23        ColliderKind::Polygon2D => 4,
24        ColliderKind::ConvexPolytope3D => 5,
25        ColliderKind::ConvexPolytope4D => 6,
26        ColliderKind::HyperSphere4D => 7,
27    }
28}
29
30pub struct Narrowphase<S: PhysicsSpace> {
31    dispatch: HashMap<(ColliderKind, ColliderKind), NarrowphaseFn<S>>,
32    order: Vec<(ColliderKind, ColliderKind)>,
33}
34
35impl<S: PhysicsSpace> Default for Narrowphase<S> {
36    fn default() -> Self {
37        Self {
38            dispatch: HashMap::new(),
39            order: Vec::new(),
40        }
41    }
42}
43
44impl<S: PhysicsSpace> Narrowphase<S> {
45    pub fn new() -> Self {
46        Self::default()
47    }
48
49    pub fn register(&mut self, a: ColliderKind, b: ColliderKind, f: NarrowphaseFn<S>) {
50        if self.dispatch.insert((a, b), f).is_some() {
51            return;
52        }
53        let key = (kind_rank(a), kind_rank(b));
54        let at = self
55            .order
56            .partition_point(|&(x, y)| (kind_rank(x), kind_rank(y)) < key);
57        self.order.insert(at, (a, b));
58    }
59
60    pub fn registrations(&self) -> &[(ColliderKind, ColliderKind)] {
61        &self.order
62    }
63
64    pub fn test(
65        &self,
66        a: &RigidBody<S>,
67        b: &RigidBody<S>,
68        geometry: &GeometryStore,
69        space: &S,
70    ) -> Option<Contact<S>>
71    where
72        S::Vector: std::ops::Mul<f32, Output = S::Vector>,
73    {
74        let key = (a.collider().kind(), b.collider().kind());
75        if let Some(&f) = self.dispatch.get(&key) {
76            return f(a, b, geometry, space);
77        }
78        let reversed = (b.collider().kind(), a.collider().kind());
79        if let Some(&f) = self.dispatch.get(&reversed) {
80            return f(b, a, geometry, space).map(|c| Contact {
81                normal: c.normal * -1.0,
82                point: c.point,
83                penetration: c.penetration,
84                restitution: c.restitution,
85            });
86        }
87        None
88    }
89}
loam_physics/narrowphase.rs

loam_physics/
narrowphase.rs
