symtropy_physics/
broadphase.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
4//! Broadphase collision detection via LBVH (Linear Bounding Volume Hierarchy).
5//!
6//! Uses Morton codes (Z-order curves) for spatial sorting, then builds a BVH
7//! from the sorted order. The Morton encoding is the only D-dependent code;
8//! the hierarchy is dimension-agnostic. Falls back to brute-force for <50 bodies.
9//!
10//! # Determinism
11//! Morton codes use integer quantization, avoiding floating-point heuristics.
12//! Pair list sorted by (BodyHandle, BodyHandle) for identical resolution order.
13
14use nalgebra::SVector;
15use crate::body::{BodyHandle, BodyType, RigidBody};
16
17/// Pair of body handles that may be colliding.
18#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
19pub struct BroadphasePair(pub BodyHandle, pub BodyHandle);
20
21/// Axis-aligned bounding box in D dimensions.
22#[derive(Clone, Debug)]
23pub struct Aabb<const D: usize> {
24    pub min: SVector<f64, D>,
25    pub max: SVector<f64, D>,
26}
27
28impl<const D: usize> Aabb<D> {
29    pub fn empty() -> Self {
30        Self { min: SVector::from_element(f64::MAX), max: SVector::from_element(f64::MIN) }
31    }
32    pub fn union(&self, other: &Self) -> Self {
33        let mut min = self.min; let mut max = self.max;
34        for i in 0..D {
35            if other.min[i] < min[i] { min[i] = other.min[i]; }
36            if other.max[i] > max[i] { max[i] = other.max[i]; }
37        }
38        Self { min, max }
39    }
40    pub fn overlaps(&self, other: &Self) -> bool {
41        for i in 0..D {
42            if self.max[i] < other.min[i] || self.min[i] > other.max[i] { return false; }
43        }
44        true
45    }
46    pub fn from_sphere(center: &SVector<f64, D>, radius: f64) -> Self {
47        Self { min: center - SVector::from_element(radius), max: center + SVector::from_element(radius) }
48    }
49}
50
51// Morton codes
52fn quantize(value: f64, world_min: f64, world_max: f64, bits: usize) -> u32 {
53    let range = world_max - world_min;
54    if range <= 0.0 { return 0; }
55    let max_val = ((1u64 << bits) - 1) as f64;
56    let normalized = ((value - world_min) / range).clamp(0.0, 1.0);
57    (normalized * max_val) as u32
58}
59
60/// Encode a D-dimensional position into a 64-bit Morton code.
61pub fn morton_encode<const D: usize>(
62    position: &SVector<f64, D>, world_min: &SVector<f64, D>, world_max: &SVector<f64, D>,
63) -> u64 {
64    let bits = 64 / D;
65    let mut code: u64 = 0;
66    for bit in 0..bits {
67        for axis in 0..D {
68            let q = quantize(position[axis], world_min[axis], world_max[axis], bits);
69            code |= (((q >> bit) & 1) as u64) << (bit * D + axis);
70        }
71    }
72    code
73}
74
75/// Extract top prefix_bits from a Morton code (for spatial authority zones).
76pub fn morton_prefix(code: u64, prefix_bits: usize) -> u64 {
77    if prefix_bits >= 64 { code } else { code >> (64 - prefix_bits) }
78}
79
80// LBVH
81const BRUTE_FORCE_THRESHOLD: usize = 50;
82const LEAF_BIT: u32 = 0x8000_0000;
83fn is_leaf(idx: u32) -> bool { idx & LEAF_BIT != 0 }
84fn leaf_index(idx: u32) -> usize { (idx & !LEAF_BIT) as usize }
85
86#[derive(Clone, Debug)]
87struct BvhNode<const D: usize> { aabb: Aabb<D>, left: u32, right: u32 }
88
89/// Linear Bounding Volume Hierarchy.
90pub struct Lbvh<const D: usize> {
91    sorted_indices: Vec<usize>,
92    leaf_aabbs: Vec<Aabb<D>>,
93    nodes: Vec<BvhNode<D>>,
94}
95
96impl<const D: usize> Lbvh<D> {
97    pub fn build(bodies: &[RigidBody<D>]) -> Self {
98        let n = bodies.len();
99        let mut aabbs = Vec::with_capacity(n);
100        let mut world_min = SVector::<f64, D>::from_element(f64::MAX);
101        let mut world_max = SVector::<f64, D>::from_element(f64::MIN);
102        for body in bodies {
103            let (c_local, r) = body.collider.bounding_sphere();
104            let c_world = body.transform.transform_point(&c_local).0;
105            let aabb = Aabb::from_sphere(&c_world, r);
106            for i in 0..D {
107                if aabb.min[i] < world_min[i] { world_min[i] = aabb.min[i]; }
108                if aabb.max[i] > world_max[i] { world_max[i] = aabb.max[i]; }
109            }
110            aabbs.push(aabb);
111        }
112        for i in 0..D {
113            if world_max[i] - world_min[i] < 1e-6 { world_min[i] -= 1.0; world_max[i] += 1.0; }
114        }
115        let mut morton_codes: Vec<u64> = aabbs.iter()
116            .map(|a| { let c = (a.min + a.max) * 0.5; morton_encode::<D>(&c, &world_min, &world_max) })
117            .collect();
118        let mut sorted_indices: Vec<usize> = (0..n).collect();
119        sorted_indices.sort_by_key(|&i| morton_codes[i]);
120        let sorted_aabbs: Vec<Aabb<D>> = sorted_indices.iter().map(|&i| aabbs[i].clone()).collect();
121        let nodes = if n <= 1 { Vec::new() } else { Self::build_nodes(&sorted_indices.iter().map(|&i| morton_codes[i]).collect::<Vec<_>>(), &sorted_aabbs) };
122        Self { sorted_indices, leaf_aabbs: sorted_aabbs, nodes }
123    }
124
125    fn find_split(codes: &[u64], first: usize, last: usize) -> usize {
126        if codes[first] == codes[last] { return (first + last) / 2; }
127        let cp = (codes[first] ^ codes[last]).leading_zeros();
128        let mut split = first; let mut step = last - first;
129        loop {
130            step = (step + 1) / 2;
131            let ns = split + step;
132            if ns < last && (codes[first] ^ codes[ns]).leading_zeros() > cp { split = ns; }
133            if step <= 1 { break; }
134        }
135        split
136    }
137
138    fn build_nodes(codes: &[u64], aabbs: &[Aabb<D>]) -> Vec<BvhNode<D>> {
139        let mut nodes = Vec::with_capacity(codes.len() - 1);
140        Self::build_rec(codes, aabbs, 0, codes.len() - 1, &mut nodes);
141        nodes
142    }
143
144    fn build_rec(codes: &[u64], aabbs: &[Aabb<D>], first: usize, last: usize, nodes: &mut Vec<BvhNode<D>>) -> u32 {
145        if first == last { return LEAF_BIT | (first as u32); }
146        let split = Self::find_split(codes, first, last);
147        let left = Self::build_rec(codes, aabbs, first, split, nodes);
148        let right = Self::build_rec(codes, aabbs, split + 1, last, nodes);
149        let la = if is_leaf(left) { aabbs[leaf_index(left)].clone() } else { nodes[left as usize].aabb.clone() };
150        let ra = if is_leaf(right) { aabbs[leaf_index(right)].clone() } else { nodes[right as usize].aabb.clone() };
151        let idx = nodes.len() as u32;
152        nodes.push(BvhNode { aabb: la.union(&ra), left, right });
153        idx
154    }
155
156    pub fn query_pairs(&self, bodies: &[RigidBody<D>]) -> Vec<BroadphasePair> {
157        let n = self.sorted_indices.len();
158        if n <= 1 { return Vec::new(); }
159        let mut pairs = Vec::new();
160        let root = (self.nodes.len() - 1) as u32;
161        for li in 0..n {
162            let la = &self.leaf_aabbs[li];
163            let lt = bodies[self.sorted_indices[li]].body_type;
164            let mut stack = arrayvec::ArrayVec::<u32, 64>::new();
165            stack.push(root);
166            while let Some(ni) = stack.pop() {
167                if is_leaf(ni) {
168                    let oi = leaf_index(ni);
169                    if oi > li {
170                        let ot = bodies[self.sorted_indices[oi]].body_type;
171                        if lt == BodyType::Static && ot == BodyType::Static { continue; }
172                        if !should_collide(&bodies[self.sorted_indices[li]], &bodies[self.sorted_indices[oi]]) { continue; }
173                        if la.overlaps(&self.leaf_aabbs[oi]) {
174                            let (ha, hb) = (bodies[self.sorted_indices[li]].handle, bodies[self.sorted_indices[oi]].handle);
175                            if ha < hb { pairs.push(BroadphasePair(ha, hb)); }
176                            else { pairs.push(BroadphasePair(hb, ha)); }
177                        }
178                    }
179                    continue;
180                }
181                let node = &self.nodes[ni as usize];
182                if la.overlaps(&node.aabb) { stack.push(node.left); stack.push(node.right); }
183            }
184        }
185        pairs.sort_unstable_by_key(|p| (p.0, p.1));
186        pairs.dedup();
187        pairs
188    }
189}
190
191/// Check if two bodies should collide based on collision group/mask filters.
192/// Both bodies must accept each other: `(a.group & b.mask != 0) && (b.group & a.mask != 0)`.
193#[inline]
194fn should_collide<const D: usize>(a: &RigidBody<D>, b: &RigidBody<D>) -> bool {
195    (a.collision_group & b.collision_mask != 0) && (b.collision_group & a.collision_mask != 0)
196}
197
198/// Find all potentially colliding pairs. Uses LBVH for ≥50 bodies, brute-force otherwise.
199pub fn find_pairs<const D: usize>(bodies: &[RigidBody<D>]) -> Vec<BroadphasePair> {
200    if bodies.len() < BRUTE_FORCE_THRESHOLD { find_pairs_brute(bodies) }
201    else { Lbvh::build(bodies).query_pairs(bodies) }
202}
203
204fn find_pairs_brute<const D: usize>(bodies: &[RigidBody<D>]) -> Vec<BroadphasePair> {
205    let mut pairs = Vec::new();
206    for i in 0..bodies.len() {
207        let ti = bodies[i].body_type;
208        for j in (i+1)..bodies.len() {
209            if ti == BodyType::Static && bodies[j].body_type == BodyType::Static { continue; }
210            if !should_collide(&bodies[i], &bodies[j]) { continue; }
211            let (ci, ri) = bodies[i].collider.bounding_sphere();
212            let (cj, rj) = bodies[j].collider.bounding_sphere();
213            let wi = bodies[i].transform.transform_point(&ci);
214            let wj = bodies[j].transform.transform_point(&cj);
215            if wi.distance(&wj) <= ri + rj {
216                pairs.push(BroadphasePair(bodies[i].handle, bodies[j].handle));
217            }
218        }
219    }
220    pairs
221}
222
223#[cfg(test)]
224mod tests {
225    use super::*;
226    use crate::body::RigidBody;
227    use symtropy_math::{Point, Sphere};
228
229    #[test]
230    fn overlapping_pair_detected() {
231        let bodies = vec![
232            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
233            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
234        ];
235        assert_eq!(find_pairs(&bodies).len(), 1);
236    }
237
238    #[test]
239    fn separated_pair_not_detected() {
240        let bodies = vec![
241            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
242            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([5.0, 0.0, 0.0]), 1.0, 1.0),
243        ];
244        assert!(find_pairs(&bodies).is_empty());
245    }
246
247    #[test]
248    fn static_static_ignored() {
249        let bodies = vec![
250            RigidBody::<3>::static_body(BodyHandle(0), Point::origin(), Box::new(Sphere::unit())),
251            RigidBody::<3>::static_body(BodyHandle(1), Point::new([0.5, 0.0, 0.0]), Box::new(Sphere::unit())),
252        ];
253        assert!(find_pairs(&bodies).is_empty());
254    }
255
256    #[test]
257    fn static_dynamic_detected() {
258        let bodies = vec![
259            RigidBody::<3>::static_body(BodyHandle(0), Point::origin(), Box::new(Sphere::unit())),
260            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
261        ];
262        assert_eq!(find_pairs(&bodies).len(), 1);
263    }
264
265    #[test]
266    fn multiple_pairs() {
267        let bodies = vec![
268            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
269            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
270            RigidBody::<3>::dynamic_sphere(BodyHandle(2), Point::new([10.0, 0.0, 0.0]), 1.0, 1.0),
271            RigidBody::<3>::dynamic_sphere(BodyHandle(3), Point::new([1.0, 1.0, 0.0]), 1.0, 1.0),
272        ];
273        assert!(find_pairs(&bodies).len() >= 2);
274    }
275
276    #[test]
277    fn morton_encode_2d_origin() {
278        let pos = SVector::<f64, 2>::from([0.0, 0.0]);
279        let min = SVector::<f64, 2>::from([0.0, 0.0]);
280        let max = SVector::<f64, 2>::from([100.0, 100.0]);
281        assert_eq!(morton_encode::<2>(&pos, &min, &max), 0);
282    }
283
284    #[test]
285    fn morton_locality() {
286        let min = SVector::<f64, 3>::from([0.0; 3]);
287        let max = SVector::<f64, 3>::from([100.0; 3]);
288        let a = morton_encode::<3>(&SVector::from([10.0, 10.0, 10.0]), &min, &max);
289        let b = morton_encode::<3>(&SVector::from([11.0, 10.0, 10.0]), &min, &max);
290        let far = morton_encode::<3>(&SVector::from([90.0, 90.0, 90.0]), &min, &max);
291        assert!((a as i64 - b as i64).unsigned_abs() < (a as i64 - far as i64).unsigned_abs());
292    }
293
294    #[test]
295    fn lbvh_matches_brute_force() {
296        let mut bodies = Vec::new();
297        for i in 0..(BRUTE_FORCE_THRESHOLD + 10) {
298            let x = (i as f64) * 0.5;
299            bodies.push(RigidBody::<2>::dynamic_sphere(BodyHandle(i), Point::new([x, 0.0]), 1.0, 1.0));
300        }
301        let mut brute = find_pairs_brute(&bodies);
302        brute.sort_by_key(|p| (p.0, p.1));
303        let lbvh = find_pairs(&bodies);
304        for pair in &brute {
305            assert!(lbvh.contains(pair), "LBVH missing pair {:?}", pair);
306        }
307    }
308
309    #[test]
310    fn lbvh_deterministic() {
311        let bodies = vec![
312            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
313            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
314            RigidBody::<3>::dynamic_sphere(BodyHandle(2), Point::new([0.5, 0.5, 0.5]), 0.8, 1.0),
315        ];
316        assert_eq!(find_pairs(&bodies), find_pairs(&bodies));
317    }
318
319    #[test]
320    fn aabb_overlap() {
321        let a = Aabb::<2> { min: SVector::from([0.0, 0.0]), max: SVector::from([2.0, 2.0]) };
322        let b = Aabb::<2> { min: SVector::from([1.0, 1.0]), max: SVector::from([3.0, 3.0]) };
323        let c = Aabb::<2> { min: SVector::from([5.0, 5.0]), max: SVector::from([6.0, 6.0]) };
324        assert!(a.overlaps(&b));
325        assert!(!a.overlaps(&c));
326    }
327}
symtropy_physics/broadphase.rs

symtropy_physics/
broadphase.rs
