CCD improvements

- Fix bug where the CCD thickness wasn’t initialized properly.
- Fix bug where the contact compliance would result in unwanted tunelling, despite CCD being enabled.

1 files changed, 3 insertions, 1 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 06a727a..3b2ed86 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -93,6 +93,7 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: WReal> {
     pub rhs_wo_bias: N,
     pub impulse: N,
     pub r: N,
+    pub cfm: N,
 }
 
 impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
@@ -103,6 +104,7 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
             rhs_wo_bias: na::zero(),
             impulse: na::zero(),
             r: na::zero(),
+            cfm: na::one(),
         }
     }
 
@@ -117,7 +119,7 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
         AngVector<N>: WDot<AngVector<N>, Result = N>,
     {
         let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
-        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
+        let new_impulse = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero());
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;