Skip to main content

rayengine_core/spatial/
index.rs

1//! Rebuildable, owned bounding-volume hierarchies for read-heavy queries.
2
3use 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                // Visit the closer child first to tighten the search limit.
173                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/// Nearest indexed 2D box contact.
251#[derive(Clone, Copy, Debug, PartialEq)]
252pub struct SpatialHit2<T> {
253    /// Caller-supplied collider ID.
254    pub id: T,
255    /// Ray/box contact.
256    pub hit: RayHit2,
257}
258
259/// Nearest indexed 3D box contact.
260#[derive(Clone, Copy, Debug, PartialEq)]
261pub struct SpatialHit3<T> {
262    /// Caller-supplied collider ID.
263    pub id: T,
264    /// Ray/box contact.
265    pub hit: RayHit3,
266}
267
268/// Owned 2D collider snapshot with a balanced AABB hierarchy.
269///
270/// Rebuild explicitly after moving colliders. Queries see the last successful
271/// snapshot. IDs need only be `Copy`; the index does not read or mutate ECS
272/// components. Duplicate IDs are accepted and reported as supplied. Rebuild
273/// validates all boxes before committing, reuses staging capacity, and needs
274/// temporary room for both the old and new snapshots.
275pub struct SpatialIndex2D<T: Copy>(Bvh<T, 2>);
276
277/// Owned 3D collider snapshot with the same lifecycle as [`SpatialIndex2D`].
278pub 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            /// Creates an empty index without allocating.
290            pub fn new() -> Self {
291                Self(Bvh::new())
292            }
293
294            /// Number of indexed colliders in the last successful snapshot.
295            pub fn len(&self) -> usize {
296                self.0.entries.len()
297            }
298
299            /// Whether the snapshot is empty.
300            pub fn is_empty(&self) -> bool {
301                self.0.entries.is_empty()
302            }
303
304            /// Copies `(id, bounds)` pairs and atomically replaces the snapshot.
305            /// Invalid input returns its original position and preserves old data.
306            /// Build cost is O(n log n); query cost depends on overlap distribution.
307            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            /// Nearest ray contact within an inclusive world-unit distance.
323            /// Exact-distance ties use original input order. Infinity is allowed.
324            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            /// Visits boxes with positive-volume overlap. Touching alone is excluded.
348            /// Visitor order follows the hierarchy, not input order. No allocation.
349            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            /// Collects positive-volume overlaps for convenience.
366            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            /// Visits boxes that may be visible in this camera. No allocation.
373            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            /// Collects potentially visible boxes for convenience.
386            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);