avian3d/collider_tree/
traverse.rs

1use obvhs::{aabb::Aabb, bvh2::node::Bvh2Node, fast_stack};
2
3use crate::{
4    collider_tree::{
5        Bvh2Ext, ColliderTree, ProxyId,
6        obvhs_ext::{Sweep, SweepHit},
7        obvhs_ray,
8    },
9    math::{Dir, RVector, Ray, ToF32Precision, Vector},
10};
11
12impl ColliderTree {
13    /// Traverses the tree for the closest intersection with the given ray.
14    ///
15    /// # Arguments
16    ///
17    /// - `ray`: The ray to be tested for intersection.
18    /// - `max_distance`: The maximum distance along the ray to consider for intersections.
19    /// - `intersection_fn`: A function that takes a proxy ID, and returns the distance to the intersection with that proxy.
20    ///   This function is called for each potential intersection found during traversal.
21    #[inline(always)]
22    pub fn ray_traverse_closest<F: FnMut(ProxyId) -> f32>(
23        &self,
24        ray: Ray,
25        max_distance: f32,
26        mut intersection_fn: F,
27    ) -> Option<(ProxyId, f32)> {
28        let obvhs_ray = obvhs_ray(&ray, max_distance);
29        let mut hit = obvhs::ray::RayHit::none();
30
31        let found_hit = self
32            .bvh
33            .ray_traverse(obvhs_ray, &mut hit, |_ray, primitive_id| {
34                let proxy_id = ProxyId::new(self.bvh.primitive_indices[primitive_id]);
35                intersection_fn(proxy_id)
36            });
37
38        if found_hit {
39            let proxy_id = ProxyId::new(self.bvh.primitive_indices[hit.primitive_id as usize]);
40            Some((proxy_id, hit.t))
41        } else {
42            None
43        }
44    }
45
46    /// Traverses the tree for all intersections with the given ray.
47    ///
48    /// Terminates when all intersections within `max_distance` have been visited or when `intersection_fn` returns false for an intersection.
49    ///
50    /// # Arguments
51    ///
52    /// - `ray`: The ray to be tested for intersection.
53    /// - `max_distance`: The maximum distance along the ray to consider for intersections.
54    /// - `intersection_fn`: A function that takes a proxy ID, and is called for each potential intersection found during traversal.
55    ///   Return false to halt traversal early.
56    #[inline(always)]
57    pub fn ray_traverse_all<F: FnMut(ProxyId) -> bool>(
58        &self,
59        ray: Ray,
60        max_distance: f32,
61        mut intersection_fn: F,
62    ) {
63        let obvhs_ray = obvhs_ray(&ray, max_distance);
64
65        self.bvh
66            .ray_traverse_anyhit(obvhs_ray, |_ray, primitive_id| {
67                let proxy_id = ProxyId::new(self.bvh.primitive_indices[primitive_id]);
68                intersection_fn(proxy_id);
69            });
70    }
71
72    /// Traverse the BVH by sweeping an AABB along a velocity vector, returning the closest hit.
73    ///
74    /// # Arguments
75    ///
76    /// - `aabb`: The axis-aligned bounding box to be swept.
77    /// - `direction`: The direction along which to sweep the AABB.
78    /// - `target_distance`: The separation distance at which a hit is still considered valid.
79    /// - `max_distance`: The maximum distance along the sweep to consider for intersections.
80    /// - `intersection_fn`: A function that takes a proxy ID, and returns the distance to the intersection with that proxy.
81    ///   This function is called for each potential intersection found during traversal.
82    #[inline(always)]
83    pub fn sweep_traverse_closest<F: FnMut(ProxyId) -> f32>(
84        &self,
85        aabb: Aabb,
86        direction: Dir,
87        max_distance: f32,
88        target_distance: f32,
89        mut intersection_fn: F,
90    ) -> Option<(ProxyId, f32)> {
91        #[cfg(feature = "2d")]
92        let direction = direction.extend(0.0).to_array().into();
93        #[cfg(feature = "3d")]
94        let direction = direction.to_array().into();
95        let sweep = Sweep::new(aabb, direction, target_distance, max_distance);
96
97        let mut hit = SweepHit::none();
98
99        let found_hit = self
100            .bvh
101            .sweep_traverse(sweep, &mut hit, |_sweep, primitive_id| {
102                let proxy_id = ProxyId::new(self.bvh.primitive_indices[primitive_id]);
103                intersection_fn(proxy_id)
104            });
105
106        if found_hit {
107            let proxy_id = ProxyId::new(self.bvh.primitive_indices[hit.primitive_id as usize]);
108            Some((proxy_id, hit.t))
109        } else {
110            None
111        }
112    }
113
114    /// Traverse the BVH by sweeping an AABB along a velocity vector, calling `intersection_fn` for each hit.
115    ///
116    /// # Arguments
117    ///
118    /// - `aabb`: The axis-aligned bounding box to be swept.
119    /// - `direction`: The direction along which to sweep the AABB.
120    /// - `target_distance`: The separation distance at which a hit is still considered valid.
121    /// - `max_distance`: The maximum distance along the sweep to consider for intersections.
122    /// - `intersection_fn`: A function that takes a proxy ID, and is called for each potential intersection found during traversal.
123    ///   Return false to halt traversal early.
124    #[inline(always)]
125    pub fn sweep_traverse_all<F: FnMut(ProxyId) -> bool>(
126        &self,
127        aabb: Aabb,
128        direction: Dir,
129        target_distance: f32,
130        max_distance: f32,
131        mut intersection_fn: F,
132    ) {
133        #[cfg(feature = "2d")]
134        let direction = direction.extend(0.0).to_array().into();
135        #[cfg(feature = "3d")]
136        let direction = direction.to_array().into();
137        let sweep = Sweep::new(aabb, direction, target_distance, max_distance);
138
139        let mut intersect_prims = |node: &Bvh2Node, _sweep: &mut Sweep, _hit: &mut SweepHit| {
140            for primitive_id in node.first_index..node.first_index + node.prim_count {
141                let proxy_id = ProxyId::new(self.bvh.primitive_indices[primitive_id as usize]);
142                intersection_fn(proxy_id);
143            }
144            true
145        };
146
147        let mut hit = SweepHit::none();
148        fast_stack!(u32, (96, 192), self.bvh.max_depth, stack, {
149            self.bvh
150                .sweep_traverse_dynamic(&mut stack, sweep, &mut hit, &mut intersect_prims)
151        });
152    }
153
154    /// Traverse the BVH with a point, returning the closest proxy and its squared distance within `max_distance_squared`.
155    ///
156    /// # Arguments
157    ///
158    /// - `point`: The point to be tested for proximity.
159    /// - `max_distance_squared`: The maximum distance from the point to consider for projections.
160    /// - `eval`: A function that takes a proxy ID and returns the squared distance from the point to that proxy. This function is called for each potential projection found during traversal.
161    #[inline(always)]
162    pub fn squared_distance_traverse_closest<F: FnMut(ProxyId) -> f32>(
163        &self,
164        point: RVector,
165        max_distance_squared: f32,
166        mut eval: F,
167    ) -> Option<(ProxyId, f32)> {
168        #[cfg(feature = "2d")]
169        let point = point.f32().extend(0.0).to_array().into();
170        #[cfg(feature = "3d")]
171        let point = point.f32().to_array().into();
172
173        let closest_leaf = self.bvh.squared_distance_traverse(
174            point,
175            max_distance_squared,
176            |_point, primitive_id| {
177                let proxy_id = ProxyId::new(self.bvh.primitive_indices[primitive_id]);
178                eval(proxy_id)
179            },
180        );
181
182        if let Some((primitive_id, distance_squared)) = closest_leaf {
183            let proxy_id = ProxyId::new(self.bvh.primitive_indices[primitive_id as usize]);
184            Some((proxy_id, distance_squared))
185        } else {
186            None
187        }
188    }
189
190    /// Traverse the BVH with a point, calling `eval` for each intersection.
191    ///
192    /// # Arguments
193    ///
194    /// - `point`: The point to be tested for intersection.
195    /// - `eval`: A function that takes a proxy ID and is called for each potential intersection found during traversal.
196    ///   Return false to halt traversal early.
197    #[inline(always)]
198    pub fn point_traverse<F: FnMut(ProxyId) -> bool>(&self, point: Vector, mut eval: F) {
199        #[cfg(feature = "2d")]
200        let point = point.extend(0.0).to_array().into();
201        #[cfg(feature = "3d")]
202        let point = point.to_array().into();
203
204        self.bvh.point_traverse(point, |bvh, node_index| {
205            let node = &bvh.nodes[node_index as usize];
206            let start = node.first_index as usize;
207            let end = start + node.prim_count as usize;
208
209            for primitive_index in bvh.primitive_indices.iter().take(end).skip(start) {
210                let proxy_id = ProxyId::new(*primitive_index);
211                if !eval(proxy_id) {
212                    return false;
213                }
214            }
215
216            true
217        });
218    }
219
220    /// Traverse the BVH with an AABB, calling `eval` for each intersection.
221    ///
222    /// # Arguments
223    ///
224    /// - `aabb`: The axis-aligned bounding box to be tested for intersection.
225    /// - `eval`: A function that takes a proxy ID and is called for each potential intersection found during traversal.
226    ///   Return false to halt traversal early.
227    #[inline(always)]
228    pub fn aabb_traverse<F: FnMut(ProxyId) -> bool>(&self, aabb: Aabb, mut eval: F) {
229        self.bvh.aabb_traverse(aabb, |bvh, node_index| {
230            let node = &bvh.nodes[node_index as usize];
231            let start = node.first_index as usize;
232            let end = start + node.prim_count as usize;
233
234            for primitive_index in bvh.primitive_indices.iter().take(end).skip(start) {
235                let proxy_id = ProxyId::new(*primitive_index);
236                if !eval(proxy_id) {
237                    return false;
238                }
239            }
240
241            true
242        });
243    }
244}
avian3d/collider_tree/traverse.rs

avian3d/collider_tree/
traverse.rs
