Allow collider modification after its insersion to the ColliderSet.

2 files changed, 72 insertions, 25 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 5df3f6a..da572f3 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -44,19 +44,15 @@ impl CollisionPipeline {
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
-        bodies.maintain(colliders);
+        colliders.handle_user_changes(bodies);
+        bodies.handle_user_changes(colliders);
         self.broadphase_collider_pairs.clear();
 
         self.broad_phase_events.clear();
-        broad_phase.update(
-            prediction_distance,
-            bodies,
-            colliders,
-            &mut self.broad_phase_events,
-        );
+        broad_phase.update(prediction_distance, colliders, &mut self.broad_phase_events);
 
+        narrow_phase.handle_user_changes(colliders, bodies, events);
         narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
-
         narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events);
         narrow_phase.compute_intersections(bodies, colliders, hooks, events);
 
@@ -64,22 +60,16 @@ impl CollisionPipeline {
             colliders,
             narrow_phase,
             self.empty_joints.joint_graph(),
-            0,
+            128,
         );
 
-        // // Update kinematic bodies velocities.
-        // bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-        //     body.compute_velocity_from_next_position(integration_parameters.inv_dt());
-        // });
-
         // Update colliders positions and kinematic bodies positions.
         bodies.foreach_active_body_mut_internal(|_, rb| {
             rb.position = rb.next_position;
             rb.update_colliders_positions(colliders);
 
             for handle in &rb.colliders {
-                let collider = &mut colliders[*handle];
-                collider.prev_position = collider.position;
+                let collider = colliders.get_mut_internal(*handle).unwrap();
                 collider.position = rb.position * collider.delta;
             }
         });
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index fa01def..296a400 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -58,7 +58,7 @@ impl PhysicsPipeline {
         }
     }
 
-    fn detect_collisions(
+    fn detect_collisions_after_user_modifications(
         &mut self,
         integration_parameters: &IntegrationParameters,
         broad_phase: &mut BroadPhase,
@@ -70,23 +70,67 @@ impl PhysicsPipeline {
     ) {
         self.counters.stages.collision_detection_time.start();
         self.counters.cd.broad_phase_time.start();
+
+        // Update broad-phase.
         self.broad_phase_events.clear();
         self.broadphase_collider_pairs.clear();
-
         broad_phase.update(
             integration_parameters.prediction_distance,
-            bodies,
             colliders,
             &mut self.broad_phase_events,
         );
-        self.counters.cd.broad_phase_time.pause();
-
-        //        println!("Num contact pairs: {}", pairs.len());
 
+        self.counters.cd.broad_phase_time.pause();
         self.counters.cd.narrow_phase_time.start();
-        narrow_phase.maintain(colliders, bodies);
+
+        // Update narrow-phase.
+        narrow_phase.handle_user_changes(colliders, bodies, events);
         narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
+        narrow_phase.compute_contacts(
+            integration_parameters.prediction_distance,
+            bodies,
+            colliders,
+            hooks,
+            events,
+        );
+        narrow_phase.compute_intersections(bodies, colliders, hooks, events);
+
+        // Clear colliders modification flags.
+        colliders.clear_modified_colliders();
+
+        self.counters.cd.narrow_phase_time.pause();
+        self.counters.stages.collision_detection_time.pause();
+    }
+
+    fn detect_collisions_after_integration(
+        &mut self,
+        integration_parameters: &IntegrationParameters,
+        broad_phase: &mut BroadPhase,
+        narrow_phase: &mut NarrowPhase,
+        bodies: &mut RigidBodySet,
+        colliders: &mut ColliderSet,
+        hooks: &dyn PhysicsHooks,
+        events: &dyn EventHandler,
+    ) {
+        self.counters.stages.collision_detection_time.resume();
+        self.counters.cd.broad_phase_time.resume();
+
+        // Update broad-phase.
+        self.broad_phase_events.clear();
+        self.broadphase_collider_pairs.clear();
+        broad_phase.update(
+            integration_parameters.prediction_distance,
+            colliders,
+            &mut self.broad_phase_events,
+        );
 
+        self.counters.cd.broad_phase_time.pause();
+        self.counters.cd.narrow_phase_time.resume();
+
+        // Update narrow-phase.
+        // NOTE: we don't need to call `narrow_phase.handle_user_changes` because this
+        //       has already been done at the beginning of the timestep.
+        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
         narrow_phase.compute_contacts(
             integration_parameters.prediction_distance,
             bodies,
@@ -95,6 +139,9 @@ impl PhysicsPipeline {
             events,
         );
         narrow_phase.compute_intersections(bodies, colliders, hooks, events);
+        // Clear colliders modification flags.
+        colliders.clear_modified_colliders();
+
         self.counters.cd.narrow_phase_time.pause();
         self.counters.stages.collision_detection_time.pause();
     }
@@ -275,10 +322,11 @@ impl PhysicsPipeline {
         events: &dyn EventHandler,
     ) {
         self.counters.step_started();
-        bodies.maintain(colliders);
+        colliders.handle_user_changes(bodies);
+        bodies.handle_user_changes(colliders);
 
         self.interpolate_kinematic_velocities(integration_parameters, bodies);
-        self.detect_collisions(
+        self.detect_collisions_after_user_modifications(
             integration_parameters,
             broad_phase,
             narrow_phase,
@@ -303,6 +351,15 @@ impl PhysicsPipeline {
             events,
         );
         self.advance_to_final_positions(bodies, colliders);
+        self.detect_collisions_after_integration(
+            integration_parameters,
+            broad_phase,
+            narrow_phase,
+            bodies,
+            colliders,
+            hooks,
+            events,
+        );
 
         bodies.modified_inactive_set.clear();
         self.counters.step_completed();