avian3d/spatial_query/pipeline.rs
1use alloc::sync::Arc;
2
3use crate::prelude::*;
4use bevy::prelude::*;
5use parry::{
6 bounding_volume::Aabb,
7 math::Isometry,
8 partitioning::Qbvh,
9 query::{
10 details::{
11 NormalConstraints, RayCompositeShapeToiAndNormalBestFirstVisitor,
12 TOICompositeShapeShapeBestFirstVisitor,
13 },
14 point::PointCompositeShapeProjBestFirstVisitor,
15 visitors::{
16 BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor,
17 },
18 DefaultQueryDispatcher, QueryDispatcher, ShapeCastOptions,
19 },
20 shape::{Shape, TypedSimdCompositeShape},
21};
22
23// TODO: It'd be nice not to store so much duplicate data.
24// Should we just query the ECS?
25#[derive(Clone)]
26pub(crate) struct QbvhProxyData {
27 pub entity: Entity,
28 pub isometry: Isometry<Scalar>,
29 pub collider: Collider,
30 pub layers: CollisionLayers,
31}
32
33/// A resource for the spatial query pipeline.
34///
35/// The pipeline maintains a quaternary bounding volume hierarchy `Qbvh` of the world's colliders
36/// as an acceleration structure for spatial queries.
37#[derive(Resource, Clone)]
38pub struct SpatialQueryPipeline {
39 pub(crate) qbvh: Qbvh<u32>,
40 pub(crate) dispatcher: Arc<dyn QueryDispatcher>,
41 // TODO: Store the proxies as `Qbvh` leaf data.
42 pub(crate) proxies: Vec<QbvhProxyData>,
43}
44
45impl Default for SpatialQueryPipeline {
46 fn default() -> Self {
47 Self {
48 qbvh: Qbvh::new(),
49 dispatcher: Arc::new(DefaultQueryDispatcher),
50 proxies: Vec::default(),
51 }
52 }
53}
54
55impl SpatialQueryPipeline {
56 /// Creates a new [`SpatialQueryPipeline`].
57 pub fn new() -> SpatialQueryPipeline {
58 SpatialQueryPipeline::default()
59 }
60
61 pub(crate) fn as_composite_shape<'a>(
62 &'a self,
63 query_filter: &'a SpatialQueryFilter,
64 ) -> QueryPipelineAsCompositeShape<'a> {
65 QueryPipelineAsCompositeShape {
66 pipeline: self,
67 proxies: &self.proxies,
68 query_filter,
69 }
70 }
71
72 pub(crate) fn as_composite_shape_with_predicate<'a: 'b, 'b>(
73 &'a self,
74 query_filter: &'a SpatialQueryFilter,
75 predicate: &'a dyn Fn(Entity) -> bool,
76 ) -> QueryPipelineAsCompositeShapeWithPredicate<'a, 'b> {
77 QueryPipelineAsCompositeShapeWithPredicate {
78 pipeline: self,
79 proxies: &self.proxies,
80 query_filter,
81 predicate,
82 }
83 }
84
85 /// Updates the associated acceleration structures with a new set of entities.
86 pub fn update<'a>(
87 &mut self,
88 colliders: impl Iterator<
89 Item = (
90 Entity,
91 &'a Position,
92 &'a Rotation,
93 &'a Collider,
94 &'a CollisionLayers,
95 ),
96 >,
97 ) {
98 self.update_internal(
99 colliders.map(
100 |(entity, position, rotation, collider, layers)| QbvhProxyData {
101 entity,
102 isometry: make_isometry(position.0, *rotation),
103 collider: collider.clone(),
104 layers: *layers,
105 },
106 ),
107 )
108 }
109
110 // TODO: Incremental updates.
111 fn update_internal(&mut self, proxies: impl Iterator<Item = QbvhProxyData>) {
112 self.proxies.clear();
113 self.proxies.extend(proxies);
114
115 struct DataGenerator<'a>(&'a Vec<QbvhProxyData>);
116
117 impl parry::partitioning::QbvhDataGenerator<u32> for DataGenerator<'_> {
118 fn size_hint(&self) -> usize {
119 self.0.len()
120 }
121
122 #[inline(always)]
123 fn for_each(&mut self, mut f: impl FnMut(u32, parry::bounding_volume::Aabb)) {
124 for (i, proxy) in self.0.iter().enumerate() {
125 // Compute and return AABB
126 let aabb = proxy.collider.shape_scaled().compute_aabb(&proxy.isometry);
127 f(i as u32, aabb)
128 }
129 }
130 }
131
132 self.qbvh
133 .clear_and_rebuild(DataGenerator(&self.proxies), 0.01);
134 }
135
136 /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
137 /// If there are no hits, `None` is returned.
138 ///
139 /// # Arguments
140 ///
141 /// - `origin`: Where the ray is cast from.
142 /// - `direction`: What direction the ray is cast in.
143 /// - `max_distance`: The maximum distance the ray can travel.
144 /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
145 /// Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
146 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
147 ///
148 /// # Related Methods
149 ///
150 /// - [`SpatialQueryPipeline::cast_ray_predicate`]
151 /// - [`SpatialQueryPipeline::ray_hits`]
152 /// - [`SpatialQueryPipeline::ray_hits_callback`]
153 pub fn cast_ray(
154 &self,
155 origin: Vector,
156 direction: Dir,
157 max_distance: Scalar,
158 solid: bool,
159 filter: &SpatialQueryFilter,
160 ) -> Option<RayHitData> {
161 self.cast_ray_predicate(origin, direction, max_distance, solid, filter, &|_| true)
162 }
163
164 /// Casts a [ray](spatial_query#raycasting) and computes the closest [hit](RayHitData) with a collider.
165 /// If there are no hits, `None` is returned.
166 ///
167 /// # Arguments
168 ///
169 /// - `origin`: Where the ray is cast from.
170 /// - `direction`: What direction the ray is cast in.
171 /// - `max_distance`: The maximum distance the ray can travel.
172 /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
173 /// Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
174 /// - `predicate`: A function called on each entity hit by the ray. The ray keeps travelling until the predicate returns `true`.
175 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
176 ///
177 /// # Related Methods
178 ///
179 /// - [`SpatialQueryPipeline::cast_ray`]
180 /// - [`SpatialQueryPipeline::ray_hits`]
181 /// - [`SpatialQueryPipeline::ray_hits_callback`]
182 pub fn cast_ray_predicate(
183 &self,
184 origin: Vector,
185 direction: Dir,
186 max_distance: Scalar,
187 solid: bool,
188 filter: &SpatialQueryFilter,
189 predicate: &dyn Fn(Entity) -> bool,
190 ) -> Option<RayHitData> {
191 let pipeline_shape = self.as_composite_shape_with_predicate(filter, predicate);
192 let ray = parry::query::Ray::new(origin.into(), direction.adjust_precision().into());
193 let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new(
194 &pipeline_shape,
195 &ray,
196 max_distance,
197 solid,
198 );
199
200 self.qbvh
201 .traverse_best_first(&mut visitor)
202 .map(|(_, (index, hit))| RayHitData {
203 entity: self.proxies[index as usize].entity,
204 distance: hit.time_of_impact,
205 normal: hit.normal.into(),
206 })
207 }
208
209 /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData) until `max_hits` is reached.
210 ///
211 /// Note that the order of the results is not guaranteed, and if there are more hits than `max_hits`,
212 /// some hits will be missed.
213 ///
214 /// # Arguments
215 ///
216 /// - `origin`: Where the ray is cast from.
217 /// - `direction`: What direction the ray is cast in.
218 /// - `max_distance`: The maximum distance the ray can travel.
219 /// - `max_hits`: The maximum number of hits. Additional hits will be missed.
220 /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
221 /// Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
222 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
223 ///
224 /// # Related Methods
225 ///
226 /// - [`SpatialQueryPipeline::cast_ray`]
227 /// - [`SpatialQueryPipeline::cast_ray_predicate`]
228 /// - [`SpatialQueryPipeline::ray_hits_callback`]
229 pub fn ray_hits(
230 &self,
231 origin: Vector,
232 direction: Dir,
233 max_distance: Scalar,
234 max_hits: u32,
235 solid: bool,
236 filter: &SpatialQueryFilter,
237 ) -> Vec<RayHitData> {
238 let mut hits = Vec::with_capacity(10);
239 self.ray_hits_callback(origin, direction, max_distance, solid, filter, |hit| {
240 hits.push(hit);
241 (hits.len() as u32) < max_hits
242 });
243 hits
244 }
245
246 /// Casts a [ray](spatial_query#raycasting) and computes all [hits](RayHitData), calling the given `callback`
247 /// for each hit. The raycast stops when `callback` returns false or all hits have been found.
248 ///
249 /// Note that the order of the results is not guaranteed.
250 ///
251 /// # Arguments
252 ///
253 /// - `origin`: Where the ray is cast from.
254 /// - `direction`: What direction the ray is cast in.
255 /// - `max_distance`: The maximum distance the ray can travel.
256 /// - `solid`: If true *and* the ray origin is inside of a collider, the hit point will be the ray origin itself.
257 /// Otherwise, the collider will be treated as hollow, and the hit point will be at its boundary.
258 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
259 /// - `callback`: A callback function called for each hit.
260 ///
261 /// # Related Methods
262 ///
263 /// - [`SpatialQueryPipeline::cast_ray`]
264 /// - [`SpatialQueryPipeline::cast_ray_predicate`]
265 /// - [`SpatialQueryPipeline::ray_hits`]
266 pub fn ray_hits_callback(
267 &self,
268 origin: Vector,
269 direction: Dir,
270 max_distance: Scalar,
271 solid: bool,
272 filter: &SpatialQueryFilter,
273 mut callback: impl FnMut(RayHitData) -> bool,
274 ) {
275 let proxies = &self.proxies;
276
277 let ray = parry::query::Ray::new(origin.into(), direction.adjust_precision().into());
278
279 let mut leaf_callback = &mut |index: &u32| {
280 if let Some(proxy) = proxies.get(*index as usize) {
281 if filter.test(proxy.entity, proxy.layers) {
282 if let Some(hit) = proxy.collider.shape_scaled().cast_ray_and_get_normal(
283 &proxy.isometry,
284 &ray,
285 max_distance,
286 solid,
287 ) {
288 let hit = RayHitData {
289 entity: proxy.entity,
290 distance: hit.time_of_impact,
291 normal: hit.normal.into(),
292 };
293
294 return callback(hit);
295 }
296 }
297 }
298 true
299 };
300
301 let mut visitor = RayIntersectionsVisitor::new(&ray, max_distance, &mut leaf_callback);
302 self.qbvh.traverse_depth_first(&mut visitor);
303 }
304
305 /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes the closest [hit](ShapeHits)
306 /// with a collider. If there are no hits, `None` is returned.
307 ///
308 /// For a more ECS-based approach, consider using the [`ShapeCaster`] component instead.
309 ///
310 /// # Arguments
311 ///
312 /// - `shape`: The shape being cast represented as a [`Collider`].
313 /// - `origin`: Where the shape is cast from.
314 /// - `shape_rotation`: The rotation of the shape being cast.
315 /// - `direction`: What direction the shape is cast in.
316 /// - `config`: A [`ShapeCastConfig`] that determines the behavior of the cast.
317 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
318 ///
319 /// # Related Methods
320 ///
321 /// - [`SpatialQueryPipeline::cast_shape_predicate`]
322 /// - [`SpatialQueryPipeline::shape_hits`]
323 /// - [`SpatialQueryPipeline::shape_hits_callback`]
324 #[allow(clippy::too_many_arguments)]
325 pub fn cast_shape(
326 &self,
327 shape: &Collider,
328 origin: Vector,
329 shape_rotation: RotationValue,
330 direction: Dir,
331 config: &ShapeCastConfig,
332 filter: &SpatialQueryFilter,
333 ) -> Option<ShapeHitData> {
334 self.cast_shape_predicate(
335 shape,
336 origin,
337 shape_rotation,
338 direction,
339 config,
340 filter,
341 &|_| true,
342 )
343 }
344
345 /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes the closest [hit](ShapeHits)
346 /// with a collider. If there are no hits, `None` is returned.
347 ///
348 /// For a more ECS-based approach, consider using the [`ShapeCaster`] component instead.
349 ///
350 /// # Arguments
351 ///
352 /// - `shape`: The shape being cast represented as a [`Collider`].
353 /// - `origin`: Where the shape is cast from.
354 /// - `shape_rotation`: The rotation of the shape being cast.
355 /// - `direction`: What direction the shape is cast in.
356 /// - `config`: A [`ShapeCastConfig`] that determines the behavior of the cast.
357 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
358 /// - `predicate`: A function called on each entity hit by the shape. The shape keeps travelling until the predicate returns `true`.
359 ///
360 /// # Related Methods
361 ///
362 /// - [`SpatialQueryPipeline::cast_shape`]
363 /// - [`SpatialQueryPipeline::shape_hits`]
364 /// - [`SpatialQueryPipeline::shape_hits_callback`]
365 #[allow(clippy::too_many_arguments)]
366 pub fn cast_shape_predicate(
367 &self,
368 shape: &Collider,
369 origin: Vector,
370 shape_rotation: RotationValue,
371 direction: Dir,
372 config: &ShapeCastConfig,
373 filter: &SpatialQueryFilter,
374 predicate: &dyn Fn(Entity) -> bool,
375 ) -> Option<ShapeHitData> {
376 let rotation: Rotation;
377 #[cfg(feature = "2d")]
378 {
379 rotation = Rotation::radians(shape_rotation);
380 }
381 #[cfg(feature = "3d")]
382 {
383 rotation = Rotation::from(shape_rotation);
384 }
385
386 let shape_isometry = make_isometry(origin, rotation);
387 let shape_direction = direction.adjust_precision().into();
388 let pipeline_shape = self.as_composite_shape_with_predicate(filter, predicate);
389 let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
390 &*self.dispatcher,
391 &shape_isometry,
392 &shape_direction,
393 &pipeline_shape,
394 shape.shape_scaled().as_ref(),
395 ShapeCastOptions {
396 max_time_of_impact: config.max_distance,
397 stop_at_penetration: !config.ignore_origin_penetration,
398 compute_impact_geometry_on_penetration: config.compute_contact_on_penetration,
399 ..default()
400 },
401 );
402
403 self.qbvh
404 .traverse_best_first(&mut visitor)
405 .map(|(_, (index, hit))| ShapeHitData {
406 entity: self.proxies[index as usize].entity,
407 distance: hit.time_of_impact,
408 point1: hit.witness1.into(),
409 point2: hit.witness2.into(),
410 normal1: hit.normal1.into(),
411 normal2: hit.normal2.into(),
412 })
413 }
414
415 /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes computes all [hits](ShapeHitData)
416 /// in the order of distance until `max_hits` is reached.
417 ///
418 /// # Arguments
419 ///
420 /// - `shape`: The shape being cast represented as a [`Collider`].
421 /// - `origin`: Where the shape is cast from.
422 /// - `shape_rotation`: The rotation of the shape being cast.
423 /// - `direction`: What direction the shape is cast in.
424 /// - `max_hits`: The maximum number of hits. Additional hits will be missed.
425 /// - `config`: A [`ShapeCastConfig`] that determines the behavior of the cast.
426 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
427 ///
428 /// # Related Methods
429 ///
430 /// - [`SpatialQueryPipeline::cast_shape`]
431 /// - [`SpatialQueryPipeline::cast_shape_predicate`]
432 /// - [`SpatialQueryPipeline::shape_hits_callback`]
433 #[allow(clippy::too_many_arguments)]
434 pub fn shape_hits(
435 &self,
436 shape: &Collider,
437 origin: Vector,
438 shape_rotation: RotationValue,
439 direction: Dir,
440 max_hits: u32,
441 config: &ShapeCastConfig,
442 filter: &SpatialQueryFilter,
443 ) -> Vec<ShapeHitData> {
444 let mut hits = Vec::with_capacity(10);
445 self.shape_hits_callback(
446 shape,
447 origin,
448 shape_rotation,
449 direction,
450 config,
451 filter,
452 |hit| {
453 hits.push(hit);
454 (hits.len() as u32) < max_hits
455 },
456 );
457 hits
458 }
459
460 /// Casts a [shape](spatial_query#shapecasting) with a given rotation and computes computes all [hits](ShapeHitData)
461 /// in the order of distance, calling the given `callback` for each hit. The shapecast stops when
462 /// `callback` returns false or all hits have been found.
463 ///
464 /// # Arguments
465 ///
466 /// - `shape`: The shape being cast represented as a [`Collider`].
467 /// - `origin`: Where the shape is cast from.
468 /// - `shape_rotation`: The rotation of the shape being cast.
469 /// - `direction`: What direction the shape is cast in.
470 /// - `config`: A [`ShapeCastConfig`] that determines the behavior of the cast.
471 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
472 /// - `callback`: A callback function called for each hit.
473 ///
474 /// # Related Methods
475 ///
476 /// - [`SpatialQueryPipeline::cast_shape`]
477 /// - [`SpatialQueryPipeline::cast_shape_predicate`]
478 /// - [`SpatialQueryPipeline::shape_hits`]
479 #[allow(clippy::too_many_arguments)]
480 pub fn shape_hits_callback(
481 &self,
482 shape: &Collider,
483 origin: Vector,
484 shape_rotation: RotationValue,
485 direction: Dir,
486 config: &ShapeCastConfig,
487 filter: &SpatialQueryFilter,
488 mut callback: impl FnMut(ShapeHitData) -> bool,
489 ) {
490 // TODO: This clone is here so that the excluded entities in the original `query_filter` aren't modified.
491 // We could remove this if shapecasting could compute multiple hits without just doing casts in a loop.
492 // See https://github.com/Jondolf/avian/issues/403.
493 let mut query_filter = filter.clone();
494
495 let shape_cast_options = ShapeCastOptions {
496 max_time_of_impact: config.max_distance,
497 target_distance: config.target_distance,
498 stop_at_penetration: !config.ignore_origin_penetration,
499 compute_impact_geometry_on_penetration: config.compute_contact_on_penetration,
500 };
501
502 let rotation: Rotation;
503 #[cfg(feature = "2d")]
504 {
505 rotation = Rotation::radians(shape_rotation);
506 }
507 #[cfg(feature = "3d")]
508 {
509 rotation = Rotation::from(shape_rotation);
510 }
511
512 let shape_isometry = make_isometry(origin, rotation);
513 let shape_direction = direction.adjust_precision().into();
514
515 loop {
516 let pipeline_shape = self.as_composite_shape(&query_filter);
517 let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
518 &*self.dispatcher,
519 &shape_isometry,
520 &shape_direction,
521 &pipeline_shape,
522 shape.shape_scaled().as_ref(),
523 shape_cast_options,
524 );
525
526 if let Some(hit) =
527 self.qbvh
528 .traverse_best_first(&mut visitor)
529 .map(|(_, (index, hit))| ShapeHitData {
530 entity: self.proxies[index as usize].entity,
531 distance: hit.time_of_impact,
532 point1: hit.witness1.into(),
533 point2: hit.witness2.into(),
534 normal1: hit.normal1.into(),
535 normal2: hit.normal2.into(),
536 })
537 {
538 query_filter.excluded_entities.insert(hit.entity);
539
540 if !callback(hit) {
541 break;
542 }
543 } else {
544 break;
545 }
546 }
547 }
548
549 /// Finds the [projection](spatial_query#point-projection) of a given point on the closest [collider](Collider).
550 /// If one isn't found, `None` is returned.
551 ///
552 /// # Arguments
553 ///
554 /// - `point`: The point that should be projected.
555 /// - `solid`: If true and the point is inside of a collider, the projection will be at the point.
556 /// Otherwise, the collider will be treated as hollow, and the projection will be at the collider's boundary.
557 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
558 ///
559 /// # Related Methods
560 ///
561 /// - [`SpatialQueryPipeline::project_point_predicate`]
562 pub fn project_point(
563 &self,
564 point: Vector,
565 solid: bool,
566 filter: &SpatialQueryFilter,
567 ) -> Option<PointProjection> {
568 self.project_point_predicate(point, solid, filter, &|_| true)
569 }
570
571 /// Finds the [projection](spatial_query#point-projection) of a given point on the closest [collider](Collider).
572 /// If one isn't found, `None` is returned.
573 ///
574 /// # Arguments
575 ///
576 /// - `point`: The point that should be projected.
577 /// - `solid`: If true and the point is inside of a collider, the projection will be at the point.
578 /// Otherwise, the collider will be treated as hollow, and the projection will be at the collider's boundary.
579 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
580 /// - `predicate`: A function for filtering which entities are considered in the query. The projection will be on the closest collider for which the `predicate` returns `true`
581 ///
582 /// # Related Methods
583 ///
584 /// - [`SpatialQueryPipeline::project_point`]
585 pub fn project_point_predicate(
586 &self,
587 point: Vector,
588 solid: bool,
589 filter: &SpatialQueryFilter,
590 predicate: &dyn Fn(Entity) -> bool,
591 ) -> Option<PointProjection> {
592 let point = point.into();
593 let pipeline_shape = self.as_composite_shape_with_predicate(filter, predicate);
594 let mut visitor =
595 PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, &point, solid);
596
597 self.qbvh
598 .traverse_best_first(&mut visitor)
599 .map(|(_, (projection, index))| PointProjection {
600 entity: self.proxies[index as usize].entity,
601 point: projection.point.into(),
602 is_inside: projection.is_inside,
603 })
604 }
605
606 /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [collider](Collider)
607 /// that contains the given point.
608 ///
609 /// # Arguments
610 ///
611 /// - `point`: The point that intersections are tested against.
612 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
613 ///
614 /// # Related Methods
615 ///
616 /// - [`SpatialQueryPipeline::point_intersections_callback`]
617 pub fn point_intersections(&self, point: Vector, filter: &SpatialQueryFilter) -> Vec<Entity> {
618 let mut intersections = vec![];
619 self.point_intersections_callback(point, filter, |e| {
620 intersections.push(e);
621 true
622 });
623 intersections
624 }
625
626 /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [collider](Collider)
627 /// that contains the given point, calling the given `callback` for each intersection.
628 /// The search stops when `callback` returns `false` or all intersections have been found.
629 ///
630 /// # Arguments
631 ///
632 /// - `point`: The point that intersections are tested against.
633 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
634 /// - `callback`: A callback function called for each intersection.
635 ///
636 /// # Related Methods
637 ///
638 /// - [`SpatialQueryPipeline::point_intersections`]
639 pub fn point_intersections_callback(
640 &self,
641 point: Vector,
642 filter: &SpatialQueryFilter,
643 mut callback: impl FnMut(Entity) -> bool,
644 ) {
645 let point = point.into();
646
647 let mut leaf_callback = &mut |index: &u32| {
648 if let Some(proxy) = self.proxies.get(*index as usize) {
649 if filter.test(proxy.entity, proxy.layers)
650 && proxy
651 .collider
652 .shape_scaled()
653 .contains_point(&proxy.isometry, &point)
654 {
655 return callback(proxy.entity);
656 }
657 }
658 true
659 };
660
661 let mut visitor = PointIntersectionsVisitor::new(&point, &mut leaf_callback);
662 self.qbvh.traverse_depth_first(&mut visitor);
663 }
664
665 /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`ColliderAabb`]
666 /// that is intersecting the given `aabb`.
667 ///
668 /// # Related Methods
669 ///
670 /// - [`SpatialQueryPipeline::aabb_intersections_with_aabb_callback`]
671 pub fn aabb_intersections_with_aabb(&self, aabb: ColliderAabb) -> Vec<Entity> {
672 let mut intersections = vec![];
673 self.aabb_intersections_with_aabb_callback(aabb, |e| {
674 intersections.push(e);
675 true
676 });
677 intersections
678 }
679
680 /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`ColliderAabb`]
681 /// that is intersecting the given `aabb`, calling `callback` for each intersection.
682 /// The search stops when `callback` returns `false` or all intersections have been found.
683 ///
684 /// # Related Methods
685 ///
686 /// - [`SpatialQueryPipeline::aabb_intersections_with_aabb`]
687 pub fn aabb_intersections_with_aabb_callback(
688 &self,
689 aabb: ColliderAabb,
690 mut callback: impl FnMut(Entity) -> bool,
691 ) {
692 let mut leaf_callback = |index: &u32| {
693 let entity = self.proxies[*index as usize].entity;
694 callback(entity)
695 };
696
697 let mut visitor = BoundingVolumeIntersectionsVisitor::new(
698 &Aabb {
699 mins: aabb.min.into(),
700 maxs: aabb.max.into(),
701 },
702 &mut leaf_callback,
703 );
704 self.qbvh.traverse_depth_first(&mut visitor);
705 }
706
707 /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`Collider`]
708 /// that is intersecting the given `shape` with a given position and rotation.
709 ///
710 /// # Arguments
711 ///
712 /// - `shape`: The shape that intersections are tested against represented as a [`Collider`].
713 /// - `shape_position`: The position of the shape.
714 /// - `shape_rotation`: The rotation of the shape.
715 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
716 ///
717 /// # Related Methods
718 ///
719 /// - [`SpatialQueryPipeline::shape_intersections_callback`]
720 pub fn shape_intersections(
721 &self,
722 shape: &Collider,
723 shape_position: Vector,
724 shape_rotation: RotationValue,
725 filter: &SpatialQueryFilter,
726 ) -> Vec<Entity> {
727 let mut intersections = vec![];
728 self.shape_intersections_callback(shape, shape_position, shape_rotation, filter, |e| {
729 intersections.push(e);
730 true
731 });
732 intersections
733 }
734
735 /// An [intersection test](spatial_query#intersection-tests) that finds all entities with a [`Collider`]
736 /// that is intersecting the given `shape` with a given position and rotation, calling `callback` for each
737 /// intersection. The search stops when `callback` returns `false` or all intersections have been found.
738 ///
739 /// # Arguments
740 ///
741 /// - `shape`: The shape that intersections are tested against represented as a [`Collider`].
742 /// - `shape_position`: The position of the shape.
743 /// - `shape_rotation`: The rotation of the shape.
744 /// - `filter`: A [`SpatialQueryFilter`] that determines which colliders are taken into account in the query.
745 /// - `callback`: A callback function called for each intersection.
746 ///
747 /// # Related Methods
748 ///
749 /// - [`SpatialQueryPipeline::shape_intersections`]
750 pub fn shape_intersections_callback(
751 &self,
752 shape: &Collider,
753 shape_position: Vector,
754 shape_rotation: RotationValue,
755 filter: &SpatialQueryFilter,
756 mut callback: impl FnMut(Entity) -> bool,
757 ) {
758 let proxies = &self.proxies;
759 let rotation: Rotation;
760 #[cfg(feature = "2d")]
761 {
762 rotation = Rotation::radians(shape_rotation);
763 }
764 #[cfg(feature = "3d")]
765 {
766 rotation = Rotation::from(shape_rotation);
767 }
768
769 let shape_isometry = make_isometry(shape_position, rotation);
770 let inverse_shape_isometry = shape_isometry.inverse();
771
772 let dispatcher = &*self.dispatcher;
773
774 let mut leaf_callback = &mut |index: &u32| {
775 if let Some(proxy) = proxies.get(*index as usize) {
776 if filter.test(proxy.entity, proxy.layers) {
777 let isometry = inverse_shape_isometry * proxy.isometry;
778
779 if dispatcher.intersection_test(
780 &isometry,
781 shape.shape_scaled().as_ref(),
782 proxy.collider.shape_scaled().as_ref(),
783 ) == Ok(true)
784 {
785 return callback(proxy.entity);
786 }
787 }
788 }
789 true
790 };
791
792 let shape_aabb = shape.shape_scaled().compute_aabb(&shape_isometry);
793 let mut visitor = BoundingVolumeIntersectionsVisitor::new(&shape_aabb, &mut leaf_callback);
794 self.qbvh.traverse_depth_first(&mut visitor);
795 }
796}
797
798pub(crate) struct QueryPipelineAsCompositeShape<'a> {
799 proxies: &'a Vec<QbvhProxyData>,
800 pipeline: &'a SpatialQueryPipeline,
801 query_filter: &'a SpatialQueryFilter,
802}
803
804impl TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'_> {
805 type PartShape = dyn Shape;
806 type PartNormalConstraints = dyn NormalConstraints;
807 type PartId = u32;
808
809 fn map_typed_part_at(
810 &self,
811 shape_id: Self::PartId,
812 mut f: impl FnMut(
813 Option<&Isometry<Scalar>>,
814 &Self::PartShape,
815 Option<&Self::PartNormalConstraints>,
816 ),
817 ) {
818 if let Some(proxy) = self.proxies.get(shape_id as usize) {
819 if self.query_filter.test(proxy.entity, proxy.layers) {
820 f(
821 Some(&proxy.isometry),
822 proxy.collider.shape_scaled().as_ref(),
823 None,
824 );
825 }
826 }
827 }
828
829 fn map_untyped_part_at(
830 &self,
831 shape_id: Self::PartId,
832 f: impl FnMut(Option<&Isometry<Scalar>>, &dyn Shape, Option<&dyn NormalConstraints>),
833 ) {
834 self.map_typed_part_at(shape_id, f);
835 }
836
837 fn typed_qbvh(&self) -> &Qbvh<Self::PartId> {
838 &self.pipeline.qbvh
839 }
840}
841
842pub(crate) struct QueryPipelineAsCompositeShapeWithPredicate<'a, 'b> {
843 proxies: &'a Vec<QbvhProxyData>,
844 pipeline: &'a SpatialQueryPipeline,
845 query_filter: &'a SpatialQueryFilter,
846 predicate: &'b dyn Fn(Entity) -> bool,
847}
848
849impl TypedSimdCompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'_, '_> {
850 type PartShape = dyn Shape;
851 type PartNormalConstraints = dyn NormalConstraints;
852 type PartId = u32;
853
854 fn map_typed_part_at(
855 &self,
856 shape_id: Self::PartId,
857 mut f: impl FnMut(
858 Option<&Isometry<Scalar>>,
859 &Self::PartShape,
860 Option<&Self::PartNormalConstraints>,
861 ),
862 ) {
863 if let Some(proxy) = self.proxies.get(shape_id as usize) {
864 if self.query_filter.test(proxy.entity, proxy.layers) && (self.predicate)(proxy.entity)
865 {
866 f(
867 Some(&proxy.isometry),
868 proxy.collider.shape_scaled().as_ref(),
869 None,
870 );
871 }
872 }
873 }
874
875 fn map_untyped_part_at(
876 &self,
877 shape_id: Self::PartId,
878 f: impl FnMut(Option<&Isometry<Scalar>>, &dyn Shape, Option<&dyn NormalConstraints>),
879 ) {
880 self.map_typed_part_at(shape_id, f);
881 }
882
883 fn typed_qbvh(&self) -> &Qbvh<Self::PartId> {
884 &self.pipeline.qbvh
885 }
886}
887
888/// The result of a [point projection](spatial_query#point-projection) on a [collider](Collider).
889#[derive(Clone, Debug, PartialEq, Reflect)]
890#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
891#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
892#[reflect(Debug, PartialEq)]
893pub struct PointProjection {
894 /// The entity of the collider that the point was projected onto.
895 pub entity: Entity,
896 /// The point where the point was projected.
897 pub point: Vector,
898 /// True if the point was inside of the collider.
899 pub is_inside: bool,
900}