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}