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 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
204// ---------------------------------------------------------------------------
205// Incremental broadphase: cache static/kinematic AABBs across frames
206// ---------------------------------------------------------------------------
207
208/// Pre-computed broadphase descriptor for a single body (AABB + sphere + filter).
209///
210/// Decoupled from `RigidBody<D>` so it can be cached without holding references.
211/// Stores both the AABB (for static-cache queries) and the bounding sphere center
212/// and radius (for dynamic–dynamic sphere-distance test, matching `find_pairs_brute`).
213#[derive(Clone, Debug)]
214pub struct BpEntry<const D: usize> {
215    pub handle: BodyHandle,
216    pub aabb: Aabb<D>,
217    /// World-space bounding sphere center.
218    pub sphere_center: SVector<f64, D>,
219    /// Bounding sphere radius.
220    pub sphere_radius: f64,
221    pub body_type: BodyType,
222    pub collision_group: u32,
223    pub collision_mask: u32,
224}
225
226impl<const D: usize> BpEntry<D> {
227    pub fn from_body(body: &RigidBody<D>) -> Self {
228        let (c_local, r) = body.collider.bounding_sphere();
229        let c_world = body.transform.transform_point(&c_local).0;
230        BpEntry {
231            handle: body.handle,
232            aabb: Aabb::from_sphere(&c_world, r),
233            sphere_center: c_world,
234            sphere_radius: r,
235            body_type: body.body_type,
236            collision_group: body.collision_group,
237            collision_mask: body.collision_mask,
238        }
239    }
240}
241
242/// Cached broadphase state for Static and Kinematic bodies.
243///
244/// Rebuilt only when a static/kinematic body is added or removed (`dirty` flag).
245/// This avoids including immovable geometry in the per-frame LBVH rebuild,
246/// which is O(n log n) in the total body count.
247///
248/// For scenes with many static bodies (terrain, walls) and few dynamic bodies,
249/// this typically halves or better the broadphase cost.
250pub struct StaticBroadphase<const D: usize> {
251    entries: Vec<BpEntry<D>>,
252}
253
254impl<const D: usize> StaticBroadphase<D> {
255    pub fn new() -> Self {
256        Self { entries: Vec::new() }
257    }
258
259    /// Rebuild the cache from all bodies in the world.
260    ///
261    /// Call this when `static_tree_dirty` is set on `PhysicsWorld`.
262    pub fn rebuild(&mut self, bodies: &[RigidBody<D>]) {
263        self.entries = bodies
264            .iter()
265            .filter(|b| b.body_type == BodyType::Static || b.body_type == BodyType::Kinematic)
266            .map(BpEntry::from_body)
267            .collect();
268    }
269
270    pub fn is_empty(&self) -> bool {
271        self.entries.is_empty()
272    }
273
274    pub fn len(&self) -> usize {
275        self.entries.len()
276    }
277
278    /// Enumerate pairs between a dynamic entry and all cached static entries.
279    ///
280    /// Uses sphere-distance test (same as `find_pairs_brute`) for exact equivalence.
281    fn query_against<'a>(
282        &'a self,
283        dyn_entry: &'a BpEntry<D>,
284    ) -> impl Iterator<Item = BroadphasePair> + 'a {
285        self.entries.iter().filter_map(move |s| {
286            let group_ok = (dyn_entry.collision_group & s.collision_mask != 0)
287                && (s.collision_group & dyn_entry.collision_mask != 0);
288            if group_ok {
289                let dist = (dyn_entry.sphere_center - s.sphere_center).norm();
290                if dist <= dyn_entry.sphere_radius + s.sphere_radius {
291                    let (ha, hb) = (dyn_entry.handle, s.handle);
292                    let pair = if ha < hb {
293                        BroadphasePair(ha, hb)
294                    } else {
295                        BroadphasePair(hb, ha)
296                    };
297                    return Some(pair);
298                }
299            }
300            None
301        })
302    }
303}
304
305impl<const D: usize> Default for StaticBroadphase<D> {
306    fn default() -> Self {
307        Self::new()
308    }
309}
310
311/// Find all potentially colliding pairs using the incremental static cache.
312///
313/// Separates the body list into dynamic and non-dynamic entries:
314/// - Dynamic–dynamic: brute-force O(n_dyn²), acceptable for typical games
315///   where the number of actively moving bodies is small.
316/// - Dynamic–static: linear scan against the prebuilt `StaticBroadphase`.
317///   For large static geometry this avoids re-packing static bodies into the
318///   LBVH every frame.
319///
320/// For very large dynamic counts (≥ BRUTE_FORCE_THRESHOLD) the existing
321/// `find_pairs` (full LBVH over all bodies) is called instead, since it is
322/// superior at scale.
323pub fn find_pairs_incremental<const D: usize>(
324    bodies: &[RigidBody<D>],
325    static_bp: &StaticBroadphase<D>,
326) -> Vec<BroadphasePair> {
327    // Fast paths
328    if bodies.is_empty() {
329        return Vec::new();
330    }
331    if static_bp.is_empty() {
332        // No static cache: identical to the standard path.
333        return find_pairs(bodies);
334    }
335
336    // Compute AABB entries for dynamic bodies only
337    let dynamic: Vec<BpEntry<D>> = bodies
338        .iter()
339        .filter(|b| b.body_type == BodyType::Dynamic)
340        .map(BpEntry::from_body)
341        .collect();
342
343    // For large dynamic counts fall back to the full LBVH path which is more
344    // efficient at scale. The static cache saves rebuilding the full tree.
345    if dynamic.len() >= BRUTE_FORCE_THRESHOLD {
346        return find_pairs(bodies);
347    }
348
349    let mut pairs = Vec::new();
350
351    // Dynamic–dynamic pairs (brute force, sphere distance test matching find_pairs_brute)
352    for i in 0..dynamic.len() {
353        for j in (i + 1)..dynamic.len() {
354            let a = &dynamic[i];
355            let b = &dynamic[j];
356            let group_ok = (a.collision_group & b.collision_mask != 0)
357                && (b.collision_group & a.collision_mask != 0);
358            if group_ok {
359                let dist = (a.sphere_center - b.sphere_center).norm();
360                if dist <= a.sphere_radius + b.sphere_radius {
361                    let (ha, hb) = (a.handle, b.handle);
362                    if ha < hb {
363                        pairs.push(BroadphasePair(ha, hb));
364                    } else {
365                        pairs.push(BroadphasePair(hb, ha));
366                    }
367                }
368            }
369        }
370    }
371
372    // Dynamic–static pairs (linear scan against cached static AABB entries)
373    for dyn_entry in &dynamic {
374        pairs.extend(static_bp.query_against(dyn_entry));
375    }
376
377    pairs.sort_unstable_by_key(|p| (p.0, p.1));
378    pairs.dedup();
379    pairs
380}
381
382fn find_pairs_brute<const D: usize>(bodies: &[RigidBody<D>]) -> Vec<BroadphasePair> {
383    let mut pairs = Vec::new();
384    for i in 0..bodies.len() {
385        let ti = bodies[i].body_type;
386        for j in (i+1)..bodies.len() {
387            if ti == BodyType::Static && bodies[j].body_type == BodyType::Static { continue; }
388            if !should_collide(&bodies[i], &bodies[j]) { continue; }
389            let (ci, ri) = bodies[i].collider.bounding_sphere();
390            let (cj, rj) = bodies[j].collider.bounding_sphere();
391            let wi = bodies[i].transform.transform_point(&ci);
392            let wj = bodies[j].transform.transform_point(&cj);
393            if wi.distance(&wj) <= ri + rj {
394                pairs.push(BroadphasePair(bodies[i].handle, bodies[j].handle));
395            }
396        }
397    }
398    pairs
399}
400
401#[cfg(test)]
402mod tests {
403    use super::*;
404    use crate::body::RigidBody;
405    use symtropy_math::{Point, Sphere};
406
407    #[test]
408    fn overlapping_pair_detected() {
409        let bodies = vec![
410            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
411            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
412        ];
413        assert_eq!(find_pairs(&bodies).len(), 1);
414    }
415
416    #[test]
417    fn separated_pair_not_detected() {
418        let bodies = vec![
419            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
420            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([5.0, 0.0, 0.0]), 1.0, 1.0),
421        ];
422        assert!(find_pairs(&bodies).is_empty());
423    }
424
425    #[test]
426    fn static_static_ignored() {
427        let bodies = vec![
428            RigidBody::<3>::static_body(BodyHandle(0), Point::origin(), Box::new(Sphere::unit())),
429            RigidBody::<3>::static_body(BodyHandle(1), Point::new([0.5, 0.0, 0.0]), Box::new(Sphere::unit())),
430        ];
431        assert!(find_pairs(&bodies).is_empty());
432    }
433
434    #[test]
435    fn static_dynamic_detected() {
436        let bodies = vec![
437            RigidBody::<3>::static_body(BodyHandle(0), Point::origin(), Box::new(Sphere::unit())),
438            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
439        ];
440        assert_eq!(find_pairs(&bodies).len(), 1);
441    }
442
443    #[test]
444    fn multiple_pairs() {
445        let bodies = vec![
446            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
447            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
448            RigidBody::<3>::dynamic_sphere(BodyHandle(2), Point::new([10.0, 0.0, 0.0]), 1.0, 1.0),
449            RigidBody::<3>::dynamic_sphere(BodyHandle(3), Point::new([1.0, 1.0, 0.0]), 1.0, 1.0),
450        ];
451        assert!(find_pairs(&bodies).len() >= 2);
452    }
453
454    #[test]
455    fn morton_encode_2d_origin() {
456        let pos = SVector::<f64, 2>::from([0.0, 0.0]);
457        let min = SVector::<f64, 2>::from([0.0, 0.0]);
458        let max = SVector::<f64, 2>::from([100.0, 100.0]);
459        assert_eq!(morton_encode::<2>(&pos, &min, &max), 0);
460    }
461
462    #[test]
463    fn morton_locality() {
464        let min = SVector::<f64, 3>::from([0.0; 3]);
465        let max = SVector::<f64, 3>::from([100.0; 3]);
466        let a = morton_encode::<3>(&SVector::from([10.0, 10.0, 10.0]), &min, &max);
467        let b = morton_encode::<3>(&SVector::from([11.0, 10.0, 10.0]), &min, &max);
468        let far = morton_encode::<3>(&SVector::from([90.0, 90.0, 90.0]), &min, &max);
469        assert!((a as i64 - b as i64).unsigned_abs() < (a as i64 - far as i64).unsigned_abs());
470    }
471
472    #[test]
473    fn lbvh_matches_brute_force() {
474        let mut bodies = Vec::new();
475        for i in 0..(BRUTE_FORCE_THRESHOLD + 10) {
476            let x = (i as f64) * 0.5;
477            bodies.push(RigidBody::<2>::dynamic_sphere(BodyHandle(i), Point::new([x, 0.0]), 1.0, 1.0));
478        }
479        let mut brute = find_pairs_brute(&bodies);
480        brute.sort_by_key(|p| (p.0, p.1));
481        let lbvh = find_pairs(&bodies);
482        for pair in &brute {
483            assert!(lbvh.contains(pair), "LBVH missing pair {:?}", pair);
484        }
485    }
486
487    #[test]
488    fn lbvh_deterministic() {
489        let bodies = vec![
490            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
491            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
492            RigidBody::<3>::dynamic_sphere(BodyHandle(2), Point::new([0.5, 0.5, 0.5]), 0.8, 1.0),
493        ];
494        assert_eq!(find_pairs(&bodies), find_pairs(&bodies));
495    }
496
497    #[test]
498    fn aabb_overlap() {
499        let a = Aabb::<2> { min: SVector::from([0.0, 0.0]), max: SVector::from([2.0, 2.0]) };
500        let b = Aabb::<2> { min: SVector::from([1.0, 1.0]), max: SVector::from([3.0, 3.0]) };
501        let c = Aabb::<2> { min: SVector::from([5.0, 5.0]), max: SVector::from([6.0, 6.0]) };
502        assert!(a.overlaps(&b));
503        assert!(!a.overlaps(&c));
504    }
505
506    // --- Incremental broadphase tests ---
507
508    #[test]
509    fn incremental_dynamic_static_detected() {
510        let bodies = vec![
511            RigidBody::<3>::static_body(BodyHandle(0), Point::origin(), Box::new(Sphere::unit())),
512            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
513        ];
514        let mut static_bp = super::StaticBroadphase::new();
515        static_bp.rebuild(&bodies);
516
517        let pairs = super::find_pairs_incremental(&bodies, &static_bp);
518        assert_eq!(pairs.len(), 1);
519        let p = pairs[0];
520        assert!((p.0 == BodyHandle(0) && p.1 == BodyHandle(1))
521            || (p.0 == BodyHandle(1) && p.1 == BodyHandle(0)));
522    }
523
524    #[test]
525    fn incremental_static_static_not_produced() {
526        let bodies = vec![
527            RigidBody::<3>::static_body(BodyHandle(0), Point::origin(), Box::new(Sphere::unit())),
528            RigidBody::<3>::static_body(BodyHandle(1), Point::new([0.5, 0.0, 0.0]), Box::new(Sphere::unit())),
529        ];
530        let mut static_bp = super::StaticBroadphase::new();
531        static_bp.rebuild(&bodies);
532
533        let pairs = super::find_pairs_incremental(&bodies, &static_bp);
534        assert!(pairs.is_empty(), "static–static must never collide");
535    }
536
537    #[test]
538    fn incremental_matches_standard_find_pairs() {
539        let bodies = vec![
540            RigidBody::<3>::static_body(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), Box::new(Sphere::unit())),
541            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
542            RigidBody::<3>::dynamic_sphere(BodyHandle(2), Point::new([0.0, 1.5, 0.0]), 1.0, 1.0),
543            RigidBody::<3>::dynamic_sphere(BodyHandle(3), Point::new([10.0, 0.0, 0.0]), 1.0, 1.0),
544        ];
545        let mut static_bp = super::StaticBroadphase::new();
546        static_bp.rebuild(&bodies);
547
548        let mut incremental = super::find_pairs_incremental(&bodies, &static_bp);
549        let mut standard = find_pairs(&bodies);
550        incremental.sort_by_key(|p| (p.0, p.1));
551        standard.sort_by_key(|p| (p.0, p.1));
552        assert_eq!(incremental, standard, "incremental must match standard broadphase");
553    }
554
555    #[test]
556    fn incremental_no_static_degrades_to_standard() {
557        let bodies = vec![
558            RigidBody::<3>::dynamic_sphere(BodyHandle(0), Point::new([0.0, 0.0, 0.0]), 1.0, 1.0),
559            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
560        ];
561        let empty_bp = super::StaticBroadphase::<3>::new(); // no rebuild
562        let pairs = super::find_pairs_incremental(&bodies, &empty_bp);
563        assert_eq!(pairs.len(), 1);
564    }
565
566    #[test]
567    fn static_broadphase_rebuild_updates_cache() {
568        let bodies = vec![
569            RigidBody::<3>::static_body(BodyHandle(0), Point::origin(), Box::new(Sphere::unit())),
570            RigidBody::<3>::dynamic_sphere(BodyHandle(1), Point::new([1.5, 0.0, 0.0]), 1.0, 1.0),
571        ];
572        let mut static_bp = super::StaticBroadphase::<3>::new();
573        assert_eq!(static_bp.len(), 0);
574
575        static_bp.rebuild(&bodies);
576        assert_eq!(static_bp.len(), 1, "should cache the one static body");
577    }
578}
symtropy_physics/broadphase.rs

symtropy_physics/
broadphase.rs
