Improve cfm configuration using the critical damping factor

1 files changed, 4 insertions, 2 deletions

diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs
index 80c97ab..f645f04 100644
--- a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs
@@ -75,6 +75,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
     #[inline]
     pub fn generic_solve(
         &mut self,
+        cfm_factor: Real,
         j_id2: usize,
         jacobians: &DVector<Real>,
         ndofs2: usize,
@@ -86,7 +87,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
             .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
             + self.rhs;
 
-        let new_impulse = (self.impulse - self.r * dvel).max(0.0);
+        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
@@ -101,6 +102,7 @@ impl VelocityGroundConstraintNormalPart<Real> {
 impl VelocityGroundConstraintElement<Real> {
     #[inline]
     pub fn generic_solve_group(
+        cfm_factor: Real,
         elements: &mut [Self],
         jacobians: &DVector<Real>,
         limit: Real,
@@ -121,7 +123,7 @@ impl VelocityGroundConstraintElement<Real> {
             for element in elements.iter_mut() {
                 element
                     .normal_part
-                    .generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas);
+                    .generic_solve(cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas);
                 nrm_j_id += j_step;
             }
         }