Take SpatialQueryFilter by reference in spatial queries (#402)

# Objective Spatial queries currently take `SpatialQueryFilter` by value and clone it several times. This could sometimes be cloning a pretty hefty amount of data if the `HashSet` of excluded entities is large. ## Solution Take spatial query filters by reference and reduce the amount of cloning. ## Future Work Shapecasting with multiple hits should be optimized to only traverse once instead of doing best-first traversals in a loop. See #403. --- ## Migration Guide Spatial queries performed through `SpatialQuery` now take `SpatialQueryFilter` by reference.
Jondolf · Aug 10, 2024 · 73fe6c7 · 73fe6c7
1 parent 8479826
commit 73fe6c7
Show file tree

Hide file tree

Showing 6 changed files with 49 additions and 41 deletions.
diff --git a/crates/avian3d/examples/cast_ray_predicate.rs b/crates/avian3d/examples/cast_ray_predicate.rs
@@ -171,7 +171,7 @@ fn raycast(
         direction,
         Scalar::MAX,
         true,
-        SpatialQueryFilter::default(),
+        &SpatialQueryFilter::default(),
         &|entity| {
             if let Ok((_, out_of_glass)) = cubes.get(entity) {
                 return !out_of_glass.0; // only look at cubes not out of glass

diff --git a/src/spatial_query/mod.rs b/src/spatial_query/mod.rs
@@ -433,8 +433,8 @@ fn update_shape_caster_positions(
 }
 
 #[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
-fn raycast(mut rays: Query<(Entity, &RayCaster, &mut RayHits)>, spatial_query: SpatialQuery) {
-    for (entity, ray, mut hits) in &mut rays {
+fn raycast(mut rays: Query<(Entity, &mut RayCaster, &mut RayHits)>, spatial_query: SpatialQuery) {
+    for (entity, mut ray, mut hits) in &mut rays {
         if ray.enabled {
             ray.cast(entity, &mut hits, &spatial_query.query_pipeline);
         } else if !hits.is_empty() {

diff --git a/src/spatial_query/pipeline.rs b/src/spatial_query/pipeline.rs
@@ -49,9 +49,9 @@ impl SpatialQueryPipeline {
         SpatialQueryPipeline::default()
     }
 
-    pub(crate) fn as_composite_shape(
-        &self,
-        query_filter: SpatialQueryFilter,
+    pub(crate) fn as_composite_shape<'a>(
+        &'a self,
+        query_filter: &'a SpatialQueryFilter,
     ) -> QueryPipelineAsCompositeShape {
         QueryPipelineAsCompositeShape {
             pipeline: self,
@@ -62,7 +62,7 @@ impl SpatialQueryPipeline {
 
     pub(crate) fn as_composite_shape_with_predicate<'a>(
         &'a self,
-        query_filter: SpatialQueryFilter,
+        query_filter: &'a SpatialQueryFilter,
         predicate: &'a dyn Fn(Entity) -> bool,
     ) -> QueryPipelineAsCompositeShapeWithPredicate {
         QueryPipelineAsCompositeShapeWithPredicate {
@@ -167,7 +167,7 @@ impl SpatialQueryPipeline {
         direction: Dir,
         max_time_of_impact: Scalar,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Option<RayHitData> {
         let pipeline_shape = self.as_composite_shape(query_filter);
         let ray = parry::query::Ray::new(origin.into(), direction.adjust_precision().into());
@@ -208,7 +208,7 @@ impl SpatialQueryPipeline {
         direction: Dir,
         max_time_of_impact: Scalar,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         predicate: &dyn Fn(Entity) -> bool,
     ) -> Option<RayHitData> {
         let pipeline_shape = self.as_composite_shape_with_predicate(query_filter, predicate);
@@ -252,7 +252,7 @@ impl SpatialQueryPipeline {
         max_time_of_impact: Scalar,
         max_hits: u32,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<RayHitData> {
         let mut hits = Vec::with_capacity(10);
         self.ray_hits_callback(
@@ -291,7 +291,7 @@ impl SpatialQueryPipeline {
         direction: Dir,
         max_time_of_impact: Scalar,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         mut callback: impl FnMut(RayHitData) -> bool,
     ) {
         let colliders = &self.colliders;
@@ -353,7 +353,7 @@ impl SpatialQueryPipeline {
         direction: Dir,
         max_time_of_impact: Scalar,
         ignore_origin_penetration: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Option<ShapeHitData> {
         let rotation: Rotation;
         #[cfg(feature = "2d")]
@@ -421,7 +421,7 @@ impl SpatialQueryPipeline {
         max_time_of_impact: Scalar,
         max_hits: u32,
         ignore_origin_penetration: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<ShapeHitData> {
         let mut hits = Vec::with_capacity(10);
         self.shape_hits_callback(
@@ -467,9 +467,14 @@ impl SpatialQueryPipeline {
         direction: Dir,
         max_time_of_impact: Scalar,
         ignore_origin_penetration: bool,
-        mut query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         mut callback: impl FnMut(ShapeHitData) -> bool,
     ) {
+        // TODO: This clone is here so that the excluded entities in the original `query_filter` aren't modified.
+        //       We could remove this if shapecasting could compute multiple hits without just doing casts in a loop.
+        //       See https://github.com/Jondolf/avian/issues/403.
+        let mut query_filter = query_filter.clone();
+
         let rotation: Rotation;
         #[cfg(feature = "2d")]
         {
@@ -484,7 +489,7 @@ impl SpatialQueryPipeline {
         let shape_direction = direction.adjust_precision().into();
 
         loop {
-            let pipeline_shape = self.as_composite_shape(query_filter.clone());
+            let pipeline_shape = self.as_composite_shape(&query_filter);
             let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
                 &*self.dispatcher,
                 &shape_isometry,
@@ -536,7 +541,7 @@ impl SpatialQueryPipeline {
         &self,
         point: Vector,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Option<PointProjection> {
         let point = point.into();
         let pipeline_shape = self.as_composite_shape(query_filter);
@@ -564,7 +569,7 @@ impl SpatialQueryPipeline {
     pub fn point_intersections(
         &self,
         point: Vector,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<Entity> {
         let mut intersections = vec![];
         self.point_intersections_callback(point, query_filter, |e| {
@@ -588,7 +593,7 @@ impl SpatialQueryPipeline {
     pub fn point_intersections_callback(
         &self,
         point: Vector,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         mut callback: impl FnMut(Entity) -> bool,
     ) {
         let point = point.into();
@@ -663,7 +668,7 @@ impl SpatialQueryPipeline {
         shape: &Collider,
         shape_position: Vector,
         shape_rotation: RotationValue,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<Entity> {
         let mut intersections = vec![];
         self.shape_intersections_callback(
@@ -697,7 +702,7 @@ impl SpatialQueryPipeline {
         shape: &Collider,
         shape_position: Vector,
         shape_rotation: RotationValue,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         mut callback: impl FnMut(Entity) -> bool,
     ) {
         let colliders = &self.colliders;
@@ -745,7 +750,7 @@ impl SpatialQueryPipeline {
 pub(crate) struct QueryPipelineAsCompositeShape<'a> {
     colliders: &'a HashMap<Entity, (Isometry<Scalar>, Collider, CollisionLayers)>,
     pipeline: &'a SpatialQueryPipeline,
-    query_filter: SpatialQueryFilter,
+    query_filter: &'a SpatialQueryFilter,
 }
 
 impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
@@ -790,7 +795,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
 pub(crate) struct QueryPipelineAsCompositeShapeWithPredicate<'a, 'b> {
     colliders: &'a HashMap<Entity, (Isometry<Scalar>, Collider, CollisionLayers)>,
     pipeline: &'a SpatialQueryPipeline,
-    query_filter: SpatialQueryFilter,
+    query_filter: &'a SpatialQueryFilter,
     predicate: &'b dyn Fn(Entity) -> bool,
 }
 

diff --git a/src/spatial_query/ray_caster.rs b/src/spatial_query/ray_caster.rs
@@ -232,21 +232,21 @@ impl RayCaster {
         any(feature = "parry-f32", feature = "parry-f64")
     ))]
     pub(crate) fn cast(
-        &self,
+        &mut self,
         caster_entity: Entity,
         hits: &mut RayHits,
         query_pipeline: &SpatialQueryPipeline,
     ) {
-        let mut query_filter = self.query_filter.clone();
-
         if self.ignore_self {
-            query_filter.excluded_entities.insert(caster_entity);
+            self.query_filter.excluded_entities.insert(caster_entity);
+        } else {
+            self.query_filter.excluded_entities.remove(&caster_entity);
         }
 
         hits.count = 0;
 
         if self.max_hits == 1 {
-            let pipeline_shape = query_pipeline.as_composite_shape(query_filter);
+            let pipeline_shape = query_pipeline.as_composite_shape(&self.query_filter);
             let ray = parry::query::Ray::new(
                 self.global_origin().into(),
                 self.global_direction().adjust_precision().into(),
@@ -281,7 +281,7 @@ impl RayCaster {
             let mut leaf_callback = &mut |entity_index: &u32| {
                 let entity = query_pipeline.entity_from_index(*entity_index);
                 if let Some((iso, shape, layers)) = query_pipeline.colliders.get(&entity) {
-                    if query_filter.test(entity, *layers) {
+                    if self.query_filter.test(entity, *layers) {
                         if let Some(hit) = shape.shape_scaled().cast_ray_and_get_normal(
                             iso,
                             &ray,

diff --git a/src/spatial_query/shape_caster.rs b/src/spatial_query/shape_caster.rs
@@ -281,6 +281,9 @@ impl ShapeCaster {
         hits: &mut ShapeHits,
         query_pipeline: &SpatialQueryPipeline,
     ) {
+        // TODO: This clone is here so that the excluded entities in the original `query_filter` aren't modified.
+        //       We could remove this if shapecasting could compute multiple hits without just doing casts in a loop.
+        //       See https://github.com/Jondolf/avian/issues/403.
         let mut query_filter = self.query_filter.clone();
 
         if self.ignore_self {
@@ -303,7 +306,7 @@ impl ShapeCaster {
         let shape_direction = self.global_direction().adjust_precision().into();
 
         while hits.count < self.max_hits {
-            let pipeline_shape = query_pipeline.as_composite_shape(query_filter.clone());
+            let pipeline_shape = query_pipeline.as_composite_shape(&query_filter);
             let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new(
                 &*query_pipeline.dispatcher,
                 &shape_isometry,

diff --git a/src/spatial_query/system_param.rs b/src/spatial_query/system_param.rs
@@ -127,7 +127,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         direction: Dir,
         max_time_of_impact: Scalar,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Option<RayHitData> {
         self.query_pipeline
             .cast_ray(origin, direction, max_time_of_impact, solid, query_filter)
@@ -183,7 +183,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         direction: Dir,
         max_time_of_impact: Scalar,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         predicate: &dyn Fn(Entity) -> bool,
     ) -> Option<RayHitData> {
         self.query_pipeline.cast_ray_predicate(
@@ -245,7 +245,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         max_time_of_impact: Scalar,
         max_hits: u32,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<RayHitData> {
         self.query_pipeline.ray_hits(
             origin,
@@ -310,7 +310,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         direction: Dir,
         max_time_of_impact: Scalar,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         callback: impl FnMut(RayHitData) -> bool,
     ) {
         self.query_pipeline.ray_hits_callback(
@@ -374,7 +374,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         direction: Dir,
         max_time_of_impact: Scalar,
         ignore_origin_penetration: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Option<ShapeHitData> {
         self.query_pipeline.cast_shape(
             shape,
@@ -443,7 +443,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         max_time_of_impact: Scalar,
         max_hits: u32,
         ignore_origin_penetration: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<ShapeHitData> {
         self.query_pipeline.shape_hits(
             shape,
@@ -517,7 +517,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         direction: Dir,
         max_time_of_impact: Scalar,
         ignore_origin_penetration: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         callback: impl FnMut(ShapeHitData) -> bool,
     ) {
         self.query_pipeline.shape_hits_callback(
@@ -567,7 +567,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         &self,
         point: Vector,
         solid: bool,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Option<PointProjection> {
         self.query_pipeline
             .project_point(point, solid, query_filter)
@@ -603,7 +603,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
     pub fn point_intersections(
         &self,
         point: Vector,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<Entity> {
         self.query_pipeline.point_intersections(point, query_filter)
     }
@@ -648,7 +648,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
     pub fn point_intersections_callback(
         &self,
         point: Vector,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         callback: impl FnMut(Entity) -> bool,
     ) {
         self.query_pipeline
@@ -758,7 +758,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         shape: &Collider,
         shape_position: Vector,
         shape_rotation: RotationValue,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
     ) -> Vec<Entity> {
         self.query_pipeline
             .shape_intersections(shape, shape_position, shape_rotation, query_filter)
@@ -810,7 +810,7 @@ impl<'w, 's> SpatialQuery<'w, 's> {
         shape: &Collider,
         shape_position: Vector,
         shape_rotation: RotationValue,
-        query_filter: SpatialQueryFilter,
+        query_filter: &SpatialQueryFilter,
         callback: impl FnMut(Entity) -> bool,
     ) {
         self.query_pipeline.shape_intersections_callback(