parry2d/partitioning/bvh/bvh_queries.rs
1use super::{Bvh, BvhNode};
2use crate::bounding_volume::Aabb;
3use crate::math::Real;
4use crate::math::Vector;
5use crate::query::PointProjection;
6use crate::query::{PointQuery, Ray};
7use crate::shape::FeatureId;
8
9#[cfg(all(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: glamx::Vec3A,
15 pub inv_dir: glamx::Vec3A,
16}
17
18#[cfg(all(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: glamx::Vec3A::from([ray.origin.x, ray.origin.y, ray.origin.z]),
30 inv_dir: glamx::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 parry3d::math::Vector;
72 ///
73 /// // Create a scene with objects
74 /// let objects = vec![
75 /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)),
76 /// Aabb::new(Vector::new(5.0, 0.0, 0.0), Vector::new(6.0, 1.0, 1.0)),
77 /// Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::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 /// Vector::new(-1.0, -1.0, -1.0),
85 /// Vector::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 parry3d::math::Vector;
102 ///
103 /// let objects = vec![
104 /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)),
105 /// Aabb::new(Vector::new(0.5, 0.0, 0.0), Vector::new(1.5, 1.0, 1.0)), // Overlaps first
106 /// Aabb::new(Vector::new(10.0, 0.0, 0.0), Vector::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 parry3d::math::Vector;
134 ///
135 /// let scene_objects = vec![
136 /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)),
137 /// Aabb::new(Vector::new(2.0, 0.0, 0.0), Vector::new(3.0, 1.0, 1.0)),
138 /// Aabb::new(Vector::new(100.0, 0.0, 0.0), Vector::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 /// Vector::new(-10.0, -10.0, -10.0),
146 /// Vector::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 parry3d::math::Vector;
166 ///
167 /// let objects = vec![
168 /// Aabb::new(Vector::ZERO, Vector::new(1.0, 1.0, 1.0)),
169 /// Aabb::new(Vector::new(1.5, 0.0, 0.0), Vector::new(2.5, 1.0, 1.0)),
170 /// Aabb::new(Vector::new(100.0, 0.0, 0.0), Vector::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 // The query as a node: the node-vs-node test is the SIMD one (same inclusive semantics).
205 let query = BvhNode::leaf(*aabb, 0);
206 self.leaves(move |node: &BvhNode| node.intersects(&query))
207 }
208
209 /// Projects a point on this BVH using the provided leaf projection function.
210 ///
211 /// The `primitive_check` delegates the point-projection task to an external function that
212 /// is assumed to map a leaf index to an actual geometry to project on. The `Real` argument
213 /// given to that closure is the distance to the closest point found so far (or is equal to
214 /// `max_distance` if no projection was found so far).
215 pub fn project_point(
216 &self,
217 point: Vector,
218 max_distance: Real,
219 primitive_check: impl Fn(u32, Real) -> Option<PointProjection>,
220 ) -> Option<(u32, (Real, PointProjection))> {
221 self.find_best(
222 max_distance,
223 |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true),
224 |primitive, _| {
225 let proj = primitive_check(primitive, max_distance)?;
226 Some((proj.point.distance(point), proj))
227 },
228 )
229 }
230
231 /// Projects a point on this BVH using the provided leaf projection function.
232 ///
233 /// Also returns the feature the point was projected on.
234 ///
235 /// The `primitive_check` delegates the point-projection task to an external function that
236 /// is assumed to map a leaf index to an actual geometry to project on. The `Real` argument
237 /// given to that closure is the distance to the closest point found so far (or is equal to
238 /// `max_distance` if no projection was found so far).
239 pub fn project_point_and_get_feature(
240 &self,
241 point: Vector,
242 max_distance: Real,
243 primitive_check: impl Fn(u32, Real) -> Option<(PointProjection, FeatureId)>,
244 ) -> Option<(u32, (Real, (PointProjection, FeatureId)))> {
245 self.find_best(
246 max_distance,
247 |node: &BvhNode, _| node.aabb().distance_to_local_point(point, true),
248 |primitive, _| {
249 let proj = primitive_check(primitive, max_distance)?;
250 Some((proj.0.point.distance(point), proj))
251 },
252 )
253 }
254
255 /// Casts a ray on this BVH using the provided leaf ray-cast function.
256 ///
257 /// The `primitive_check` delegates the ray-casting task to an external function that
258 /// is assumed to map a leaf index to an actual geometry to cast a ray on. The `Real` argument
259 /// given to that closure is the distance to the closest ray hit found so far (or is equal to
260 /// `max_time_of_impact` if no projection was found so far).
261 #[cfg(not(all(feature = "dim3", feature = "f32")))]
262 pub fn cast_ray(
263 &self,
264 ray: &Ray,
265 max_time_of_impact: Real,
266 primitive_check: impl Fn(u32, Real) -> Option<Real>,
267 ) -> Option<(u32, Real)> {
268 self.find_best(
269 max_time_of_impact,
270 |node: &BvhNode, best_so_far| node.cast_ray(ray, best_so_far),
271 primitive_check,
272 )
273 }
274
275 /// Casts a ray on this BVH using the provided leaf ray-cast function.
276 ///
277 /// The `primitive_check` delegates the ray-casting task to an external function that
278 /// is assumed to map a leaf index to an actual geometry to cast a ray on. The `Real` argument
279 /// given to that closure is the distance to the closest ray hit found so far (or is equal to
280 /// `max_time_of_impact` if no projection was found so far).
281 #[cfg(all(feature = "dim3", feature = "f32"))]
282 pub fn cast_ray(
283 &self,
284 ray: &Ray,
285 max_time_of_impact: Real,
286 primitive_check: impl Fn(u32, Real) -> Option<Real>,
287 ) -> Option<(u32, Real)> {
288 // The commented code below relies on depth-first traversal instead of
289 // depth first. Interesting to compare both approaches (the best-first is
290 // expected to be consistently faster though).
291 //
292 // let simd_inv_ray = SimdInvRay::from(*ray);
293 // let mut best_primitive = u32::MAX;
294 // let mut best_toi = Real::MAX;
295 // self.traverse(|node: &BvhNode| {
296 // if node.cast_inv_ray_simd(&simd_inv_ray) >= best_toi {
297 // return TraversalAction::Prune;
298 // }
299 //
300 // if let Some(primitive) = node.leaf_data() {
301 // let toi = primitive_check(primitive as u32, best_toi);
302 // if toi < best_toi {
303 // best_primitive = primitive as u32;
304 // best_toi = toi;
305 // }
306 // }
307 //
308 // TraversalAction::Continue
309 // });
310 // (best_primitive, best_toi)
311
312 let simd_inv_ray = SimdInvRay::from(*ray);
313 self.find_best(
314 max_time_of_impact,
315 |node: &BvhNode, _best_so_far| node.cast_inv_ray_simd(&simd_inv_ray),
316 primitive_check,
317 )
318 }
319}