Run the position solver after the CCD motion clamping.

src/dynamics/solver/island_solver.rs

@@ -24,7 +24,25 @@ impl IslandSolver {
         }
     }
 
-    pub fn solve_island(
+    pub fn solve_position_constraints(
+        &mut self,
+        island_id: usize,
+        counters: &mut Counters,
+        params: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+    ) {
+        counters.solver.position_resolution_time.resume();
+        self.position_solver.solve(
+            island_id,
+            params,
+            bodies,
+            &self.contact_constraints.position_constraints,
+            &self.joint_constraints.position_constraints,
+        );
+        counters.solver.position_resolution_time.pause();
+    }
+
+    pub fn init_constraints_and_solve_velocity_constraints(
         &mut self,
         island_id: usize,
         counters: &mut Counters,
@@ -62,17 +80,9 @@ impl IslandSolver {
                 rb.integrate_next_position(params.dt, true)
             });
             counters.solver.velocity_update_time.pause();
-
-            counters.solver.position_resolution_time.resume();
-            self.position_solver.solve(
-                island_id,
-                params,
-                bodies,
-                &self.contact_constraints.position_constraints,
-                &self.joint_constraints.position_constraints,
-            );
-            counters.solver.position_resolution_time.pause();
         } else {
+            self.contact_constraints.clear();
+            self.joint_constraints.clear();
             counters.solver.velocity_update_time.resume();
             bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
                 // Since we didn't run the velocity solver we need to integrate the accelerations here

src/dynamics/solver/parallel_island_solver.rs

@@ -200,11 +200,9 @@ impl ParallelIslandSolver {
                     let dvel = &mut self.mj_lambdas[rb.active_set_offset];
 
                     dvel.linear += rb.force * (rb.effective_inv_mass * params.dt);
-                    rb.force = na::zero();
 
                     // dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor:
                     dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt;
-                    rb.torque = na::zero();
                 }
             }
         }

src/dynamics/solver/position_solver.rs

@@ -21,6 +21,11 @@ impl PositionSolver {
         contact_constraints: &[AnyPositionConstraint],
         joint_constraints: &[AnyJointPositionConstraint],
     ) {
+        if contact_constraints.is_empty() && joint_constraints.is_empty() {
+            // Nothing to do.
+            return;
+        }
+
         self.positions.clear();
         self.positions.extend(
             bodies

src/dynamics/solver/solver_constraints.rs

@@ -38,6 +38,15 @@ impl<VelocityConstraint, PositionConstraint>
             position_constraints: Vec::new(),
         }
     }
+
+    pub fn clear(&mut self) {
+        self.not_ground_interactions.clear();
+        self.ground_interactions.clear();
+        self.interaction_groups.clear();
+        self.ground_interaction_groups.clear();
+        self.velocity_constraints.clear();
+        self.position_constraints.clear();
+    }
 }
 
 impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {

src/pipeline/physics_pipeline.rs

@@ -58,7 +58,7 @@ impl PhysicsPipeline {
         }
     }
 
