17 files changed, 291 insertions, 127 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index bdde135..9e0ab8e 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -60,12 +60,20 @@ impl CCDSolver {
 
                     let min_toi = (rb.ccd.ccd_thickness
                         * 0.15
-                        * crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels)))
+                        * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels)))
                     .min(dt);
-                    // println!("Min toi: {}, Toi: {}", min_toi, toi);
-                    let new_pos = rb
-                        .vels
-                        .integrate(toi.max(min_toi), &rb.pos.position, &local_com);
+                    // println!(
+                    //     "Min toi: {}, Toi: {}, thick: {}, max_vel: {}",
+                    //     min_toi,
+                    //     toi,
+                    //     rb.ccd.ccd_thickness,
+                    //     rb.ccd.max_point_velocity(&rb.integrated_vels)
+                    // );
+                    let new_pos = rb.integrated_vels.integrate(
+                        toi.max(min_toi),
+                        &rb.pos.position,
+                        &local_com,
+                    );
                     rb.pos.next_position = new_pos;
                 }
             }
@@ -95,7 +103,7 @@ impl CCDSolver {
                 } else {
                     None
                 };
-                let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces);
+                let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces);
                 rb.ccd.ccd_active = moving_fast;
                 ccd_active = ccd_active || moving_fast;
             }
@@ -131,7 +139,7 @@ impl CCDSolver {
                 let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
                     dt,
                     &rb1.forces,
-                    &rb1.vels,
+                    &rb1.integrated_vels,
                     &rb1.mprops,
                 );
 
@@ -256,7 +264,7 @@ impl CCDSolver {
                 let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
                     dt,
                     &rb1.forces,
-                    &rb1.vels,
+                    &rb1.integrated_vels,
                     &rb1.mprops,
                 );
 
@@ -491,7 +499,10 @@ impl CCDSolver {
                 let local_com1 = &rb1.mprops.local_mprops.local_com;
                 let frozen1 = frozen.get(&b1);
                 let pos1 = frozen1
-                    .map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1))
+                    .map(|t| {
+                        rb1.integrated_vels
+                            .integrate(*t, &rb1.pos.position, local_com1)
+                    })
                     .unwrap_or(rb1.pos.next_position);
                 pos1 * co_parent1.pos_wrt_parent
             } else {
@@ -504,7 +515,10 @@ impl CCDSolver {
                 let local_com2 = &rb2.mprops.local_mprops.local_com;
                 let frozen2 = frozen.get(&b2);
                 let pos2 = frozen2
-                    .map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2))
+                    .map(|t| {
+                        rb2.integrated_vels
+                            .integrate(*t, &rb2.pos.position, local_com2)
+                    })
                     .unwrap_or(rb2.pos.next_position);
                 pos2 * co_parent2.pos_wrt_parent
             } else {
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 4591825..6f5a47d 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -56,14 +56,14 @@ impl TOIEntry {
             return None;
         }
 
-        let linvel1 =
-            frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero());
-        let linvel2 =
-            frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero());
-        let angvel1 =
-            frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero());
-        let angvel2 =
-            frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero());
+        let linvel1 = frozen1.is_none() as u32 as Real
+            * rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
+        let linvel2 = frozen2.is_none() as u32 as Real
+            * rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
+        let angvel1 = frozen1.is_none() as u32 as Real
+            * rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
+        let angvel2 = frozen2.is_none() as u32 as Real
+            * rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
 
         #[cfg(feature = "dim2")]
         let vel12 = (linvel2 - linvel1).norm()
