arcane_core/physics/
broadphase.rs

1use std::collections::{HashMap, HashSet};
2
3use super::types::BodyId;
4
5/// Default speculative margin for continuous collision detection.
6/// Objects within this distance (plus velocity-based expansion) generate
7/// speculative contacts to prevent tunneling.
8pub const SPECULATIVE_MARGIN: f32 = 5.0;
9
10pub struct SpatialHash {
11    #[allow(dead_code)]
12    cell_size: f32,
13    inv_cell_size: f32,
14    cells: HashMap<(i32, i32), Vec<BodyId>>,
15}
16
17impl SpatialHash {
18    pub fn new(cell_size: f32) -> Self {
19        let cell_size = if cell_size > 0.0 { cell_size } else { 64.0 };
20        Self {
21            cell_size,
22            inv_cell_size: 1.0 / cell_size,
23            cells: HashMap::new(),
24        }
25    }
26
27    pub fn clear(&mut self) {
28        self.cells.clear();
29    }
30
31    /// Insert a body's AABB into all overlapping cells.
32    pub fn insert(&mut self, id: BodyId, min_x: f32, min_y: f32, max_x: f32, max_y: f32) {
33        let x0 = (min_x * self.inv_cell_size).floor() as i32;
34        let y0 = (min_y * self.inv_cell_size).floor() as i32;
35        let x1 = (max_x * self.inv_cell_size).floor() as i32;
36        let y1 = (max_y * self.inv_cell_size).floor() as i32;
37
38        for cx in x0..=x1 {
39            for cy in y0..=y1 {
40                self.cells.entry((cx, cy)).or_default().push(id);
41            }
42        }
43    }
44
45    /// Insert a body's AABB expanded by velocity for speculative contact detection.
46    /// The expansion is: velocity * dt + fixed margin. This catches fast-moving
47    /// objects that might tunnel through thin obstacles.
48    pub fn insert_speculative(
49        &mut self,
50        id: BodyId,
51        min_x: f32,
52        min_y: f32,
53        max_x: f32,
54        max_y: f32,
55        vx: f32,
56        vy: f32,
57        dt: f32,
58    ) {
59        // Expand AABB by velocity projection + margin
60        let expand_x = vx.abs() * dt + SPECULATIVE_MARGIN;
61        let expand_y = vy.abs() * dt + SPECULATIVE_MARGIN;
62
63        self.insert(
64            id,
65            min_x - expand_x,
66            min_y - expand_y,
67            max_x + expand_x,
68            max_y + expand_y,
69        );
70    }
71
72    /// Collect unique pairs of bodies that share at least one cell.
73    pub fn get_pairs(&self) -> Vec<(BodyId, BodyId)> {
74        let mut seen = HashSet::new();
75        let mut pairs = Vec::new();
76
77        for cell_bodies in self.cells.values() {
78            let n = cell_bodies.len();
79            for i in 0..n {
80                for j in (i + 1)..n {
81                    let a = cell_bodies[i];
82                    let b = cell_bodies[j];
83                    let pair = if a < b { (a, b) } else { (b, a) };
84                    if seen.insert(pair) {
85                        pairs.push(pair);
86                    }
87                }
88            }
89        }
90        pairs
91    }
92}
arcane_core/physics/broadphase.rs

arcane_core/physics/
broadphase.rs
