scirs2_spatial/collision/
broadphase.rs

1//! Broad-phase collision detection algorithms
2//!
3//! This module provides efficient algorithms for quickly filtering out pairs of objects
4//! that cannot possibly be colliding, reducing the number of detailed collision tests needed.
5
6use super::shapes::{Box2D, Box3D, Circle, Sphere};
7
8/// A simple AABB (Axis-Aligned Bounding Box) for spatial partitioning
9#[derive(Debug, Clone, Copy)]
10pub struct AABB {
11    /// Minimum corner of the bounding box
12    pub min: [f64; 3],
13    /// Maximum corner of the bounding box
14    pub max: [f64; 3],
15}
16
17impl AABB {
18    /// Creates a new AABB with the given minimum and maximum corners
19    pub fn new(min: [f64; 3], max: [f64; 3]) -> Self {
20        AABB { min, max }
21    }
22
23    /// Creates an AABB from a 3D box
24    pub fn from_box3d(box3d: &Box3D) -> Self {
25        AABB {
26            min: box3d.min,
27            max: box3d.max,
28        }
29    }
30
31    /// Creates an AABB from a sphere
32    pub fn from_sphere(sphere: &Sphere) -> Self {
33        let radius = sphere.radius;
34        AABB {
35            min: [
36                sphere.center[0] - radius,
37                sphere.center[1] - radius,
38                sphere.center[2] - radius,
39            ],
40            max: [
41                sphere.center[0] + radius,
42                sphere.center[1] + radius,
43                sphere.center[2] + radius,
44            ],
45        }
46    }
47
48    /// Creates an AABB from a 2D box, setting z-coordinates to 0
49    pub fn from_box2d(box2d: &Box2D) -> Self {
50        AABB {
51            min: [box2d.min[0], box2d.min[1], 0.0],
52            max: [box2d.max[0], box2d.max[1], 0.0],
53        }
54    }
55
56    /// Creates an AABB from a circle, setting z-coordinates to 0
57    pub fn from_circle(circle: &Circle) -> Self {
58        let radius = circle.radius;
59        AABB {
60            min: [circle.center[0] - radius, circle.center[1] - radius, 0.0],
61            max: [circle.center[0] + radius, circle.center[1] + radius, 0.0],
62        }
63    }
64
65    /// Tests if this AABB intersects with another AABB
66    pub fn intersects(&self, other: &AABB) -> bool {
67        self.min[0] <= other.max[0]
68            && self.max[0] >= other.min[0]
69            && self.min[1] <= other.max[1]
70            && self.max[1] >= other.min[1]
71            && self.min[2] <= other.max[2]
72            && self.max[2] >= other.min[2]
73    }
74}
75
76/// A simple grid-based spatial partitioning structure for 2D space
77pub struct SpatialGrid2D {
78    /// Cell size (width and height)
79    cellsize: f64,
80    /// Total width of the grid
81    width: f64,
82    /// Total height of the grid
83    height: f64,
84    /// Number of cells in the x-direction
85    cells_x: usize,
86    /// Number of cells in the y-direction
87    cells_y: usize,
88}
89
90impl SpatialGrid2D {
91    /// Creates a new 2D spatial grid with the given dimensions and cell size
92    pub fn new(_width: f64, height: f64, cellsize: f64) -> Self {
93        let cells_x = (_width / cellsize).ceil() as usize;
94        let cells_y = (height / cellsize).ceil() as usize;
95        SpatialGrid2D {
96            cellsize,
97            width: _width,
98            height,
99            cells_x,
100            cells_y,
101        }
102    }
103
104    /// Returns the cell indices for a given 2D position
105    pub fn get_cell_indices(&self, pos: &[f64; 2]) -> Option<(usize, usize)> {
106        if pos[0] < 0.0 || pos[0] >= self.width || pos[1] < 0.0 || pos[1] >= self.height {
107            return None;
108        }
109
110        let x = (pos[0] / self.cellsize) as usize;
111        let y = (pos[1] / self.cellsize) as usize;
112
113        // Ensure indices are within bounds
114        if x >= self.cells_x || y >= self.cells_y {
115            return None;
116        }
117
118        Some((x, y))
119    }
120
121    /// Returns the cell indices for a 2D circle, potentially spanning multiple cells
122    pub fn get_circle_cell_indices(&self, circle: &Circle) -> Vec<(usize, usize)> {
123        let min_x = (circle.center[0] - circle.radius).max(0.0);
124        let min_y = (circle.center[1] - circle.radius).max(0.0);
125        let max_x = (circle.center[0] + circle.radius).min(self.width);
126        let max_y = (circle.center[1] + circle.radius).min(self.height);
127
128        let min_cell_x = (min_x / self.cellsize) as usize;
129        let min_cell_y = (min_y / self.cellsize) as usize;
130        let max_cell_x = (max_x / self.cellsize) as usize;
131        let max_cell_y = (max_y / self.cellsize) as usize;
132
133        let mut cells = Vec::new();
134        for y in min_cell_y..=max_cell_y {
135            for x in min_cell_x..=max_cell_x {
136                if x < self.cells_x && y < self.cells_y {
137                    cells.push((x, y));
138                }
139            }
140        }
141
142        cells
143    }
144
145    /// Returns the cell indices for a 2D box, potentially spanning multiple cells
146    pub fn get_box_cell_indices(&self, box2d: &Box2D) -> Vec<(usize, usize)> {
147        let min_x = box2d.min[0].max(0.0);
148        let min_y = box2d.min[1].max(0.0);
149        let max_x = box2d.max[0].min(self.width);
150        let max_y = box2d.max[1].min(self.height);
151
152        let min_cell_x = (min_x / self.cellsize) as usize;
153        let min_cell_y = (min_y / self.cellsize) as usize;
154        let max_cell_x = (max_x / self.cellsize) as usize;
155        let max_cell_y = (max_y / self.cellsize) as usize;
156
157        let mut cells = Vec::new();
158        for y in min_cell_y..=max_cell_y {
159            for x in min_cell_x..=max_cell_x {
160                if x < self.cells_x && y < self.cells_y {
161                    cells.push((x, y));
162                }
163            }
164        }
165
166        cells
167    }
168}
169
170/// A simple grid-based spatial partitioning structure for 3D space
171pub struct SpatialGrid3D {
172    /// Cell size (width, height, and depth)
173    cellsize: f64,
174    /// Total width of the grid (x-dimension)
175    width: f64,
176    /// Total height of the grid (y-dimension)
177    height: f64,
178    /// Total depth of the grid (z-dimension)
179    depth: f64,
180    /// Number of cells in the x-direction
181    cells_x: usize,
182    /// Number of cells in the y-direction
183    cells_y: usize,
184    /// Number of cells in the z-direction
185    cells_z: usize,
186}
187
188impl SpatialGrid3D {
189    /// Creates a new 3D spatial grid with the given dimensions and cell size
190    pub fn new(_width: f64, height: f64, depth: f64, cellsize: f64) -> Self {
191        let cells_x = (_width / cellsize).ceil() as usize;
192        let cells_y = (height / cellsize).ceil() as usize;
193        let cells_z = (depth / cellsize).ceil() as usize;
194        SpatialGrid3D {
195            cellsize,
196            width: _width,
197            height,
198            depth,
199            cells_x,
200            cells_y,
201            cells_z,
202        }
203    }
204
205    /// Returns the cell indices for a given 3D position
206    pub fn get_cell_indices(&self, pos: &[f64; 3]) -> Option<(usize, usize, usize)> {
207        if pos[0] < 0.0
208            || pos[0] >= self.width
209            || pos[1] < 0.0
210            || pos[1] >= self.height
211            || pos[2] < 0.0
212            || pos[2] >= self.depth
213        {
214            return None;
215        }
216
217        let x = (pos[0] / self.cellsize) as usize;
218        let y = (pos[1] / self.cellsize) as usize;
219        let z = (pos[2] / self.cellsize) as usize;
220
221        // Ensure indices are within bounds
222        if x >= self.cells_x || y >= self.cells_y || z >= self.cells_z {
223            return None;
224        }
225
226        Some((x, y, z))
227    }
228
229    /// Returns the cell indices for a 3D sphere, potentially spanning multiple cells
230    pub fn get_sphere_cell_indices(&self, sphere: &Sphere) -> Vec<(usize, usize, usize)> {
231        let min_x = (sphere.center[0] - sphere.radius).max(0.0);
232        let min_y = (sphere.center[1] - sphere.radius).max(0.0);
233        let min_z = (sphere.center[2] - sphere.radius).max(0.0);
234        let max_x = (sphere.center[0] + sphere.radius).min(self.width);
235        let max_y = (sphere.center[1] + sphere.radius).min(self.height);
236        let max_z = (sphere.center[2] + sphere.radius).min(self.depth);
237
238        let min_cell_x = (min_x / self.cellsize) as usize;
239        let min_cell_y = (min_y / self.cellsize) as usize;
240        let min_cell_z = (min_z / self.cellsize) as usize;
241        let max_cell_x = (max_x / self.cellsize) as usize;
242        let max_cell_y = (max_y / self.cellsize) as usize;
243        let max_cell_z = (max_z / self.cellsize) as usize;
244
245        let mut cells = Vec::new();
246        for z in min_cell_z..=max_cell_z {
247            for y in min_cell_y..=max_cell_y {
248                for x in min_cell_x..=max_cell_x {
249                    if x < self.cells_x && y < self.cells_y && z < self.cells_z {
250                        cells.push((x, y, z));
251                    }
252                }
253            }
254        }
255
256        cells
257    }
258
259    /// Returns the cell indices for a 3D box, potentially spanning multiple cells
260    pub fn get_box_cell_indices(&self, box3d: &Box3D) -> Vec<(usize, usize, usize)> {
261        let min_x = box3d.min[0].max(0.0);
262        let min_y = box3d.min[1].max(0.0);
263        let min_z = box3d.min[2].max(0.0);
264        let max_x = box3d.max[0].min(self.width);
265        let max_y = box3d.max[1].min(self.height);
266        let max_z = box3d.max[2].min(self.depth);
267
268        let min_cell_x = (min_x / self.cellsize) as usize;
269        let min_cell_y = (min_y / self.cellsize) as usize;
270        let min_cell_z = (min_z / self.cellsize) as usize;
271        let max_cell_x = (max_x / self.cellsize) as usize;
272        let max_cell_y = (max_y / self.cellsize) as usize;
273        let max_cell_z = (max_z / self.cellsize) as usize;
274
275        let mut cells = Vec::new();
276        for z in min_cell_z..=max_cell_z {
277            for y in min_cell_y..=max_cell_y {
278                for x in min_cell_x..=max_cell_x {
279                    if x < self.cells_x && y < self.cells_y && z < self.cells_z {
280                        cells.push((x, y, z));
281                    }
282                }
283            }
284        }
285
286        cells
287    }
288}
289
290/// A sweep and prune algorithm for broad-phase collision detection in 1D
291pub struct SweepAndPrune1D {
292    /// The axis to use for sorting objects (0 = x, 1 = y, 2 = z)
293    axis: usize,
294}
295
296impl SweepAndPrune1D {
297    /// Creates a new sweep and prune algorithm for the given axis
298    pub fn new(axis: usize) -> Self {
299        SweepAndPrune1D { axis }
300    }
301
302    /// Checks if two AABBs could be colliding along the chosen axis
303    pub fn may_collide(&self, aabb1: &AABB, aabb2: &AABB) -> bool {
304        aabb1.min[self.axis] <= aabb2.max[self.axis] && aabb1.max[self.axis] >= aabb2.min[self.axis]
305    }
306
307    /// Gets the starting point of an AABB along the chosen axis
308    pub fn get_start(&self, aabb: &AABB) -> f64 {
309        aabb.min[self.axis]
310    }
311
312    /// Gets the ending point of an AABB along the chosen axis
313    pub fn get_end(&self, aabb: &AABB) -> f64 {
314        aabb.max[self.axis]
315    }
316}
317
318/// The standard swept-prune algorithm for broad-phase collision detection
319pub struct SweepAndPrune {
320    /// Sweep and prune for the x-axis
321    x_axis: SweepAndPrune1D,
322    /// Sweep and prune for the y-axis
323    y_axis: SweepAndPrune1D,
324    /// Sweep and prune for the z-axis
325    z_axis: SweepAndPrune1D,
326}
327
328impl Default for SweepAndPrune {
329    fn default() -> Self {
330        Self::new()
331    }
332}
333
334impl SweepAndPrune {
335    /// Creates a new sweep and prune algorithm
336    pub fn new() -> Self {
337        SweepAndPrune {
338            x_axis: SweepAndPrune1D::new(0),
339            y_axis: SweepAndPrune1D::new(1),
340            z_axis: SweepAndPrune1D::new(2),
341        }
342    }
343
344    /// Checks if two AABBs could potentially be colliding
345    pub fn may_collide(&self, aabb1: &AABB, aabb2: &AABB) -> bool {
346        self.x_axis.may_collide(aabb1, aabb2)
347            && self.y_axis.may_collide(aabb1, aabb2)
348            && self.z_axis.may_collide(aabb1, aabb2)
349    }
350}
scirs2_spatial/collision/broadphase.rs

scirs2_spatial/collision/
broadphase.rs