@@ -114,6 +114,20 @@ impl TOIEntry {
         // because the colliders may be in a separating trajectory.
         let stop_at_penetration = is_pseudo_intersection_test;
 
+        // let pos12 = motion_c1
+        //     .position_at_time(start_time)
+        //     .inv_mul(&motion_c2.position_at_time(start_time));
+        // let vel12 = linvel2 - linvel1;
+        // let res_toi = query_dispatcher
+        //     .time_of_impact(
+        //         &pos12,
+        //         &vel12,
+        //         co1.shape.as_ref(),
+        //         co2.shape.as_ref(),
+        //         end_time - start_time,
+        //     )
+        //     .ok();
+
         let res_toi = query_dispatcher
             .nonlinear_time_of_impact(
                 &motion_c1,
@@ -144,8 +158,8 @@ impl TOIEntry {
             NonlinearRigidMotion::new(
                 rb.pos.position,
                 rb.mprops.local_mprops.local_com,
-                rb.vels.linvel,
-                rb.vels.angvel,
+                rb.integrated_vels.linvel,
+                rb.integrated_vels.angvel,
             )
         } else {
             NonlinearRigidMotion::constant_position(rb.pos.next_position)
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 0309cff..c4ec734 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -42,6 +42,7 @@ pub struct ImpulseJointSet {
     rb_graph_ids: Coarena<RigidBodyGraphIndex>,
     joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
     joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
+    pub(crate) to_wake_up: Vec<RigidBodyHandle>, // A set of rigid-body handles to wake-up during the next timestep.
 }
 
 impl ImpulseJointSet {
@@ -51,6 +52,7 @@ impl ImpulseJointSet {
             rb_graph_ids: Coarena::new(),
             joint_ids: Arena::new(),
             joint_graph: InteractionGraph::new(),
+            to_wake_up: vec![],
         }
     }
 
@@ -180,11 +182,15 @@ impl ImpulseJointSet {
     }
 
     /// Inserts a new joint into this set and retrieve its handle.
+    ///
+    /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
+    /// automatically woken up during the next timestep.
     pub fn insert(
         &mut self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
         data: impl Into<GenericJoint>,
+        wake_up: bool,
     ) -> ImpulseJointHandle {
         let data = data.into();
         let handle = self.joint_ids.insert(0.into());
@@ -217,6 +223,12 @@ impl ImpulseJointSet {
         }
 
         self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint);
+
+        if wake_up {
+            self.to_wake_up.push(body1);
+            self.to_wake_up.push(body2);
+        }
+
         ImpulseJointHandle(handle)
     }
 
@@ -257,23 +269,16 @@ impl ImpulseJointSet {
     ///
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
     /// automatically woken up.
-    pub fn remove(
-        &mut self,
-        handle: ImpulseJointHandle,
-        islands: &mut IslandManager,
-        bodies: &mut RigidBodySet,
-        wake_up: bool,
-    ) -> Option<ImpulseJoint> {
+    pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option<ImpulseJoint> {
         let id = self.joint_ids.remove(handle.0)?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
 
         if wake_up {
-            // Wake-up the bodies attached to this joint.
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
-                islands.wake_up(bodies, *rb_handle, true);
+                self.to_wake_up.push(*rb_handle);
             }
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
-                islands.wake_up(bodies, *rb_handle, true);
+                self.to_wake_up.push(*rb_handle);
             }
         }
 
@@ -294,8 +299,6 @@ impl ImpulseJointSet {
     pub fn remove_joints_attached_to_rigid_body(
         &mut self,
         handle: RigidBodyHandle,
-        islands: &mut IslandManager,
-        bodies: &mut RigidBodySet,
     ) -> Vec<ImpulseJointHandle> {
         let mut deleted = vec![];
 
@@ -324,8 +327,8 @@ impl ImpulseJointSet {
                     }
 
                     // Wake up the attached bodies.
-                    islands.wake_up(bodies, h1, true);
-                    islands.wake_up(bodies, h2, true);
+                    self.to_wake_up.push(h1);
+                    self.to_wake_up.push(h2);
                 }
 
                 if let Some(other) = self.joint_graph.remove_node(deleted_id) {
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index 50a9438..f1b2ffe 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -1,8 +1,6 @@
 use crate::data::{Arena, Coarena, Index};
 use crate::dynamics::joint::MultibodyLink;
-use crate::dynamics::{
-    GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet,
-};
+use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle};
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
 use crate::parry::partitioning::IndexedData;
 
@@ -84,6 +82,7 @@ pub struct MultibodyJointSet {
     // NOTE: this is mostly for the island extraction. So perhaps we won’t need
     //       that any more in the future when we improve our island builder.
     pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
+    pub(crate) to_wake_up: Vec<RigidBodyHandle>,
 }
 
 impl MultibodyJointSet {
@@ -93,6 +92,7 @@ impl MultibodyJointSet {
             multibodies: Arena::new(),
             rb2mb: Coarena::new(),
             connectivity_graph: InteractionGraph::new(),
+            to_wake_up: vec![],
         }
     }
 
@@ -113,6 +113,7 @@ impl MultibodyJointSet {
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
         data: impl Into<GenericJoint>,
+        wake_up: bool,
     ) -> Option<MultibodyJointHandle> {
         let data = data.into();
         let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {
@@ -155,6 +156,11 @@ impl MultibodyJointSet {
 
         multibody1.append(mb2, link1.id, MultibodyJoint::new(data));
 
+        if wake_up {
+            self.to_wake_up.push(body1);
+            self.to_wake_up.push(body2);
+        }
+
         // Because each rigid-body can only have one parent link,
         // we can use the second rigid-body’s handle as the multibody_joint’s
         // handle.
@@ -162,13 +168,7 @@ impl MultibodyJointSet {
     }
 
     /// Removes an multibody_joint from this set.
-    pub fn remove(
-        &mut self,
-        handle: MultibodyJointHandle,
-        islands: &mut IslandManager,
-        bodies: &mut RigidBodySet,
-        wake_up: bool,
-    ) {
+    pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) {
         if let Some(removed) = self.rb2mb.get(handle.0).copied() {
             let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
 
@@ -181,8 +181,8 @@ impl MultibodyJointSet {
                 );
 
                 if wake_up {
-                    islands.wake_up(bodies, RigidBodyHandle(handle.0), true);
-                    islands.wake_up(bodies, parent_rb, true);
+                    self.to_wake_up.push(RigidBodyHandle(handle.0));
+                    self.to_wake_up.push(parent_rb);
                 }
 
                 // TODO: remove the node if it no longer has any attached edges?
@@ -211,13 +211,7 @@ impl MultibodyJointSet {
     }
 
     /// Removes all the multibody_joints from the multibody the given rigid-body is part of.
-    pub fn remove_multibody_articulations(
-        &mut self,
-        handle: RigidBodyHandle,
-        islands: &mut IslandManager,
-        bodies: &mut RigidBodySet,
-        wake_up: bool,
-    ) {
+    pub fn remove_multibody_articulations(&mut self, handle: RigidBodyHandle, wake_up: bool) {
         if let Some(removed) = self.rb2mb.get(handle.0).copied() {
             // Remove the multibody.
             let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -225,7 +219,7 @@ impl MultibodyJointSet {
                 let rb_handle = link.rigid_body;
 
                 if wake_up {
-                    islands.wake_up(bodies, rb_handle, true);
+                    self.to_wake_up.push(rb_handle);
                 }
 
                 // Remove the rigid-body <-> multibody mapping for this link.
@@ -239,12 +233,7 @@ impl MultibodyJointSet {
     }
 
     /// Removes all the multibody joints attached to a rigid-body.
-    pub fn remove_joints_attached_to_rigid_body(
-        &mut self,
-        rb_to_remove: RigidBodyHandle,
-        islands: &mut IslandManager,
-        bodies: &mut RigidBodySet,
-    ) {
+    pub fn remove_joints_attached_to_rigid_body(&mut self, rb_to_remove: RigidBodyHandle) {
         // TODO: optimize this.
         if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
             let mut articulations_to_remove = vec![];
@@ -255,12 +244,12 @@ impl MultibodyJointSet {
                 // There is a multibody_joint handle is equal to the second rigid-body’s handle.
                 articulations_to_remove.push(MultibodyJointHandle(rb2.0));
 
-                islands.wake_up(bodies, rb1, true);
-                islands.wake_up(bodies, rb2, true);
+                self.to_wake_up.push(rb1);
+                self.to_wake_up.push(rb2);
             }
 
             for articulation_handle in articulations_to_remove {
-                self.remove(articulation_handle, islands, bodies, true);
+                self.remove(articulation_handle, true);
             }
         }
     }
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 24e9754..5eca5a2 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -19,6 +19,10 @@ use num::Zero;
 pub struct RigidBody {
     pub(crate) pos: RigidBodyPosition,
     pub(crate) mprops: RigidBodyMassProps,
+    // NOTE: we need this so that the CCD can use the actual velocities obtained
+    //       by the velocity solver with bias. If we switch to intepolation, we
+    //       should remove this field.
+    pub(crate) integrated_vels: RigidBodyVelocity,
     pub(crate) vels: RigidBodyVelocity,
     pub(crate) damping: RigidBodyDamping,
     pub(crate) forces: RigidBodyForces,
@@ -47,6 +51,7 @@ impl RigidBody {
         Self {
             pos: RigidBodyPosition::default(),
             mprops: RigidBodyMassProps::default(),
+            integrated_vels: RigidBodyVelocity::default(),
             vels: RigidBodyVelocity::default(),
             damping: RigidBodyDamping::default(),
             forces: RigidBodyForces::default(),
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index b818ce2..3d35d17 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -752,7 +752,7 @@ pub struct RigidBodyCcd {
 impl Default for RigidBodyCcd {
     fn default() -> Self {
         Self {
-            ccd_thickness: 0.0,
+            ccd_thickness: Real::MAX,
             ccd_max_dist: 0.0,
             ccd_active: false,
             ccd_enabled: false,
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 62a6a54..cd8f30a 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -109,8 +109,8 @@ impl RigidBodySet {
         /*
          * Remove impulse_joints attached to this rigid-body.
          */
-        impulse_joints.remove_joints_attached_to_rigid_body(handle, islands, self);
-        multibody_joints.remove_joints_attached_to_rigid_body(handle, islands, self);
+        impulse_joints.remove_joints_attached_to_rigid_body(handle);
+        multibody_joints.remove_joints_attached_to_rigid_body(handle);
 
         Some(rb)
     }
diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index 318727e..ed8c569 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_constraint.rs
@@ -34,6 +34,7 @@ impl GenericVelocityConstraint {
         jacobian_id: &mut usize,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -45,6 +46,7 @@ impl GenericVelocityConstraint {
 
         let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
         let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
+        let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
 
         let multibody1 = multibodies
             .rigid_body_link(handle1)
@@ -92,6 +94,7 @@ impl GenericVelocityConstraint {
             .enumerate()
         {
             let chunk_j_id = *jacobian_id;
+            let mut is_fast_contact = false;
             let mut constraint = VelocityConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
@@ -107,6 +110,7 @@ impl GenericVelocityConstraint {
                 } else {
                     na::zero()
                 },
+                cfm_factor,
                 limit: 0.0,
                 mj_lambda1,
                 mj_lambda2,
@@ -196,10 +200,13 @@ impl GenericVelocityConstraint {
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5);
+
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r,
@@ -283,6 +290,8 @@ impl GenericVelocityConstraint {
                 }
             }
 
+            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+
             let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0);
             let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0);
             // NOTE: we use the generic constraint for non-dynamic bodies because this will
@@ -310,7 +319,6 @@ impl GenericVelocityConstraint {
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         jacobians: &DVector<Real>,
         mj_lambdas: &mut [DeltaVel<Real>],
         generic_mj_lambdas: &mut DVector<Real>,
@@ -332,7 +340,7 @@ impl GenericVelocityConstraint {
         let elements = &mut self.velocity_constraint.elements
             [..self.velocity_constraint.num_contacts as usize];
         VelocityConstraintElement::generic_solve_group(
-            cfm_factor,
+            self.velocity_constraint.cfm_factor,
             elements,
             jacobians,
             &self.velocity_constraint.dir1,
@@ -364,7 +372,7 @@ impl GenericVelocityConstraint {
         self.velocity_constraint.writeback_impulses(manifolds_all);
     }
 
-    pub fn remove_bias_from_rhs(&mut self) {
-        self.velocity_constraint.remove_bias_from_rhs();
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.velocity_constraint.remove_cfm_and_bias_from_rhs();
     }
 }
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs
index 16648c4..91bb9fb 100644
--- a/src/dynamics/solver/generic_velocity_ground_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs
@@ -32,6 +32,7 @@ impl GenericVelocityGroundConstraint {
         jacobian_id: &mut usize,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -85,12 +86,14 @@ impl GenericVelocityGroundConstraint {
             .enumerate()
         {
             let chunk_j_id = *jacobian_id;
+            let mut is_fast_contact = false;
             let mut constraint = VelocityGroundConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
                 tangent1: tangents1[0],
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2: mprops2.effective_inv_mass,
+                cfm_factor,
                 limit: 0.0,
                 mj_lambda2,
                 manifold_id,
@@ -130,16 +133,20 @@ impl GenericVelocityGroundConstraint {
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
 
-                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1);
+                    let dvel = (vel1 - vel2).dot(&force_dir1);
+                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    is_fast_contact =
+                        is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5);
+
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2: na::zero(), // Unused for generic constraints.
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r,
@@ -181,6 +188,8 @@ impl GenericVelocityGroundConstraint {
                 }
             }
 
+            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+
             let constraint = GenericVelocityGroundConstraint {
                 velocity_constraint: constraint,
                 j_id: chunk_j_id,
@@ -198,7 +207,6 @@ impl GenericVelocityGroundConstraint {
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         jacobians: &DVector<Real>,
         generic_mj_lambdas: &mut DVector<Real>,
         solve_restitution: bool,
@@ -209,7 +217,7 @@ impl GenericVelocityGroundConstraint {
         let elements = &mut self.velocity_constraint.elements
             [..self.velocity_constraint.num_contacts as usize];
         VelocityGroundConstraintElement::generic_solve_group(
-            cfm_factor,
+            self.velocity_constraint.cfm_factor,
             elements,
             jacobians,
             self.velocity_constraint.limit,
@@ -226,7 +234,7 @@ impl GenericVelocityGroundConstraint {
         self.velocity_constraint.writeback_impulses(manifolds_all);
     }
 
-    pub fn remove_bias_from_rhs(&mut self) {
-        self.velocity_constraint.remove_bias_from_rhs();
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.velocity_constraint.remove_cfm_and_bias_from_rhs();
     }
 }
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index ab34a42..3db1cdc 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/src/dynamics/solver/parallel_velocity_solver.rs
@@ -45,7 +45,6 @@ impl ParallelVelocitySolver {
         let joint_descs = &joint_constraints.constraint_descs[..];
         let mut target_num_desc = 0;
         let mut shift = 0;
-        let cfm_factor = params.cfm_factor();
 
         // Each thread will concurrently grab thread.batch_size constraint desc to
         // solve. If the batch size is large enough to cross the boundary of
@@ -122,7 +121,6 @@ impl ParallelVelocitySolver {
                 // Solve rigid-body contacts.
                 solve!(
                     contact_constraints,
-                    cfm_factor,
                     &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
                     &mut self.generic_mj_lambdas,
@@ -135,7 +133,6 @@ impl ParallelVelocitySolver {
                 // Solve generic rigid-body contacts.
                 solve!(
                     contact_constraints,
-                    cfm_factor,
                     &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
                     &mut self.generic_mj_lambdas,
@@ -148,7 +145,6 @@ impl ParallelVelocitySolver {
                 if solve_friction {
                     solve!(
                         contact_constraints,
-                        cfm_factor,
                         &contact_constraints.generic_jacobians,
                         &mut self.mj_lambdas,
                         &mut self.generic_mj_lambdas,
@@ -173,7 +169,6 @@ impl ParallelVelocitySolver {
             for _ in 0..remaining_friction_iterations {
                 solve!(
                     contact_constraints,
-                    cfm_factor,
                     &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
                     &mut self.generic_mj_lambdas,
@@ -265,7 +260,6 @@ impl ParallelVelocitySolver {
 
                 solve!(
                     contact_constraints,
-                    cfm_factor,
                     &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
                     &mut self.generic_mj_lambdas,
@@ -277,7 +271,6 @@ impl ParallelVelocitySolver {
 
                 solve!(
                     contact_constraints,
-                    cfm_factor,
                     &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
                     &mut self.generic_mj_lambdas,
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 23d4b97..7d2294e 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -47,21 +47,20 @@ impl AnyVelocityConstraint {
 
     pub fn remove_bias_from_rhs(&mut self) {
         match self {
-            AnyVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(),
-            AnyVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(),
+            AnyVelocityConstraint::Nongrouped(c) => c.remove_cfm_and_bias_from_rhs(),
+            AnyVelocityConstraint::NongroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::Grouped(c) => c.remove_bias_from_rhs(),
+            AnyVelocityConstraint::Grouped(c) => c.remove_cfm_and_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::GroupedGround(c) => c.remove_bias_from_rhs(),
-            AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_bias_from_rhs(),
-            AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_bias_from_rhs(),
+            AnyVelocityConstraint::GroupedGround(c) => c.remove_cfm_and_bias_from_rhs(),
+            AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_cfm_and_bias_from_rhs(),
+            AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_cfm_and_bias_from_rhs(),
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         jacobians: &DVector<Real>,
         mj_lambdas: &mut [DeltaVel<Real>],
         generic_mj_lambdas: &mut DVector<Real>,
@@ -70,21 +69,20 @@ impl AnyVelocityConstraint {
     ) {
         match self {
             AnyVelocityConstraint::NongroupedGround(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
+                c.solve(mj_lambdas, solve_restitution, solve_friction)
             }
             AnyVelocityConstraint::Nongrouped(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
+                c.solve(mj_lambdas, solve_restitution, solve_friction)
             }
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::GroupedGround(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
+                c.solve(mj_lambdas, solve_restitution, solve_friction)
             }
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::Grouped(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
+                c.solve(mj_lambdas, solve_restitution, solve_friction)
             }
             AnyVelocityConstraint::NongroupedGeneric(c) => c.solve(
-                cfm_factor,
                 jacobians,
                 mj_lambdas,
                 generic_mj_lambdas,
@@ -92,7 +90,6 @@ impl AnyVelocityConstraint {
                 solve_friction,
             ),
             AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve(
-                cfm_factor,
                 jacobians,
                 generic_mj_lambdas,
                 solve_restitution,
@@ -124,6 +121,7 @@ pub(crate) struct VelocityConstraint {
     pub tangent1: Vector<Real>, // One of the friction force directions.
     pub im1: Vector<Real>,
     pub im2: Vector<Real>,
+    pub cfm_factor: Real,
     pub limit: Real,
     pub mj_lambda1: usize,
     pub mj_lambda2: usize,
@@ -153,6 +151,7 @@ impl VelocityConstraint {
     ) {
         assert_eq!(manifold.data.relative_dominance, 0);
 
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -163,6 +162,7 @@ impl VelocityConstraint {
         let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
         let rb2 = &bodies[handle2];
         let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
+        let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
 
         let mj_lambda1 = rb1.ids.active_set_offset;
         let mj_lambda2 = rb2.ids.active_set_offset;
@@ -180,6 +180,8 @@ impl VelocityConstraint {
             .chunks(MAX_MANIFOLD_POINTS)
             .enumerate()
         {
+            let mut is_fast_contact = false;
+
             #[cfg(not(target_arch = "wasm32"))]
             let mut constraint = VelocityConstraint {
                 dir1: force_dir1,
@@ -188,6 +190,7 @@ impl VelocityConstraint {
                 elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im1: mprops1.effective_inv_mass,
                 im2: mprops2.effective_inv_mass,
+                cfm_factor,
                 limit: 0.0,
                 mj_lambda1,
                 mj_lambda2,
@@ -235,6 +238,7 @@ impl VelocityConstraint {
                 }
                 constraint.im1 = mprops1.effective_inv_mass;
                 constraint.im2 = mprops2.effective_inv_mass;
+                constraint.cfm_factor = cfm_factor;
                 constraint.limit = 0.0;
                 constraint.mj_lambda1 = mj_lambda1;
                 constraint.mj_lambda2 = mj_lambda2;
@@ -280,10 +284,14 @@ impl VelocityConstraint {
                     let rhs_bias = /* is_resting
                         * */  erp_inv_dt
                         * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);