Allow the PhysicsPipeline and CollisionPipeline to update the QueryPipeline incrementally

12 files changed, 212 insertions, 97 deletions

diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs
index 6a3f756..bca730b 100644
--- a/benchmarks3d/all_benchmarks3.rs
+++ b/benchmarks3d/all_benchmarks3.rs
@@ -20,6 +20,7 @@ mod joint_fixed3;
 mod joint_prismatic3;
 mod joint_revolute3;
 mod keva3;
+mod many_static3;
 mod pyramid3;
 mod stacks3;
 mod trimesh3;
@@ -54,6 +55,7 @@ pub fn main() {
         ("CCD", ccd3::init_world),
         ("Compound", compound3::init_world),
         ("Convex polyhedron", convex_polyhedron3::init_world),
+        ("Many static", many_static3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),
diff --git a/benchmarks3d/many_static3.rs b/benchmarks3d/many_static3.rs
new file mode 100644
index 0000000..3a0dbea
--- /dev/null
+++ b/benchmarks3d/many_static3.rs
@@ -0,0 +1,52 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Create the balls
+     */
+    let num = 50;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + 1.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+    let centerz = shift * (num as f32) / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num {
+            for k in 0..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery;
+                let z = k as f32 * shift - centerz;
+
+                let status = if j < num - 1 {
+                    RigidBodyType::Fixed
+                } else {
+                    RigidBodyType::Dynamic
+                };
+                let density = 0.477;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y, z]);
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::ball(rad).density(density);
+                colliders.insert_with_parent(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 9e0ab8e..9b06a80 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -123,7 +123,6 @@ impl CCDSolver {
     ) -> Option<Real> {
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
-            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithPredictedPosition { dt },
@@ -245,7 +244,6 @@ impl CCDSolver {
 
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
-            islands,
             bodies,
             colliders,
             QueryPipelineMode::SweepTestWithNextPosition,
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index 4f0a791..cbd9649 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -5,7 +5,6 @@ use crate::utils::{WBasis, WReal};
 
 #[cfg(feature = "dim3")]
 use crate::dynamics::SphericalJoint;
-use crate::geometry::ColliderEnabled;
 
 #[cfg(feature = "dim3")]
 bitflags::bitflags! {
diff --git a/src/geometry/broad_phase_qbvh.rs b/src/geometry/broad_phase_qbvh.rs
new file mode 100644
index 0000000..22ca562
--- /dev/null
+++ b/src/geometry/broad_phase_qbvh.rs
@@ -0,0 +1,88 @@
+use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
+use parry::bounding_volume::BoundingVolume;
+use parry::math::Real;
+use parry::partitioning::Qbvh;
+use parry::partitioning::QbvhUpdateWorkspace;
+use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
+pub struct BroadPhase {
+    qbvh: Qbvh<ColliderHandle>,
+    stack: Vec<(u32, u32)>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    workspace: QbvhUpdateWorkspace,
+}
+
+impl Default for BroadPhase {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl BroadPhase {
+    pub fn new() -> Self {
+        Self {
+            qbvh: Qbvh::new(),
+            stack: vec![],
+            workspace: QbvhUpdateWorkspace::default(),
+        }
+    }
+
+    #[allow(dead_code)] // This broad-phase is just experimental right now.
+    pub fn update(
+        &mut self,
+        prediction_distance: Real,
+        colliders: &ColliderSet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        events: &mut Vec<BroadPhasePairEvent>,
+    ) {
+        let margin = 0.01;
+
+        if modified_colliders.is_empty() {
+            return;
+        }
+
+        // Visitor to find collision pairs.
+        let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::new(
+            |co1: &ColliderHandle, co2: &ColliderHandle| {
+                events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(*co1, *co2)));
+                true
+            },
+        );
+
+        let full_rebuild = self.qbvh.raw_nodes().is_empty();
+
+        if full_rebuild {
+            self.qbvh.clear_and_rebuild(
+                colliders.iter().map(|(handle, collider)| {
+                    (
+                        handle,
+                        collider.compute_aabb().loosened(prediction_distance / 2.0),
+                    )
+                }),
+                margin,
+            );
+            self.qbvh
+                .traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
+        } else {
+            for modified in modified_colliders {
+                self.qbvh.pre_update_or_insert(*modified);
+            }
+
+            for removed in removed_colliders {
+                self.qbvh.remove(*removed);
+            }
+
+            let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
+                colliders[*handle]
+                    .compute_aabb()
+                    .loosened(prediction_distance / 2.0)
+            });
+            self.qbvh
+                .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
+            self.qbvh.rebalance(margin, &mut self.workspace);
+        }
+    }
+}
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index 1069f1b..4bc2a03 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -285,6 +285,7 @@ impl ColliderSet {
 
     // Just a very long name instead of `.get_mut` to make sure
     // this is really the method we wanted to use instead of `get_mut_internal`.
+    #[allow(dead_code)]
     pub(crate) fn get_mut_internal_with_modification_tracking(
         &mut self,
         handle: ColliderHandle,
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 4f3ada8..a91c4b9 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -1,6 +1,9 @@
 //! Structures related to geometry: colliders, shapes, etc.
 
-pub use self::broad_phase_multi_sap::{BroadPhase, BroadPhasePairEvent, ColliderPair};
+pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
+
+pub use self::broad_phase_multi_sap::BroadPhase;
+// pub use self::broad_phase_qbvh::BroadPhase;
 pub use self::collider_components::*;
 pub use self::contact_pair::{
     ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
@@ -199,5 +202,6 @@ mod interaction_graph;
 mod interaction_groups;
 mod narrow_phase;
 
+mod broad_phase_qbvh;
 mod collider;
 mod collider_set;
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 631388b..d0beedd 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -5,7 +5,7 @@ use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase,
 };
 use crate::math::Real;
-use crate::pipeline::{EventHandler, PhysicsHooks};
+use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
 
 /// The collision pipeline, responsible for performing collision detection between colliders.
@@ -111,6 +111,7 @@ impl CollisionPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
+        query_pipeline: Option<&mut QueryPipeline>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -154,6 +155,10 @@ impl CollisionPipeline {
             true,
         );
 
+        if let Some(queries) = query_pipeline {
+            queries.update_incremental(colliders, &modified_colliders, &removed_colliders, true);
+        }
+
         self.clear_modified_colliders(colliders, &mut modified_colliders);
         removed_colliders.clear();
     }
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index f05a3f0..7226063 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -14,7 +14,7 @@ use crate::geometry::{
     ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex,
 };
 use crate::math::{Real, Vector};
-use crate::pipeline::{EventHandler, PhysicsHooks};
+use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
 use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet};
 
 /// The physics pipeline, responsible for stepping the whole physics simulation.
@@ -404,6 +404,7 @@ impl PhysicsPipeline {
         impulse_joints: &mut ImpulseJointSet,
         multibody_joints: &mut MultibodyJointSet,
         ccd_solver: &mut CCDSolver,
+        mut query_pipeline: Option<&mut QueryPipeline>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -468,13 +469,17 @@ impl PhysicsPipeline {
             colliders,
             impulse_joints,
             multibody_joints,
-            &modified_colliders[..],
-            &mut removed_colliders,
+            &modified_colliders,
+            &removed_colliders,
             hooks,
             events,
             true,
         );
 
+        if let Some(queries) = query_pipeline.as_deref_mut() {
+            queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false);
+        }
+
         self.clear_modified_colliders(colliders, &mut modified_colliders);
         removed_colliders.clear();
 
@@ -595,13 +600,22 @@ impl PhysicsPipeline {
                 colliders,
                 impulse_joints,
                 multibody_joints,
-                &mut modified_colliders,
-                &mut removed_colliders,
+                &modified_colliders,
+                &[],
                 hooks,
                 events,
                 false,
             );
 
+            if let Some(queries) = query_pipeline.as_deref_mut() {
+                queries.update_incremental(
+                    colliders,
+                    &modified_colliders,
+                    &[],
+                    remaining_substeps == 0,
+                );
+            }
+
             self.clear_modified_colliders(colliders, &mut modified_colliders);
         }
 
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 6682370..8a667de 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,10 +1,10 @@
-use crate::dynamics::{IslandManager, RigidBodyHandle};
+use crate::dynamics::RigidBodyHandle;
 use crate::geometry::{
     Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection,
 };
 use crate::math::{Isometry, Point, Real, Vector};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