-    fn detect_collisions_after_user_modifications(
+    fn detect_collisions(
         &mut self,
         integration_parameters: &IntegrationParameters,
         broad_phase: &mut BroadPhase,
@@ -67,6 +67,7 @@ impl PhysicsPipeline {
         colliders: &mut ColliderSet,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
+        handle_user_changes: bool,
     ) {
         self.counters.stages.collision_detection_time.resume();
         self.counters.cd.broad_phase_time.resume();
@@ -84,7 +85,9 @@ impl PhysicsPipeline {
         self.counters.cd.narrow_phase_time.resume();
 
         // Update narrow-phase.
-        narrow_phase.handle_user_changes(colliders, bodies, events);
+        if handle_user_changes {
+            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,
@@ -102,51 +105,27 @@ impl PhysicsPipeline {
         self.counters.stages.collision_detection_time.pause();
     }
 
-    fn detect_collisions_after_integration(
+    fn solve_position_constraints(
         &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();
+        #[cfg(not(feature = "parallel"))]
-        self.counters.cd.broad_phase_time.resume();
+        {
+            enable_flush_to_zero!();
 
-        // Update broad-phase.
+            for island_id in 0..bodies.num_islands() {
-        self.broad_phase_events.clear();
+                self.solvers[island_id].solve_position_constraints(
-        self.broadphase_collider_pairs.clear();
+                    island_id,
-        broad_phase.update(
+                    &mut self.counters,
-            integration_parameters.prediction_distance,
+                    integration_parameters,
-            colliders,
+                    bodies,
-            &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,
-            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 build_islands_and_solve_constraints(
+    fn build_islands_and_solve_velocity_constraints(
         &mut self,
         gravity: &Vector<Real>,
         integration_parameters: &IntegrationParameters,
@@ -196,7 +175,7 @@ impl PhysicsPipeline {
             enable_flush_to_zero!();
 
             for island_id in 0..bodies.num_islands() {
-                self.solvers[island_id].solve_island(
+                self.solvers[island_id].init_constraints_and_solve_velocity_constraints(
                     island_id,
                     &mut self.counters,
                     integration_parameters,
@@ -246,6 +225,7 @@ impl PhysicsPipeline {
                             &manifold_indices[island_id],
                             joints,
                             &joint_constraint_indices[island_id],
+                            is_last_substep,
                         )
                     });
             });
@@ -258,23 +238,28 @@ impl PhysicsPipeline {
         integration_parameters: &IntegrationParameters,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
+        narrow_phase: &NarrowPhase,
         ccd_solver: &mut CCDSolver,
         events: &dyn EventHandler,
     ) {
+        self.counters.ccd.toi_computation_time.start();
         // Handle CCD
         let impacts = ccd_solver.predict_impacts_at_next_positions(
             integration_parameters.dt,
             bodies,
             colliders,
+            narrow_phase,
             events,
         );
         ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
+        self.counters.ccd.toi_computation_time.pause();
     }
 
     fn advance_to_final_positions(
         &mut self,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
+        clear_forces: bool,
     ) {
         // Set the rigid-bodies and kinematic bodies to their final position.
         bodies.foreach_active_body_mut_internal(|_, rb| {
@@ -283,6 +268,11 @@ impl PhysicsPipeline {
                 rb.angvel = na::zero();
             }
 
+            if clear_forces {
+                rb.force = na::zero();
+                rb.torque = na::zero();
+            }
+
             rb.position = rb.next_position;
             rb.update_colliders_positions(colliders);
         });
@@ -322,7 +312,7 @@ impl PhysicsPipeline {
         colliders.handle_user_changes(bodies);
         bodies.handle_user_changes(colliders);
 
-        self.detect_collisions_after_user_modifications(
+        self.detect_collisions(
             integration_parameters,
             broad_phase,
             narrow_phase,
@@ -330,23 +320,41 @@ impl PhysicsPipeline {
             colliders,
             hooks,
             events,
+            true,
         );
 
         let mut remaining_time = integration_parameters.dt;
-        let mut remaining_substeps = integration_parameters.max_ccd_substeps;
         let mut integration_parameters = *integration_parameters;
 
-        let ccd_active = ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt);
+        let (ccd_is_enabled, mut remaining_substeps) =
+            if integration_parameters.max_ccd_substeps == 0 {
+                (false, 1)
+            } else {
+                (true, integration_parameters.max_ccd_substeps)
+            };
 
-        loop {
+        while remaining_substeps > 0 {
-            if ccd_active && remaining_substeps > 1 {
+            // If there are more than one CCD substep, we need to split
-                // If there are more than one CCD substep, we need to split
+            // the timestep into multiple intervals. First, estimate the
-                // the timestep into multiple intervals. First, estimate the
+            // size of the time slice we will integrate for this substep.
-                // size of the time slice we will integrate for this substep.
+            //
-                //
+            // Note that we must do this now, before the constrains resolution
-                // If there is only one or zero CCD substep, there is no need
+            // because we need to use the correct timestep length for the
-                // to split the timetsep interval. So we can just skip this part.
+            // integration of external forces.
-                if let Some(toi) = ccd_solver.find_first_impact(remaining_time, bodies, colliders) {
+            //
+            // If there is only one or zero CCD substep, there is no need
+            // to split the timetsep interval. So we can just skip this part.
+            if ccd_is_enabled && remaining_substeps > 1 {
+                // NOTE: Take forces into account when updating the bodies CCD activation flags
+                //       these forces have not been integrated to the body's velocity yet.
+                let ccd_active = ccd_solver.update_ccd_active_flags(bodies, remaining_time, true);
+                let first_impact = if ccd_active {
+                    ccd_solver.find_first_impact(remaining_time, bodies, colliders, narrow_phase)
+                } else {
+                    None
+                };
+
+                if let Some(toi) = first_impact {
                     let original_interval = remaining_time / (remaining_substeps as Real);
 
                     if toi < original_interval {
@@ -360,7 +368,7 @@ impl PhysicsPipeline {
                 } else {
                     // No impact, don't do any other substep after this one.
                     integration_parameters.dt = remaining_time;
-                    remaining_substeps = 1;
+                    remaining_substeps = 0;
                 }
 
                 remaining_time -= integration_parameters.dt;
@@ -368,16 +376,18 @@ impl PhysicsPipeline {
                 // Avoid substep length that are too small.
                 if remaining_time <= integration_parameters.min_ccd_dt {
                     integration_parameters.dt += remaining_time;
-                    remaining_substeps = 1;
+                    remaining_substeps = 0;
                 }
             } else {
                 integration_parameters.dt = remaining_time;
                 remaining_time = 0.0;
-                remaining_substeps = 1;
+                remaining_substeps = 0;
             }
 
+            self.counters.ccd.num_substeps += 1;
+
             self.interpolate_kinematic_velocities(&integration_parameters, bodies);
-            self.build_islands_and_solve_constraints(
+            self.build_islands_and_solve_velocity_constraints(
                 gravity,
                 &integration_parameters,
                 narrow_phase,
@@ -387,18 +397,35 @@ impl PhysicsPipeline {
             );
 
             // If CCD is enabled, execute the CCD motion clamping.
-            if ccd_active && remaining_substeps > 0 {
+            if ccd_is_enabled {
-                self.run_ccd_motion_clamping(
+                // NOTE: don't the forces into account when updating the CCD active flags because
-                    &integration_parameters,
+                //       they have already been integrated into the velocities by the solver.
-                    bodies,
+                let ccd_active =
-                    colliders,
+                    ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt, false);
-                    ccd_solver,
+                if ccd_active {
-                    events,
+                    self.run_ccd_motion_clamping(
-                );
+                        &integration_parameters,
+                        bodies,
+                        colliders,
+                        narrow_phase,
+                        ccd_solver,
+                        events,
+                    );
+                }
             }
 
-            self.advance_to_final_positions(bodies, colliders);
+            // NOTE: we need to run the position solver **after** the
-            self.detect_collisions_after_integration(
+            //       CCD motion clamping because otherwise the clamping
+            //       would undo the depenetration done by the position
+            //       solver.
+            //       This happens because our CCD use the real rigid-body
+            //       velocities instead of just interpolating between
+            //       isometries.
+            self.solve_position_constraints(&integration_parameters, bodies);
+
+            let clear_forces = remaining_substeps == 0;
+            self.advance_to_final_positions(bodies, colliders, clear_forces);
+            self.detect_collisions(
                 &integration_parameters,
                 broad_phase,
                 narrow_phase,
@@ -406,15 +433,12 @@ impl PhysicsPipeline {
                 colliders,
                 hooks,
                 events,
+                false,
             );
 
             bodies.modified_inactive_set.clear();
-
-            if !ccd_active || remaining_substeps <= 1 {
-                // We executed all the substeps.
-                break;
-            }
         }
+
         self.counters.step_completed();
     }
 }

src_testbed/engine.rs

@@ -624,19 +624,17 @@ impl GraphicsManager {
         // );
         for (_, ns) in self.b2sn.iter_mut() {
             for n in ns.iter_mut() {
-                /*
+                // if let Some(co) = colliders.get(n.collider()) {
-                if let Some(co) = colliders.get(n.collider()) {
+                //     let bo = &_bodies[co.parent()];
-                    let bo = &bodies[co.parent()];
+                //
-
+                //     if bo.is_dynamic() {
-                    if bo.is_dynamic() {
+                //         if bo.is_ccd_active() {
-                        if bo.is_sleeping() {
+                //             n.set_color(Point3::new(1.0, 0.0, 0.0));
-                            n.set_color(Point3::new(1.0, 0.0, 0.0));
+                //         } else {
-                        } else {
+                //             n.set_color(Point3::new(0.0, 1.0, 0.0));
-                            n.set_color(Point3::new(0.0, 1.0, 0.0));
+                //         }
-                        }
+                //     }
-                    }
+                // }
-                }
-                 */
 
                 n.update(colliders);
                 n.draw(window);