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