-use parry::partitioning::QbvhDataGenerator;
+use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace};
 use parry::query::details::{
     IntersectionCompositeShapeShapeBestFirstVisitor,
     NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -30,8 +30,9 @@ pub struct QueryPipeline {
     )]
     query_dispatcher: Arc<dyn QueryDispatcher>,
     qbvh: Qbvh<ColliderHandle>,
-    tree_built: bool,
     dilation_factor: Real,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    workspace: QbvhUpdateWorkspace,
 }
 
 struct QueryPipelineAsCompositeShape<'a> {
@@ -310,8 +311,8 @@ impl QueryPipeline {
         Self {
             query_dispatcher: Arc::new(d),
             qbvh: Qbvh::new(),
-            tree_built: false,
             dilation_factor: 0.01,
+            workspace: QbvhUpdateWorkspace::default(),
         }
     }
 
@@ -320,25 +321,39 @@ impl QueryPipeline {
         &*self.query_dispatcher
     }
 
-    /// Update the acceleration structure on the query pipeline.
-    pub fn update(
+    /// Update the query pipeline incrementally, avoiding a complete rebuild of its
+    /// internal data-structure.
+    pub fn update_incremental(
         &mut self,
-        islands: &IslandManager,
-        bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        refit_and_rebalance: bool,
     ) {
-        self.update_with_mode(
-            islands,
-            bodies,
-            colliders,
-            QueryPipelineMode::CurrentPosition,
-        )
+        for modified in modified_colliders {
+            self.qbvh.pre_update_or_insert(*modified);
+        }
+
+        for removed in removed_colliders {
+            self.qbvh.remove(*removed);
+        }
+
+        if refit_and_rebalance {
+            let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| {
+                colliders[*handle].compute_aabb()
+            });
+            self.qbvh.rebalance(0.0, &mut self.workspace);
+        }
+    }
+
+    /// Update the acceleration structure on the query pipeline.
+    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+        self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition)
     }
 
     /// Update the acceleration structure on the query pipeline.
     pub fn update_with_mode(
         &mut self,
-        islands: &IslandManager,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
         mode: QueryPipelineMode,
@@ -392,71 +407,12 @@ impl QueryPipeline {
             }
         }
 
