parry3d/partitioning/bvh/bvh_queries.rs
1use super::{Bvh, BvhNode};
2use crate::bounding_volume::{Aabb, BoundingVolume};
3use crate::math::Point;
4use crate::math::Real;
5use crate::query::PointProjection;
6use crate::query::{PointQuery, Ray};
7use crate::shape::FeatureId;
8
9#[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))]
10pub(super) struct SimdInvRay {
11 // TODO: we need to use `glam` here instead of `wide` because `wide` is lacking
12 // operations for getting the min/max vector element.
13 // We can switch back to `wide` once it's supported.
14 pub origin: glam::Vec3A,
15 pub inv_dir: glam::Vec3A,
16}
17
18#[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))]
19impl From<Ray> for SimdInvRay {
20 fn from(ray: Ray) -> Self {
21 let inv_dir = ray.dir.map(|r| {
22 if r.abs() < Real::EPSILON {
23 r.signum() / Real::EPSILON
24 } else {
25 1.0 / r
26 }
27 });
28 Self {
29 origin: glam::Vec3A::from([ray.origin.x, ray.origin.y, ray.origin.z]),
30 inv_dir: glam::Vec3A::from([inv_dir.x, inv_dir.y, inv_dir.z]),
31 }
32 }
33}
34
35impl Bvh {
36 /// Returns an iterator over all leaves whose AABBs intersect the given AABB.
37 ///
38 /// This efficiently finds all objects in the scene that potentially overlap with a
39 /// query region. It's commonly used for:
40 /// - Finding objects in a specific area
41 /// - Broad-phase collision detection (find candidate pairs)
42 /// - Spatial queries (what's near this position?)
43 /// - Frustum culling (what's in the camera's view?)
44 ///
45 /// The iterator returns leaf indices corresponding to objects that were added to the BVH.
46 ///
47 /// # Arguments
48 ///
49 /// * `aabb` - The axis-aligned bounding box to test for intersections
50 ///
51 /// # Returns
52 ///
53 /// An iterator yielding `u32` leaf indices for all leaves whose AABBs intersect the
54 /// query AABB. The iteration order is depth-first through the tree.
55 ///
56 /// # Performance
57 ///
58 /// - **Time**: O(log n) average case with good spatial locality
59 /// - **Worst case**: O(n) if the query AABB is very large
60 /// - Efficiently prunes entire subtrees that don't intersect
61 /// - Zero allocations
62 ///
63 /// # Examples
64 ///
65 /// ## Find objects in a region
66 ///
67 /// ```
68 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
69 /// use parry3d::partitioning::{Bvh, BvhBuildStrategy};
70 /// use parry3d::bounding_volume::Aabb;
71 /// use nalgebra::Point3;
72 ///
73 /// // Create a scene with objects
74 /// let objects = vec![
75 /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)),
76 /// Aabb::new(Point3::new(5.0, 0.0, 0.0), Point3::new(6.0, 1.0, 1.0)),
77 /// Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)),
78 /// ];
79 ///
80 /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &objects);
81 ///
82 /// // Query: which objects are near the origin?
83 /// let query_region = Aabb::new(
84 /// Point3::new(-1.0, -1.0, -1.0),
85 /// Point3::new(2.0, 2.0, 2.0)
86 /// );
87 ///
88 /// for leaf_id in bvh.intersect_aabb(&query_region) {
89 /// println!("Object {} is in the query region", leaf_id);
90 /// // You can now test the actual geometry of objects[leaf_id]
91 /// }
92 /// # }
93 /// ```
94 ///
95 /// ## Broad-phase collision detection
96 ///
97 /// ```
98 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
99 /// use parry3d::partitioning::{Bvh, BvhBuildStrategy};
100 /// use parry3d::bounding_volume::Aabb;
101 /// use nalgebra::Point3;
102 ///
103 /// let objects = vec![
104 /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)),
105 /// Aabb::new(Point3::new(0.5, 0.0, 0.0), Point3::new(1.5, 1.0, 1.0)), // Overlaps first
106 /// Aabb::new(Point3::new(10.0, 0.0, 0.0), Point3::new(11.0, 1.0, 1.0)), // Far away
107 /// ];
108 ///
109 /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &objects);
110 ///
111 /// // Find potential collision pairs
112 /// let mut collision_pairs = Vec::new();
113 ///
114 /// for (i, aabb) in objects.iter().enumerate() {
115 /// for leaf_id in bvh.intersect_aabb(aabb) {
116 /// if leaf_id as usize > i { // Avoid duplicate pairs
117 /// collision_pairs.push((i as u32, leaf_id));
118 /// }
119 /// }
120 /// }
121 ///
122 /// // collision_pairs now contains potential collision pairs
123 /// // Next step: narrow-phase test actual geometry
124 /// # }
125 /// ```
126 ///
127 /// ## Frustum culling for rendering
128 ///
129 /// ```
130 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
131 /// use parry3d::partitioning::{Bvh, BvhBuildStrategy};
132 /// use parry3d::bounding_volume::Aabb;
133 /// use nalgebra::Point3;
134 ///
135 /// let scene_objects = vec![
136 /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)),
137 /// Aabb::new(Point3::new(2.0, 0.0, 0.0), Point3::new(3.0, 1.0, 1.0)),
138 /// Aabb::new(Point3::new(100.0, 0.0, 0.0), Point3::new(101.0, 1.0, 1.0)), // Far away
139 /// ];
140 ///
141 /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &scene_objects);
142 ///
143 /// // Camera frustum as an AABB (simplified - real frustums are more complex)
144 /// let camera_frustum = Aabb::new(
145 /// Point3::new(-10.0, -10.0, -10.0),
146 /// Point3::new(10.0, 10.0, 10.0)
147 /// );
148 ///
149 /// let mut visible_objects = Vec::new();
150 /// for leaf_id in bvh.intersect_aabb(&camera_frustum) {
151 /// visible_objects.push(leaf_id);
152 /// }
153 ///
154 /// // Only render visible_objects - massive performance win!
155 /// assert_eq!(visible_objects.len(), 2); // Far object culled
156 /// # }
157 /// ```
158 ///
159 /// ## Finding neighbors for an object
160 ///
161 /// ```
162 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
163 /// use parry3d::partitioning::{Bvh, BvhBuildStrategy};
164 /// use parry3d::bounding_volume::{Aabb, BoundingVolume};
165 /// use nalgebra::Point3;
166 ///
167 /// let objects = vec![
168 /// Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0)),
169 /// Aabb::new(Point3::new(1.5, 0.0, 0.0), Point3::new(2.5, 1.0, 1.0)),
170 /// Aabb::new(Point3::new(100.0, 0.0, 0.0), Point3::new(101.0, 1.0, 1.0)),
171 /// ];
172 ///
173 /// let bvh = Bvh::from_leaves(BvhBuildStrategy::default(), &objects);
174 ///
175 /// // Expand an object's AABB to find nearby objects
176 /// let search_radius = 2.0;
177 /// let expanded = objects[0].loosened(search_radius);
178 ///
179 /// let mut neighbors = Vec::new();
180 /// for leaf_id in bvh.intersect_aabb(&expanded) {
181 /// if leaf_id != 0 { // Don't include the object itself
182 /// neighbors.push(leaf_id);
183 /// }
184 /// }
185 ///
186 /// // neighbors contains objects within search_radius of object 0
187 /// # }
188 /// ```
189 ///
190 /// # Notes
191 ///
192 /// - This only tests AABB-AABB intersection, which is conservative (may have false positives)
193 /// - For exact collision detection, test actual geometry after getting candidates
194 /// - The iterator is lazy - work is only done as you consume items
195 /// - Empty queries (no intersections) are very fast due to early pruning
196 ///
197 /// # See Also
198 ///
199 /// - [`cast_ray`](Self::cast_ray) - Ray casting queries
200 /// - [`project_point`](Self::project_point) - Find closest point
201 /// - [`traverse`](Self::traverse) - Custom traversal logic
202 /// - [`leaves`](Self::leaves) - General leaf iteration with predicate
203 pub fn intersect_aabb<'a>(&'a self, aabb: &'a Aabb) -> impl Iterator<Item = u32> + 'a {
204 self.leaves(|node: &BvhNode| node.aabb().intersects(aabb))
205 }
206
207 /// Projects a point on this BVH using the provided leaf projection function.
208 ///
209 /// The `primitive_check` delegates the point-projection task to an external function that
210 /// is assumed to map a leaf index to an actual geometry to project on. The `Real` argument
211 /// given to that closure is the distance to the closest point found so far (or is equal to
212 /// `max_distance` if no projection was found so far).
213 pub fn project_point(
214 &self,
215 point: &Point<Real>,
216 max_distance: Real,
217 primitive_check: impl Fn(u32, Real) -> Option<PointProjection>,
218 ) -> Option<(u32, (Real, PointProjection))> {
219 self.find_best(
220 max_distance,
221 |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true),
222 |primitive, _| {
223 let proj = primitive_check(primitive, max_distance)?;
224 Some((na::distance(&proj.point, point), proj))
225 },
226 )
227 }
228
229 /// Projects a point on this BVH using the provided leaf projection function.
230 ///
231 /// Also returns the feature the point was projected on.
232 ///
233 /// The `primitive_check` delegates the point-projection task to an external function that
234 /// is assumed to map a leaf index to an actual geometry to project on. The `Real` argument
235 /// given to that closure is the distance to the closest point found so far (or is equal to
236 /// `max_distance` if no projection was found so far).
237 pub fn project_point_and_get_feature(
238 &self,
239 point: &Point<Real>,
240 max_distance: Real,
241 primitive_check: impl Fn(u32, Real) -> Option<(PointProjection, FeatureId)>,
242 ) -> Option<(u32, (Real, (PointProjection, FeatureId)))> {
243 self.find_best(
244 max_distance,
245 |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true),
246 |primitive, _| {
247 let proj = primitive_check(primitive, max_distance)?;
248 Some((na::distance(&proj.0.point, point), proj))
249 },
250 )
251 }
252
253 /// Casts a ray on this BVH using the provided leaf ray-cast function.
254 ///
255 /// The `primitive_check` delegates the ray-casting task to an external function that
256 /// is assumed to map a leaf index to an actual geometry to cast a ray on. The `Real` argument
257 /// given to that closure is the distance to the closest ray hit found so far (or is equal to
258 /// `max_time_of_impact` if no projection was found so far).
259 #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))]
260 pub fn cast_ray(
261 &self,
262 ray: &Ray,
263 max_time_of_impact: Real,
264 primitive_check: impl Fn(u32, Real) -> Option<Real>,
265 ) -> Option<(u32, Real)> {
266 self.find_best(
267 max_time_of_impact,
268 |node: &BvhNode, best_so_far| node.cast_ray(ray, best_so_far),
269 primitive_check,
270 )
271 }
272
273 /// Casts a ray on this BVH using the provided leaf ray-cast function.
274 ///
275 /// The `primitive_check` delegates the ray-casting task to an external function that
276 /// is assumed to map a leaf index to an actual geometry to cast a ray on. The `Real` argument
277 /// given to that closure is the distance to the closest ray hit found so far (or is equal to
278 /// `max_time_of_impact` if no projection was found so far).
279 #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))]
280 pub fn cast_ray(
281 &self,
282 ray: &Ray,
283 max_time_of_impact: Real,
284 primitive_check: impl Fn(u32, Real) -> Option<Real>,
285 ) -> Option<(u32, Real)> {
286 // The commented code below relies on depth-first traversal instead of
287 // depth first. Interesting to compare both approaches (the best-first is
288 // expected to be consistently faster though).
289 //
290 // let simd_inv_ray = SimdInvRay::from(*ray);
291 // let mut best_primitive = u32::MAX;
292 // let mut best_toi = Real::MAX;
293 // self.traverse(|node: &BvhNode| {
294 // if node.cast_inv_ray_simd(&simd_inv_ray) >= best_toi {
295 // return TraversalAction::Prune;
296 // }
297 //
298 // if let Some(primitive) = node.leaf_data() {
299 // let toi = primitive_check(primitive as u32, best_toi);
300 // if toi < best_toi {
301 // best_primitive = primitive as u32;
302 // best_toi = toi;
303 // }
304 // }
305 //
306 // TraversalAction::Continue
307 // });
308 // (best_primitive, best_toi)
309
310 let simd_inv_ray = SimdInvRay::from(*ray);
311 self.find_best(
312 max_time_of_impact,
313 |node: &BvhNode, _best_so_far| node.cast_inv_ray_simd(&simd_inv_ray),
314 |primitive, best_so_far| primitive_check(primitive, best_so_far),
315 )
316 }
317}