euv_engine/spatial/
impl.rs

1use crate::*;
2
3/// Implements construction, insertion, and querying for `SpatialHashGrid2D`.
4impl SpatialHashGrid2D {
5    /// Creates a new 2D spatial hash grid with the given cell size.
6    ///
7    /// # Arguments
8    ///
9    /// - `f64` - The world-space size of each grid cell.
10    ///
11    /// # Returns
12    ///
13    /// - `SpatialHashGrid2D` - The new grid.
14    pub fn create(cell_size: f64) -> SpatialHashGrid2D {
15        let safe_size: f64 = cell_size.max(EPSILON);
16        let mut grid: SpatialHashGrid2D = SpatialHashGrid2D::new(safe_size);
17        grid.set_inverse_cell_size(1.0 / safe_size);
18        grid
19    }
20
21    /// Creates a new 2D spatial hash grid with the default cell size.
22    ///
23    /// # Returns
24    ///
25    /// - `SpatialHashGrid2D` - The new grid.
26    pub fn with_default_size() -> SpatialHashGrid2D {
27        Self::create(SPATIAL_DEFAULT_CELL_SIZE_2D)
28    }
29
30    /// Inserts a body index into all cells overlapping the given bounding box.
31    ///
32    /// # Arguments
33    ///
34    /// - `usize` - The body index to insert.
35    /// - `Vector2D` - The minimum corner of the bounding box.
36    /// - `Vector2D` - The maximum corner of the bounding box.
37    pub fn insert(&mut self, index: usize, min: Vector2D, max: Vector2D) {
38        let inv: f64 = self.get_inverse_cell_size();
39        let min_col: i32 = (min.get_x() * inv).floor() as i32;
40        let min_row: i32 = (min.get_y() * inv).floor() as i32;
41        let max_col: i32 = (max.get_x() * inv).floor() as i32;
42        let max_row: i32 = (max.get_y() * inv).floor() as i32;
43        for col in min_col..=max_col {
44            for row in min_row..=max_row {
45                self.get_mut_cells()
46                    .entry((col, row))
47                    .or_default()
48                    .push(index);
49            }
50        }
51    }
52
53    /// Returns all candidate body indices whose cells overlap the given bounding box.
54    ///
55    /// Deduplicates indices so each candidate appears at most once.
56    ///
57    /// # Arguments
58    ///
59    /// - `Vector2D` - The minimum corner of the query box.
60    /// - `Vector2D` - The maximum corner of the query box.
61    ///
62    /// # Returns
63    ///
64    /// - `Vec<usize>` - The list of candidate body indices.
65    pub fn query(&self, min: Vector2D, max: Vector2D) -> Vec<usize> {
66        let inv: f64 = self.get_inverse_cell_size();
67        let min_col: i32 = (min.get_x() * inv).floor() as i32;
68        let min_row: i32 = (min.get_y() * inv).floor() as i32;
69        let max_col: i32 = (max.get_x() * inv).floor() as i32;
70        let max_row: i32 = (max.get_y() * inv).floor() as i32;
71        let mut seen: HashSet<usize> = HashSet::new();
72        let mut result: Vec<usize> = Vec::new();
73        for col in min_col..=max_col {
74            for row in min_row..=max_row {
75                if let Some(entries) = self.get_cells().get(&(col, row)) {
76                    for index in entries {
77                        if seen.insert(*index) {
78                            result.push(*index);
79                        }
80                    }
81                }
82            }
83        }
84        result
85    }
86
87    /// Removes all entries from the grid, preparing it for a fresh insertion pass.
88    pub fn clear(&mut self) {
89        self.get_mut_cells().clear();
90    }
91}
92
93/// Implements construction, insertion, and querying for `SpatialHashGrid3D`.
94impl SpatialHashGrid3D {
95    /// Creates a new 3D spatial hash grid with the given cell size.
96    ///
97    /// # Arguments
98    ///
99    /// - `f64` - The world-space size of each grid cell.
100    ///
101    /// # Returns
102    ///
103    /// - `SpatialHashGrid3D` - The new grid.
104    pub fn create(cell_size: f64) -> SpatialHashGrid3D {
105        let safe_size: f64 = cell_size.max(EPSILON);
106        let mut grid: SpatialHashGrid3D = SpatialHashGrid3D::new(safe_size);
107        grid.set_inverse_cell_size(1.0 / safe_size);
108        grid
109    }
110
111    /// Creates a new 3D spatial hash grid with the default cell size.
112    ///
113    /// # Returns
114    ///
115    /// - `SpatialHashGrid3D` - The new grid.
116    pub fn with_default_size() -> SpatialHashGrid3D {
117        Self::create(SPATIAL_DEFAULT_CELL_SIZE_3D)
118    }
119
120    /// Inserts a body index into all cells overlapping the given 3D bounding box.
121    ///
122    /// # Arguments
123    ///
124    /// - `usize` - The body index to insert.
125    /// - `Vector3D` - The minimum corner of the bounding box.
126    /// - `Vector3D` - The maximum corner of the bounding box.
127    pub fn insert(&mut self, index: usize, min: Vector3D, max: Vector3D) {
128        let inv: f64 = self.get_inverse_cell_size();
129        let min_col: i32 = (min.get_x() * inv).floor() as i32;
130        let min_row: i32 = (min.get_y() * inv).floor() as i32;
131        let min_layer: i32 = (min.get_z() * inv).floor() as i32;
132        let max_col: i32 = (max.get_x() * inv).floor() as i32;
133        let max_row: i32 = (max.get_y() * inv).floor() as i32;
134        let max_layer: i32 = (max.get_z() * inv).floor() as i32;
135        for col in min_col..=max_col {
136            for row in min_row..=max_row {
137                for layer in min_layer..=max_layer {
138                    self.get_mut_cells()
139                        .entry((col, row, layer))
140                        .or_default()
141                        .push(index);
142                }
143            }
144        }
145    }
146
147    /// Returns all candidate body indices whose cells overlap the given 3D bounding box.
148    ///
149    /// Deduplicates indices so each candidate appears at most once.
150    ///
151    /// # Arguments
152    ///
153    /// - `Vector3D` - The minimum corner of the query box.
154    /// - `Vector3D` - The maximum corner of the query box.
155    ///
156    /// # Returns
157    ///
158    /// - `Vec<usize>` - The list of candidate body indices.
159    pub fn query(&self, min: Vector3D, max: Vector3D) -> Vec<usize> {
160        let inv: f64 = self.get_inverse_cell_size();
161        let min_col: i32 = (min.get_x() * inv).floor() as i32;
162        let min_row: i32 = (min.get_y() * inv).floor() as i32;
163        let min_layer: i32 = (min.get_z() * inv).floor() as i32;
164        let max_col: i32 = (max.get_x() * inv).floor() as i32;
165        let max_row: i32 = (max.get_y() * inv).floor() as i32;
166        let max_layer: i32 = (max.get_z() * inv).floor() as i32;
167        let mut seen: HashSet<usize> = HashSet::new();
168        let mut result: Vec<usize> = Vec::new();
169        for col in min_col..=max_col {
170            for row in min_row..=max_row {
171                for layer in min_layer..=max_layer {
172                    if let Some(entries) = self.get_cells().get(&(col, row, layer)) {
173                        for index in entries {
174                            if seen.insert(*index) {
175                                result.push(*index);
176                            }
177                        }
178                    }
179                }
180            }
181        }
182        result
183    }
184
185    /// Removes all entries from the grid, preparing it for a fresh insertion pass.
186    pub fn clear(&mut self) {
187        self.get_mut_cells().clear();
188    }
189}
190
191/// Implements `Default` for `SpatialHashGrid2D` with the default cell size.
192impl Default for SpatialHashGrid2D {
193    fn default() -> SpatialHashGrid2D {
194        SpatialHashGrid2D::with_default_size()
195    }
196}
197
198/// Implements `Default` for `SpatialHashGrid3D` with the default cell size.
199impl Default for SpatialHashGrid3D {
200    fn default() -> SpatialHashGrid3D {
201        SpatialHashGrid3D::with_default_size()
202    }
203}
euv_engine/spatial/impl.rs

euv_engine/spatial/
impl.rs
