avian3d/collider_tree/
traverse.rs

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

avian3d/collider_tree/
traverse.rs