-        if !self.tree_built {
-            let generator = DataGenerator {
-                bodies,
-                colliders,
-                mode,
-            };
-            self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
-
-            // FIXME: uncomment this once we handle insertion/removals properly.
-            // self.tree_built = true;
-            return;
-        }
-
-        for handle in islands.iter_active_bodies() {
-            let rb = &bodies[handle];
-            for handle in &rb.colliders.0 {
-                self.qbvh.pre_update(*handle)
-            }
-        }
-
-        match mode {
-            QueryPipelineMode::CurrentPosition => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        co.shape.compute_aabb(&co.pos)
-                    },
-                    self.dilation_factor,
-                );
-            }
-            QueryPipelineMode::SweepTestWithNextPosition => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        if let Some(parent) = &co.parent {
-                            let rb_next_pos = &bodies[parent.handle].pos.next_position;
-                            let next_position = rb_next_pos * parent.pos_wrt_parent;
-                            co.shape.compute_swept_aabb(&co.pos, &next_position)
-                        } else {
-                            co.shape.compute_aabb(&co.pos)
-                        }
-                    },
-                    self.dilation_factor,
-                );
-            }
-            QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        if let Some(parent) = co.parent {
-                            let rb = &bodies[parent.handle];
-                            let predicted_pos = rb.pos.integrate_forces_and_velocities(
-                                dt, &rb.forces, &rb.vels, &rb.mprops,
-                            );
-
-                            let next_position = predicted_pos * parent.pos_wrt_parent;
-                            co.shape.compute_swept_aabb(&co.pos, &next_position)
-                        } else {
-                            co.shape.compute_aabb(&co.pos)
-                        }
-                    },
-                    self.dilation_factor,
-                );
-            }
-        }
+        let generator = DataGenerator {
+            bodies,
+            colliders,
+            mode,
+        };
+        self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
     }
 
     /// Find the closest intersection between a ray and a set of collider.
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index adfd023..00096de 100644
--- a/src/pipeline/user_changes.rs
+++ b/src/pipeline/user_changes.rs
@@ -46,7 +46,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
     impulse_joints: &mut ImpulseJointSet,
-    multibody_joints: &mut MultibodyJointSet,
+    _multibody_joints: &mut MultibodyJointSet, // FIXME: propagate disabled state to multibodies
     modified_bodies: &[RigidBodyHandle],
     modified_colliders: &mut Vec<ColliderHandle>,
 ) {
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index 2ed9745..91d4da9 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -215,6 +215,7 @@ impl Harness {
                     &mut physics.impulse_joints,
                     &mut physics.multibody_joints,
                     &mut physics.ccd_solver,
+                    &mut physics.query_pipeline,
                     &*physics.hooks,
                     event_handler,
                 );
@@ -233,16 +234,11 @@ impl Harness {
             &mut self.physics.impulse_joints,
             &mut self.physics.multibody_joints,
             &mut self.physics.ccd_solver,
+            Some(&mut self.physics.query_pipeline),
             &*self.physics.hooks,
             &self.event_handler,
         );
 
-        self.physics.query_pipeline.update(
-            &self.physics.islands,
-            &self.physics.bodies,
-            &self.physics.colliders,
-        );
-
         for plugin in &mut self.plugins {
             plugin.step(&mut self.physics, &self.state)
         }