Merge pull request #334 from dimforge/fixes

Some CCD and debug-render improvements

8 files changed, 115 insertions, 60 deletions

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);
+
+                    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: projected_mass,
@@ -329,6 +337,8 @@ impl VelocityConstraint {
                 }
             }
 
+            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+
             #[cfg(not(target_arch = "wasm32"))]
             if let Some(at) = insert_at {
                 out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint);
@@ -340,7 +350,6 @@ impl VelocityConstraint {
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -349,7 +358,7 @@ impl VelocityConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         VelocityConstraintElement::solve_group(
-            cfm_factor,
+            self.cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
@@ -386,7 +395,8 @@ impl VelocityConstraint {
         }
     }
 
-    pub fn remove_bias_from_rhs(&mut self) {
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.cfm_factor = 1.0;
         for elt in &mut self.elements {
             elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
         }
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 68246da..b8e0a7e 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -12,6 +12,7 @@ use crate::math::{
 use crate::utils::WBasis;
 use crate::utils::{self, WAngularInertia, WCross, WDot};
 use num::Zero;
+use parry::math::SimdBool;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
@@ -23,6 +24,7 @@ pub(crate) struct WVelocityConstraint {
     pub num_contacts: u8,
     pub im1: Vector<SimdReal>,
     pub im2: Vector<SimdReal>,
+    pub cfm_factor: SimdReal,
     pub limit: SimdReal,
     pub mj_lambda1: [usize; SIMD_WIDTH],
     pub mj_lambda2: [usize; SIMD_WIDTH],
@@ -43,6 +45,8 @@ impl WVelocityConstraint {
             assert_eq!(manifolds[ii].data.relative_dominance, 0);
         }
 
+        let cfm_factor = SimdReal::splat(params.cfm_factor());
+        let dt = SimdReal::splat(params.dt);
         let inv_dt = SimdReal::splat(params.inv_dt());
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
@@ -58,6 +62,10 @@ impl WVelocityConstraint {
         let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
         let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
 
+        let ccd_thickness1 = SimdReal::from(gather![|ii| bodies[handles1[ii]].ccd.ccd_thickness]);
+        let ccd_thickness2 = SimdReal::from(gather![|ii| bodies[handles2[ii]].ccd.ccd_thickness]);
+        let ccd_thickness = ccd_thickness1 + ccd_thickness2;
+
         let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
         let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);
         let ii1: AngularInertia<SimdReal> =
@@ -91,6 +99,7 @@ impl WVelocityConstraint {
                 gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
+            let mut is_fast_contact = SimdBool::splat(false);
             let mut constraint = WVelocityConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
@@ -98,6 +107,7 @@ impl WVelocityConstraint {
                 elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im1,
                 im2,
+                cfm_factor,
                 limit: SimdReal::splat(0.0),
                 mj_lambda1,
                 mj_lambda2,
@@ -147,6 +157,10 @@ impl WVelocityConstraint {
                         .simd_clamp(-max_penetration_correction, SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    is_fast_contact =
+                        is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
+
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
@@ -189,6 +203,8 @@ impl WVelocityConstraint {
                 }
             }
 
+            constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+
             if let Some(at) = insert_at {
                 out_constraints[at + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::Grouped(constraint);
@@ -200,7 +216,6 @@ impl WVelocityConstraint {
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -220,7 +235,7 @@ impl WVelocityConstraint {
         };
 
         VelocityConstraintElement::solve_group(
-            SimdReal::splat(cfm_factor),
+            self.cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
@@ -270,7 +285,8 @@ impl WVelocityConstraint {
         }
     }
 
-    pub fn remove_bias_from_rhs(&mut self) {
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.cfm_factor = SimdReal::splat(1.0);
         for elt in &mut self.elements {
             elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
         }
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 39a2a19..19bf7e6 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -17,6 +17,7 @@ pub(crate) struct VelocityGroundConstraint {
     #[cfg(feature = "dim3")]
     pub tangent1: Vector<Real>, // One of the friction force directions.
     pub im2: Vector<Real>,
+    pub cfm_factor: Real,
     pub limit: Real,
     pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
 
@@ -34,6 +35,7 @@ impl VelocityGroundConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -80,6 +82,7 @@ impl VelocityGroundConstraint {
                 tangent1: tangents1[0],
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2: mprops2.effective_inv_mass,
+                cfm_factor,
                 limit: 0.0,
                 mj_lambda2,
                 manifold_id,
@@ -125,6 +128,7 @@ impl VelocityGroundConstraint {
                     constraint.tangent1 = tangents1[0];
                 }
                 constraint.im2 = mprops2.effective_inv_mass;
+                constraint.cfm_factor = cfm_factor;
                 constraint.limit = 0.0;
                 constraint.mj_lambda2 = mj_lambda2;
                 constraint.manifold_id = manifold_id;
@@ -132,6 +136,8 @@ impl VelocityGroundConstraint {
                 constraint.num_contacts = manifold_points.len() as u8;
             }
 
+            let mut is_fast_contact = false;
+
             for k in 0..manifold_points.len() {
                 let manifold_point = &manifold_points[k];
                 let dp2 = manifold_point.point - mprops2.world_com;
@@ -156,17 +162,21 @@ impl VelocityGroundConstraint {
                     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 + params.allowed_linear_error).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,
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r: projected_mass,
@@ -206,6 +216,8 @@ impl VelocityGroundConstraint {
                 }
             }
 
+            constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
+
             #[cfg(not(target_arch = "wasm32"))]
             if let Some(at) = insert_at {
                 out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint);
@@ -217,7 +229,6 @@ impl VelocityGroundConstraint {
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -225,7 +236,7 @@ impl VelocityGroundConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         VelocityGroundConstraintElement::solve_group(
-            cfm_factor,
+            self.cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
@@ -260,7 +271,8 @@ impl VelocityGroundConstraint {
         }
     }
 
-    pub fn remove_bias_from_rhs(&mut self) {
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.cfm_factor = 1.0;
         for elt in &mut self.elements {
             elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
         }
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 37f4688..25bb30d 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -13,6 +13,7 @@ use crate::math::{
 use crate::utils::WBasis;
 use crate::utils::{self, WAngularInertia, WCross, WDot};
 use num::Zero;
+use parry::math::SimdBool;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
@@ -23,6 +24,7 @@ pub(crate) struct WVelocityGroundConstraint {
     pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub im2: Vector<SimdReal>,
+    pub cfm_factor: SimdReal,
     pub limit: SimdReal,
     pub mj_lambda2: [usize; SIMD_WIDTH],
     pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
@@ -38,6 +40,8 @@ impl WVelocityGroundConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = SimdReal::splat(params.cfm_factor());
+        let dt = SimdReal::splat(params.dt);
         let inv_dt = SimdReal::splat(params.inv_dt());
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
@@ -65,11 +69,12 @@ impl WVelocityGroundConstraint {
                 .unwrap_or_else(Point::origin)
         }]);
 
-        let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
-            gather![|ii| &bodies[handles2[ii].unwrap()].vels];
-        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids];
-        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
-            gather![|ii| &bodies[handles2[ii].unwrap()].mprops];
+        let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]];
+
+        let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels];
+        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids];
+        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops];
+        let ccd_thickness = SimdReal::from(gather![|ii| bodies2[ii].ccd.ccd_thickness]);
 
         let flipped_sign = SimdReal::from(flipped);
 
@@ -101,12 +106,14 @@ impl WVelocityGroundConstraint {
             let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
+            let mut is_fast_contact = SimdBool::splat(false);
             let mut constraint = WVelocityGroundConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
                 tangent1: tangents1[0],
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2,
+                cfm_factor,
                 limit: SimdReal::splat(0.0),
                 mj_lambda2,
                 manifold_id,
@@ -152,9 +159,13 @@ impl WVelocityGroundConstraint {
                         .simd_clamp(-max_penetration_correction, SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    is_fast_contact =
+                        is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
+
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r: projected_mass,
@@ -187,6 +198,8 @@ impl WVelocityGroundConstraint {
                 }
             }
 
+            constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+
             if let Some(at) = insert_at {
                 out_constraints[at + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::GroupedGround(constraint);
@@ -198,7 +211,6 @@ impl WVelocityGroundConstraint {
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -211,7 +223,7 @@ impl WVelocityGroundConstraint {
         };
 
         VelocityGroundConstraintElement::solve_group(
-            SimdReal::splat(cfm_factor),
+            self.cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
@@ -256,7 +268,8 @@ impl WVelocityGroundConstraint {
         }
     }
 
-    pub fn remove_bias_from_rhs(&mut self) {
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.cfm_factor = SimdReal::splat(1.0);
         for elt in &mut self.elements {
             elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
         }
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index d5dc77d..d15ea68 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -35,7 +35,6 @@ impl VelocitySolver {
         joint_constraints: &mut [AnyJointVelocityConstraint],
         generic_joint_jacobians: &DVector<Real>,
     ) {
-        let cfm_factor = params.cfm_factor();
         self.mj_lambdas.clear();
         self.mj_lambdas
             .resize(islands.active_island(island_id).len(), DeltaVel::zero());
@@ -86,7 +85,6 @@ impl VelocitySolver {
 
             for constraint in &mut *contact_constraints {
                 constraint.solve(
-                    cfm_factor,
                     generic_contact_jacobians,
                     &mut self.mj_lambdas[..],
                     &mut self.generic_mj_lambdas,
@@ -98,7 +96,6 @@ impl VelocitySolver {
             if solve_friction {
                 for constraint in &mut *contact_constraints {
                     constraint.solve(
-                        cfm_factor,
                         generic_contact_jacobians,
                         &mut self.mj_lambdas[..],
                         &mut self.generic_mj_lambdas,
@@ -121,7 +118,6 @@ impl VelocitySolver {
         for _ in 0..remaining_friction_iterations {
             for constraint in &mut *contact_constraints {
                 constraint.solve(
-                    cfm_factor,
                     generic_contact_jacobians,
                     &mut self.mj_lambdas[..],
                     &mut self.generic_mj_lambdas,
@@ -168,6 +164,7 @@ impl VelocitySolver {
                     &rb.pos.position,
                     &rb.mprops.local_mprops.local_com,
                 );
+                rb.integrated_vels = new_vels;
                 rb.pos = new_pos;
             }
         }
@@ -190,7 +187,6 @@ impl VelocitySolver {
 
             for constraint in &mut *contact_constraints {
                 constraint.solve(
-                    1.0,
                     generic_contact_jacobians,
                     &mut self.mj_lambdas[..],
                     &mut self.generic_mj_lambdas,
@@ -201,7 +197,6 @@ impl VelocitySolver {
 
             for constraint in &mut *contact_constraints {
                 constraint.solve(
-                    1.0,
                     generic_contact_jacobians,
                     &mut self.mj_lambdas[..],
                     &mut self.generic_mj_lambdas,