Use entity index for QBVH nodes instead of entity bits (#59)

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<Entity, Added<Collider>>,
     changed_colliders: Query<Entity, ColliderChangedFilter>,
-    mut removed: RemovedComponents<Collider>,
+    mut removed_colliders: RemovedComponents<Collider>,
     mut query_pipeline: ResMut<SpatialQueryPipeline>,
 ) {
     let colliders: HashMap<Entity, (Isometry<Scalar>, &dyn parry::shape::Shape)> = colliders
@@ -383,7 +384,8 @@ fn update_query_pipeline(
             )
         })
         .collect();
+    let added = added_colliders.iter().collect::<Vec<_>>();
     let modified = changed_colliders.iter().collect::<Vec<_>>();
-    let removed = removed.iter().collect::<Vec<_>>();
-    query_pipeline.update_incremental(&colliders, modified, removed, true);
+    let removed = removed_colliders.iter().collect::<Vec<_>>();
+    query_pipeline.update_incremental(&colliders, added, modified, removed, true);
 }

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<u64>,
+    pub(crate) qbvh: Qbvh<u32>,
     pub(crate) dispatcher: Arc<dyn QueryDispatcher>,
     pub(crate) workspace: QbvhUpdateWorkspace,
+    pub(crate) entity_generations: HashMap<u32, u32>,
 }
 
 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<Entity, (Isometry<Scalar>, &dyn Shape)>,
+        added: Vec<Entity>,
         modified: Vec<Entity>,
         removed: Vec<Entity>,
         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<Scalar>>, &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);

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(

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(),

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<Entity> {
         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) {