From 78c8bc6cdef26d14c57d0eeb23188cba592961bf Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Sun, 23 Jan 2022 16:50:02 +0100
Subject: Improve cfm configuration using the critical damping factor

---
 src/dynamics/solver/velocity_constraint_element.rs | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

(limited to 'src/dynamics/solver/velocity_constraint_element.rs')

diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index b0f8087..2d2221d 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -131,6 +131,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
     #[inline]
     pub fn solve(
         &mut self,
+        cfm_factor: N,
         dir1: &Vector<N>,
         im1: &Vector<N>,
         im2: &Vector<N>,
@@ -143,7 +144,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
             - dir1.dot(&mj_lambda2.linear)
             + self.gcross2.gdot(mj_lambda2.angular)
             + self.rhs;
-        let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
+        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
@@ -171,6 +172,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
 
     #[inline]
     pub fn solve_group(
+        cfm_factor: N,
         elements: &mut [Self],
         dir1: &Vector<N>,
         #[cfg(feature = "dim3")] tangent1: &Vector<N>,
@@ -191,7 +193,7 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
             for element in elements.iter_mut() {
                 element
                     .normal_part
-                    .solve(&dir1, im1, im2, mj_lambda1, mj_lambda2);
+                    .solve(cfm_factor, &dir1, im1, im2, mj_lambda1, mj_lambda2);
             }
         }
 
-- 
cgit