1use super::{
4 Frustum2D, Frustum3D, Ray2, Ray3, RayHit2, RayHit3, SpatialError, ray::slab, valid_distance,
5};
6use crate::collision::{Aabb2, Aabb3};
7use glam::{Vec2, Vec3};
8use std::cmp::Ordering;
9
10const LEAF_CAPACITY: usize = 8;
11
12#[derive(Clone, Copy)]
13struct Bounds<const N: usize> {
14 min: [f32; N],
15 max: [f32; N],
16}
17
18impl<const N: usize> Bounds<N> {
19 fn valid(self) -> bool {
20 (0..N).all(|axis| {
21 self.min[axis].is_finite()
22 && self.max[axis].is_finite()
23 && self.min[axis] <= self.max[axis]
24 })
25 }
26
27 fn union(mut self, other: Self) -> Self {
28 for axis in 0..N {
29 self.min[axis] = self.min[axis].min(other.min[axis]);
30 self.max[axis] = self.max[axis].max(other.max[axis]);
31 }
32 self
33 }
34
35 fn overlaps(self, other: Self) -> bool {
36 (0..N).all(|axis| self.min[axis] < other.max[axis] && self.max[axis] > other.min[axis])
37 }
38
39 fn center(self, axis: usize) -> f32 {
40 self.min[axis] * 0.5 + self.max[axis] * 0.5
41 }
42}
43
44#[derive(Clone, Copy)]
45struct Entry<T, const N: usize> {
46 id: T,
47 bounds: Bounds<N>,
48 rank: usize,
49}
50
51enum Children {
52 Leaf { start: usize, end: usize },
53 Branch { left: usize, right: usize },
54}
55
56struct Node<const N: usize> {
57 bounds: Bounds<N>,
58 children: Children,
59}
60
61struct Bvh<T, const N: usize> {
62 entries: Vec<Entry<T, N>>,
63 nodes: Vec<Node<N>>,
64 staging_entries: Vec<Entry<T, N>>,
65 staging_nodes: Vec<Node<N>>,
66}
67
68impl<T: Copy, const N: usize> Bvh<T, N> {
69 fn new() -> Self {
70 Self {
71 entries: Vec::new(),
72 nodes: Vec::new(),
73 staging_entries: Vec::new(),
74 staging_nodes: Vec::new(),
75 }
76 }
77
78 fn rebuild(
79 &mut self,
80 source: impl IntoIterator<Item = (T, Bounds<N>)>,
81 ) -> Result<usize, SpatialError> {
82 self.staging_entries.clear();
83 for (rank, (id, bounds)) in source.into_iter().enumerate() {
84 if !bounds.valid() {
85 return Err(SpatialError::InvalidBounds(rank));
86 }
87 self.staging_entries.push(Entry { id, bounds, rank });
88 }
89 self.staging_nodes.clear();
90 if !self.staging_entries.is_empty() {
91 build(&mut self.staging_entries, &mut self.staging_nodes, 0);
92 }
93 std::mem::swap(&mut self.entries, &mut self.staging_entries);
94 std::mem::swap(&mut self.nodes, &mut self.staging_nodes);
95 Ok(self.entries.len())
96 }
97
98 fn visit(&self, intersects: impl Fn(Bounds<N>) -> bool, mut emit: impl FnMut(T)) {
99 if !self.nodes.is_empty() {
100 self.visit_node(0, &intersects, &mut emit);
101 }
102 }
103
104 fn visit_node(
105 &self,
106 index: usize,
107 intersects: &impl Fn(Bounds<N>) -> bool,
108 emit: &mut impl FnMut(T),
109 ) {
110 let node = &self.nodes[index];
111 if !intersects(node.bounds) {
112 return;
113 }
114 match node.children {
115 Children::Leaf { start, end } => {
116 for entry in &self.entries[start..end] {
117 if intersects(entry.bounds) {
118 emit(entry.id);
119 }
120 }
121 }
122 Children::Branch { left, right } => {
123 self.visit_node(left, intersects, emit);
124 self.visit_node(right, intersects, emit);
125 }
126 }
127 }
128
129 fn nearest(
130 &self,
131 origin: [f32; N],
132 direction: [f32; N],
133 max_distance: f32,
134 ) -> Option<(T, f32, [f32; N])> {
135 let mut best = None;
136 if !self.nodes.is_empty() {
137 self.nearest_node(0, origin, direction, max_distance, &mut best);
138 }
139 best.map(|(id, distance, normal, _)| (id, distance, normal))
140 }
141
142 fn nearest_node(
143 &self,
144 index: usize,
145 origin: [f32; N],
146 direction: [f32; N],
147 max_distance: f32,
148 best: &mut Option<(T, f32, [f32; N], usize)>,
149 ) {
150 let node = &self.nodes[index];
151 let limit = best.as_ref().map_or(max_distance, |hit| hit.1);
152 if slab(origin, direction, node.bounds.min, node.bounds.max, limit).is_none() {
153 return;
154 }
155 match node.children {
156 Children::Leaf { start, end } => {
157 for entry in &self.entries[start..end] {
158 let limit = best.as_ref().map_or(max_distance, |hit| hit.1);
159 if let Some((distance, normal)) =
160 slab(origin, direction, entry.bounds.min, entry.bounds.max, limit)
161 && best.as_ref().is_none_or(|hit| {
162 distance < hit.1 || (distance == hit.1 && entry.rank < hit.3)
163 })
164 {
165 *best = Some((entry.id, distance, normal, entry.rank));
166 }
167 }
168 }
169 Children::Branch { left, right } => {
170 let left_distance = self.node_distance(left, origin, direction, limit);
171 let right_distance = self.node_distance(right, origin, direction, limit);
172 match (left_distance, right_distance) {
174 (Some(a), Some(b)) => {
175 let (first, second) = if a <= b { (left, right) } else { (right, left) };
176 self.nearest_node(first, origin, direction, max_distance, best);
177 self.nearest_node(second, origin, direction, max_distance, best);
178 }
179 (Some(_), None) => {
180 self.nearest_node(left, origin, direction, max_distance, best)
181 }
182 (None, Some(_)) => {
183 self.nearest_node(right, origin, direction, max_distance, best)
184 }
185 (None, None) => {}
186 }
187 }
188 }
189 }
190
191 fn node_distance(
192 &self,
193 index: usize,
194 origin: [f32; N],
195 direction: [f32; N],
196 limit: f32,
197 ) -> Option<f32> {
198 let bounds = self.nodes[index].bounds;
199 slab(origin, direction, bounds.min, bounds.max, limit).map(|hit| hit.0)
200 }
201}
202
203fn build<T: Copy, const N: usize>(
204 entries: &mut [Entry<T, N>],
205 nodes: &mut Vec<Node<N>>,
206 start: usize,
207) -> usize {
208 let mut bounds = entries[0].bounds;
209 let mut min_center = [f32::INFINITY; N];
210 let mut max_center = [f32::NEG_INFINITY; N];
211 for entry in entries.iter() {
212 bounds = bounds.union(entry.bounds);
213 for axis in 0..N {
214 let center = entry.bounds.center(axis);
215 min_center[axis] = min_center[axis].min(center);
216 max_center[axis] = max_center[axis].max(center);
217 }
218 }
219 let index = nodes.len();
220 nodes.push(Node {
221 bounds,
222 children: Children::Leaf {
223 start,
224 end: start + entries.len(),
225 },
226 });
227 if entries.len() > LEAF_CAPACITY {
228 let axis = (0..N)
229 .max_by(|&a, &b| {
230 (max_center[a] - min_center[a])
231 .partial_cmp(&(max_center[b] - min_center[b]))
232 .unwrap_or(Ordering::Equal)
233 })
234 .unwrap_or(0);
235 let middle = entries.len() / 2;
236 entries.select_nth_unstable_by(middle, |a, b| {
237 a.bounds
238 .center(axis)
239 .total_cmp(&b.bounds.center(axis))
240 .then(a.rank.cmp(&b.rank))
241 });
242 let (left_entries, right_entries) = entries.split_at_mut(middle);
243 let left = build(left_entries, nodes, start);
244 let right = build(right_entries, nodes, start + middle);
245 nodes[index].children = Children::Branch { left, right };
246 }
247 index
248}
249
250#[derive(Clone, Copy, Debug, PartialEq)]
252pub struct SpatialHit2<T> {
253 pub id: T,
255 pub hit: RayHit2,
257}
258
259#[derive(Clone, Copy, Debug, PartialEq)]
261pub struct SpatialHit3<T> {
262 pub id: T,
264 pub hit: RayHit3,
266}
267
268pub struct SpatialIndex2D<T: Copy>(Bvh<T, 2>);
276
277pub struct SpatialIndex3D<T: Copy>(Bvh<T, 3>);
279
280macro_rules! index_impl {
281 ($index:ident, $ray:ident, $hit:ident, $ray_hit:ident, $box:ident, $vec:ident, $frustum:ident, $n:expr) => {
282 impl<T: Copy> Default for $index<T> {
283 fn default() -> Self {
284 Self::new()
285 }
286 }
287
288 impl<T: Copy> $index<T> {
289 pub fn new() -> Self {
291 Self(Bvh::new())
292 }
293
294 pub fn len(&self) -> usize {
296 self.0.entries.len()
297 }
298
299 pub fn is_empty(&self) -> bool {
301 self.0.entries.is_empty()
302 }
303
304 pub fn rebuild(
308 &mut self,
309 source: impl IntoIterator<Item = (T, $box)>,
310 ) -> Result<usize, SpatialError> {
311 self.0.rebuild(source.into_iter().map(|(id, bounds)| {
312 (
313 id,
314 Bounds {
315 min: bounds.min.to_array(),
316 max: bounds.max.to_array(),
317 },
318 )
319 }))
320 }
321
322 pub fn nearest(
325 &self,
326 ray: $ray,
327 max_distance: f32,
328 ) -> Result<Option<$hit<T>>, SpatialError> {
329 valid_distance(max_distance)?;
330 Ok(self
331 .0
332 .nearest(
333 ray.origin.to_array(),
334 ray.direction.to_array(),
335 max_distance,
336 )
337 .map(|(id, distance, normal)| $hit {
338 id,
339 hit: $ray_hit {
340 distance,
341 point: ray.at(distance),
342 normal: <$vec>::from_array(normal),
343 },
344 }))
345 }
346
347 pub fn visit_overlapping(
350 &self,
351 area: $box,
352 emit: impl FnMut(T),
353 ) -> Result<(), SpatialError> {
354 let bounds = Bounds {
355 min: area.min.to_array(),
356 max: area.max.to_array(),
357 };
358 if !bounds.valid() {
359 return Err(SpatialError::InvalidBounds(0));
360 }
361 self.0.visit(|candidate| candidate.overlaps(bounds), emit);
362 Ok(())
363 }
364
365 pub fn overlapping(&self, area: $box) -> Result<Vec<T>, SpatialError> {
367 let mut ids = Vec::new();
368 self.visit_overlapping(area, |id| ids.push(id))?;
369 Ok(ids)
370 }
371
372 pub fn visit_visible(&self, view: &$frustum, emit: impl FnMut(T)) {
374 self.0.visit(
375 |bounds| {
376 view.intersects($box {
377 min: <$vec>::from_array(bounds.min),
378 max: <$vec>::from_array(bounds.max),
379 })
380 },
381 emit,
382 );
383 }
384
385 pub fn visible(&self, view: &$frustum) -> Vec<T> {
387 let mut ids = Vec::new();
388 self.visit_visible(view, |id| ids.push(id));
389 ids
390 }
391 }
392 };
393}
394
395index_impl!(
396 SpatialIndex2D,
397 Ray2,
398 SpatialHit2,
399 RayHit2,
400 Aabb2,
401 Vec2,
402 Frustum2D,
403 2
404);
405index_impl!(
406 SpatialIndex3D,
407 Ray3,
408 SpatialHit3,
409 RayHit3,
410 Aabb3,
411 Vec3,
412 Frustum3D,
413 3
414);