From c7d274f0b543c75aafe73d9c1dceb3633a5687d4 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 8 Jul 2023 20:13:54 +0300 Subject: [PATCH] Use entity index for QBVH nodes instead of entity bits (#59) --- src/plugins/spatial_query/mod.rs | 8 +++--- src/plugins/spatial_query/pipeline.rs | 31 ++++++++++++++++++----- src/plugins/spatial_query/ray_caster.rs | 8 +++--- src/plugins/spatial_query/shape_caster.rs | 4 +-- src/plugins/spatial_query/system_param.rs | 28 ++++++++++---------- 5 files changed, 49 insertions(+), 30 deletions(-) diff --git a/src/plugins/spatial_query/mod.rs b/src/plugins/spatial_query/mod.rs index 64477f50..b1c474da 100644 --- a/src/plugins/spatial_query/mod.rs +++ b/src/plugins/spatial_query/mod.rs @@ -367,8 +367,9 @@ type ColliderChangedFilter = ( fn update_query_pipeline( colliders: Query<(Entity, &Position, &Rotation, &Collider)>, + added_colliders: Query>, changed_colliders: Query, - mut removed: RemovedComponents, + mut removed_colliders: RemovedComponents, mut query_pipeline: ResMut, ) { let colliders: HashMap, &dyn parry::shape::Shape)> = colliders @@ -383,7 +384,8 @@ fn update_query_pipeline( ) }) .collect(); + let added = added_colliders.iter().collect::>(); let modified = changed_colliders.iter().collect::>(); - let removed = removed.iter().collect::>(); - query_pipeline.update_incremental(&colliders, modified, removed, true); + let removed = removed_colliders.iter().collect::>(); + query_pipeline.update_incremental(&colliders, added, modified, removed, true); } diff --git a/src/plugins/spatial_query/pipeline.rs b/src/plugins/spatial_query/pipeline.rs index b23eced2..cd93729b 100644 --- a/src/plugins/spatial_query/pipeline.rs +++ b/src/plugins/spatial_query/pipeline.rs @@ -15,9 +15,10 @@ use parry::{ /// as an acceleration structure for spatial queries. #[derive(Resource, Clone)] pub struct SpatialQueryPipeline { - pub(crate) qbvh: Qbvh, + pub(crate) qbvh: Qbvh, pub(crate) dispatcher: Arc, pub(crate) workspace: QbvhUpdateWorkspace, + pub(crate) entity_generations: HashMap, } impl Default for SpatialQueryPipeline { @@ -26,6 +27,7 @@ impl Default for SpatialQueryPipeline { qbvh: Qbvh::new(), dispatcher: Arc::new(DefaultQueryDispatcher), workspace: QbvhUpdateWorkspace::default(), + entity_generations: HashMap::default(), } } } @@ -46,23 +48,38 @@ impl SpatialQueryPipeline { pub(crate) fn update_incremental( &mut self, colliders: &HashMap, &dyn Shape)>, + added: Vec, modified: Vec, removed: Vec, refit_and_balance: bool, ) { + // Insert or update generations of added entities + for added in added { + let index = added.index(); + if let Some(generation) = self.entity_generations.get_mut(&index) { + *generation = added.generation(); + } else { + self.entity_generations.insert(index, added.generation()); + } + } + for removed in removed { - self.qbvh.remove(removed.to_bits()); + self.qbvh.remove(removed.index()); } for modified in modified { if colliders.get(&modified).is_some() { - self.qbvh.pre_update_or_insert(modified.to_bits()); + self.qbvh.pre_update_or_insert(modified.index()); } } if refit_and_balance { - let _ = self.qbvh.refit(0.0, &mut self.workspace, |entity_bits| { - let (iso, shape) = colliders.get(&Entity::from_bits(*entity_bits)).unwrap(); + let _ = self.qbvh.refit(0.0, &mut self.workspace, |entity_index| { + // Construct entity ID + let generation = self.entity_generations.get(entity_index).map_or(0, |i| *i); + let entity = Entity::from_bits((generation as u64) << 32 | *entity_index as u64); + // Compute and return AABB + let (iso, shape) = colliders.get(&entity).unwrap(); shape.compute_aabb(iso) }); self.qbvh.rebalance(0.0, &mut self.workspace); @@ -78,7 +95,7 @@ pub(crate) struct QueryPipelineAsCompositeShape<'a> { impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { type PartShape = dyn Shape; - type PartId = u64; + type PartId = u32; type QbvhStorage = DefaultStorage; fn map_typed_part_at( @@ -87,7 +104,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), ) { if let Some((entity, (iso, shape, layers))) = - self.colliders.get_key_value(&Entity::from_bits(shape_id)) + self.colliders.get_key_value(&Entity::from_raw(shape_id)) { if self.query_filter.test(*entity, *layers) { f(Some(iso), &**shape); diff --git a/src/plugins/spatial_query/ray_caster.rs b/src/plugins/spatial_query/ray_caster.rs index 49cfb61d..d546da7d 100644 --- a/src/plugins/spatial_query/ray_caster.rs +++ b/src/plugins/spatial_query/ray_caster.rs @@ -202,8 +202,8 @@ impl RayCaster { ); if let Some(hit) = query_pipeline.qbvh.traverse_best_first(&mut visitor).map( - |(_, (entity_bits, hit))| RayHitData { - entity: Entity::from_bits(entity_bits), + |(_, (entity_index, hit))| RayHitData { + entity: Entity::from_raw(entity_index), time_of_impact: hit.toi, normal: hit.normal.into(), }, @@ -219,8 +219,8 @@ impl RayCaster { let ray = parry::query::Ray::new(self.global_origin().into(), self.global_direction().into()); - let mut leaf_callback = &mut |entity_bits: &u64| { - let entity = Entity::from_bits(*entity_bits); + let mut leaf_callback = &mut |entity_index: &u32| { + let entity = Entity::from_raw(*entity_index); if let Some((iso, shape, layers)) = colliders.get(&entity) { if self.query_filter.test(entity, *layers) { if let Some(hit) = shape.cast_ray_and_get_normal( diff --git a/src/plugins/spatial_query/shape_caster.rs b/src/plugins/spatial_query/shape_caster.rs index 6f418642..cbd89e25 100644 --- a/src/plugins/spatial_query/shape_caster.rs +++ b/src/plugins/spatial_query/shape_caster.rs @@ -257,8 +257,8 @@ impl ShapeCaster { query_pipeline .qbvh .traverse_best_first(&mut visitor) - .map(|(_, (entity_bits, hit))| ShapeHitData { - entity: Entity::from_bits(entity_bits), + .map(|(_, (entity_index, hit))| ShapeHitData { + entity: Entity::from_raw(entity_index), time_of_impact: hit.toi, point1: hit.witness1.into(), point2: hit.witness2.into(), diff --git a/src/plugins/spatial_query/system_param.rs b/src/plugins/spatial_query/system_param.rs index a428b703..89932c0c 100644 --- a/src/plugins/spatial_query/system_param.rs +++ b/src/plugins/spatial_query/system_param.rs @@ -165,8 +165,8 @@ impl<'w, 's> SpatialQuery<'w, 's> { self.query_pipeline .qbvh .traverse_best_first(&mut visitor) - .map(|(_, (entity_bits, hit))| RayHitData { - entity: Entity::from_bits(entity_bits), + .map(|(_, (entity_index, hit))| RayHitData { + entity: Entity::from_raw(entity_index), time_of_impact: hit.toi, normal: hit.normal.into(), }) @@ -298,8 +298,8 @@ impl<'w, 's> SpatialQuery<'w, 's> { let mut hits = Vec::with_capacity(10); let ray = parry::query::Ray::new(origin.into(), direction.into()); - let mut leaf_callback = &mut |entity_bits: &u64| { - let entity = Entity::from_bits(*entity_bits); + let mut leaf_callback = &mut |entity_index: &u32| { + let entity = Entity::from_raw(*entity_index); if let Some((iso, shape, layers)) = colliders.get(&entity) { if query_filter.test(entity, *layers) { if let Some(hit) = @@ -408,8 +408,8 @@ impl<'w, 's> SpatialQuery<'w, 's> { self.query_pipeline .qbvh .traverse_best_first(&mut visitor) - .map(|(_, (entity_bits, hit))| ShapeHitData { - entity: Entity::from_bits(entity_bits), + .map(|(_, (entity_index, hit))| ShapeHitData { + entity: Entity::from_raw(entity_index), time_of_impact: hit.toi, point1: hit.witness1.into(), point2: hit.witness2.into(), @@ -466,8 +466,8 @@ impl<'w, 's> SpatialQuery<'w, 's> { self.query_pipeline .qbvh .traverse_best_first(&mut visitor) - .map(|(_, (projection, entity_bits))| PointProjection { - entity: Entity::from_bits(entity_bits), + .map(|(_, (projection, entity_index))| PointProjection { + entity: Entity::from_raw(entity_index), point: projection.point.into(), is_inside: projection.is_inside, }) @@ -555,8 +555,8 @@ impl<'w, 's> SpatialQuery<'w, 's> { let mut intersections = vec![]; - let mut leaf_callback = &mut |entity_bits: &u64| { - let entity = Entity::from_bits(*entity_bits); + let mut leaf_callback = &mut |entity_index: &u32| { + let entity = Entity::from_raw(*entity_index); if let Ok((entity, position, rotation, shape, layers)) = self.colliders.get(entity) { let isometry = utils::make_isometry(position.0, rotation); if query_filter.test(entity, layers.map_or(CollisionLayers::default(), |l| *l)) @@ -637,8 +637,8 @@ impl<'w, 's> SpatialQuery<'w, 's> { mut callback: impl FnMut(Entity) -> bool, ) -> Vec { let mut intersections = vec![]; - let mut leaf_callback = |entity_bits: &u64| { - let entity = Entity::from_bits(*entity_bits); + let mut leaf_callback = |entity_index: &u32| { + let entity = Entity::from_raw(*entity_index); intersections.push(entity); callback(entity) }; @@ -764,8 +764,8 @@ impl<'w, 's> SpatialQuery<'w, 's> { let dispatcher = &*self.query_pipeline.dispatcher; let mut intersections = vec![]; - let mut leaf_callback = &mut |entity_bits: &u64| { - let entity = Entity::from_bits(*entity_bits); + let mut leaf_callback = &mut |entity_index: &u32| { + let entity = Entity::from_raw(*entity_index); if let Some((collider_isometry, collider_shape, layers)) = colliders.get(&entity) { if query_filter.test(entity, *layers) {