Improve cfm configuration using the critical damping factor

.gitignore

@@ -6,4 +6,4 @@ target
 .DS_Store
 package-lock.json
 **/*.csv
-.vscode/
+.history

src/dynamics/integration_parameters.rs

@@ -29,13 +29,11 @@ pub struct IntegrationParameters {
     /// A good non-zero value is around `0.2`.
     /// (default `0.0`).
     pub erp: Real,
-
-    /// 0-1: multiplier applied to each accumulated impulse during  constraints resolution.
-    /// This is similar to the concept of CFN (Constraint Force Mixing) except that it is
-    /// a multiplicative factor instead of an additive factor.
-    /// Larger values lead to stiffer constraints (1.0 being completely stiff).
-    /// Smaller values lead to more compliant constraints.
-    pub delassus_inv_factor: Real,
+    /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
+    /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
+    /// before stabilization).
+    /// (default `0.25`).
+    pub damping_ratio: Real,
 
     /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
     pub allowed_linear_error: Real,
@@ -89,10 +87,42 @@ impl IntegrationParameters {
         }
     }
 
-    /// Convenience: `erp / dt`
-    #[inline]
-    pub(crate) fn erp_inv_dt(&self) -> Real {
-        self.erp * self.inv_dt()
+    /// The ERP coefficient, multiplied by the inverse timestep length.
+    pub fn erp_inv_dt(&self) -> Real {
+        0.8 / self.dt
+    }
+
+    /// The CFM factor to be used in the constraints resolution.
+    pub fn cfm_factor(&self) -> Real {
+        // Compute CFM assuming a critically damped spring multiplied by the dampingratio.
+        let inv_erp_minus_one = 1.0 / self.erp - 1.0;
+
+        // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
+        //     / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
+        // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
+        //     / (dt * inv_erp_minus_one);
+        // let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
+        // NOTE: This simplies to cfm = cfm_coefff / projected_mass:
+        let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
+            / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio);
+
+        // Furthermore, we use this coefficient inside of the impulse resolution.
+        // Surprisingly, several simplifications happen there.
+        // Let `m` the projected mass of the constraint.
+        // Let `m’` the projected mass that includes CFM: `m’ = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)`
+        // We have:
+        // new_impulse = old_impulse - m’ (delta_vel - cfm * old_impulse)
+        //             = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse)
+        //             = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel
+        //             = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff)
+        //             = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel)
+        // So, setting cfm_factor = 1 / (1 + cfm_coeff).
+        // We obtain:
+        // new_impulse = cfm_factor * (old_impulse - m * delta_vel)
+        //
+        // The value returned by this function is this cfm_factor that can be used directly
+        // in the constraints solver.
+        1.0 / (1.0 + cfm_coeff)
     }
 }
 
@@ -103,14 +133,14 @@ impl Default for IntegrationParameters {
             min_ccd_dt: 1.0 / 60.0 / 100.0,
             velocity_solve_fraction: 1.0,
             erp: 0.8,
-            delassus_inv_factor: 0.75,
+            damping_ratio: 0.25,
             allowed_linear_error: 0.001, // 0.005
             prediction_distance: 0.002,
             max_velocity_iterations: 4,
             max_velocity_friction_iterations: 8,
             max_stabilization_iterations: 1,
             interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability.
-            // FIXME: what is the optimal value for min_island_size?
+            // TODO: what is the optimal value for min_island_size?
             // It should not be too big so that we don't end up with
             // huge islands that don't fit in cache.
             // However we don't want it to be too small and end up with

src/dynamics/solver/generic_velocity_constraint.rs

@@ -23,6 +23,7 @@ pub(crate) enum AnyGenericVelocityConstraint {
 impl AnyGenericVelocityConstraint {
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         jacobians: &DVector<Real>,
         mj_lambdas: &mut [DeltaVel<Real>],
         generic_mj_lambdas: &mut DVector<Real>,
@@ -31,13 +32,15 @@ impl AnyGenericVelocityConstraint {
     ) {
         match self {
             AnyGenericVelocityConstraint::Nongrouped(c) => c.solve(
+                cfm_factor,
                 jacobians,
                 mj_lambdas,
                 generic_mj_lambdas,
                 solve_restitution,
                 solve_friction,
             ),
             AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve(
+                cfm_factor,
                 jacobians,
                 generic_mj_lambdas,
                 solve_restitution,
@@ -379,6 +382,7 @@ impl GenericVelocityConstraint {
 
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         jacobians: &DVector<Real>,
         mj_lambdas: &mut [DeltaVel<Real>],
         generic_mj_lambdas: &mut DVector<Real>,
@@ -400,6 +404,7 @@ impl GenericVelocityConstraint {
         let elements = &mut self.velocity_constraint.elements
             [..self.velocity_constraint.num_contacts as usize];
         VelocityConstraintElement::generic_solve_group(
+            cfm_factor,
             elements,
             jacobians,
             &self.velocity_constraint.dir1,

src/dynamics/solver/generic_velocity_constraint_element.rs

@@ -243,6 +243,7 @@ impl VelocityConstraintNormalPart<Real> {
     #[inline]
     pub fn generic_solve(
         &mut self,
+        cfm_factor: Real,
         j_id: usize,
         jacobians: &DVector<Real>,
         dir1: &Vector<Real>,
@@ -261,7 +262,7 @@ impl VelocityConstraintNormalPart<Real> {
             + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
             + 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;
 
@@ -291,6 +292,7 @@ impl VelocityConstraintNormalPart<Real> {
 impl VelocityConstraintElement<Real> {
     #[inline]
     pub fn generic_solve_group(
+        cfm_factor: Real,
         elements: &mut [Self],
         jacobians: &DVector<Real>,
         dir1: &Vector<Real>,
@@ -318,8 +320,8 @@ impl VelocityConstraintElement<Real> {
 
             for element in elements.iter_mut() {
                 element.normal_part.generic_solve(
-                    nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, mj_lambda2,
-                    mj_lambdas,
+                    cfm_factor, nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1,
+                    mj_lambda2, mj_lambdas,
                 );
                 nrm_j_id += j_step;
             }

src/dynamics/solver/generic_velocity_ground_constraint.rs

@@ -210,6 +210,7 @@ impl GenericVelocityGroundConstraint {
 
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         jacobians: &DVector<Real>,
         generic_mj_lambdas: &mut DVector<Real>,
         solve_restitution: bool,
@@ -220,6 +221,7 @@ impl GenericVelocityGroundConstraint {
         let elements = &mut self.velocity_constraint.elements
             [..self.velocity_constraint.num_contacts as usize];
         VelocityGroundConstraintElement::generic_solve_group(
+            cfm_factor,
             elements,
             jacobians,
             self.velocity_constraint.limit,

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;
             }
         }

src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs

@@ -524,6 +524,6 @@ impl JointGenericVelocityGroundConstraint {
     }
 
     pub fn remove_bias_from_rhs(&mut self) {
-        self.rhs = self.rhs_wo_bias;
+        self.rhs = &mut self.rhs_wo_bias;
     }
 }

src/dynamics/solver/velocity_constraint.rs

@@ -55,23 +55,26 @@ impl AnyVelocityConstraint {
 
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
     ) {
         match self {
             AnyVelocityConstraint::NongroupedGround(c) => {
-                c.solve(mj_lambdas, solve_normal, solve_friction)
+                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
             }
             AnyVelocityConstraint::Nongrouped(c) => {
-                c.solve(mj_lambdas, solve_normal, solve_friction)
+                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
             }
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::GroupedGround(c) => {
-                c.solve(mj_lambdas, solve_normal, solve_friction)
+                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
             }
             #[cfg(feature = "simd-is-enabled")]
-            AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas, solve_normal, solve_friction),
+            AnyVelocityConstraint::Grouped(c) => {
+                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
+            }
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
@@ -236,7 +239,7 @@ impl VelocityConstraint {
                         .transform_vector(dp2.gcross(-force_dir1));
 
                     let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
-                    let r = params.delassus_inv_factor
+                    let projected_mass = 1.0
                         / (force_dir1.dot(&imsum.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
@@ -251,14 +254,13 @@ impl VelocityConstraint {
                     let rhs_bias = /* is_resting
                         * */  erp_inv_dt
                         * (manifold_point.dist + params.allowed_linear_error).min(0.0);
-
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
                         rhs: rhs_wo_bias + rhs_bias,
                         rhs_wo_bias,
-                        impulse: 0.0,
-                        r,
+                        impulse: na::zero(),
+                        r: projected_mass,
                     };
                 }
 
@@ -310,6 +312,7 @@ impl VelocityConstraint {
 
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -318,6 +321,7 @@ impl VelocityConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         VelocityConstraintElement::solve_group(
+            cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]

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);
             }
         }
 

src/dynamics/solver/velocity_constraint_wide.rs

@@ -48,9 +48,8 @@ impl WVelocityConstraint {
 
         let inv_dt = SimdReal::splat(params.inv_dt());
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
-        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
-        let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
+        let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
 
         let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
         let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
@@ -121,7 +120,6 @@ impl WVelocityConstraint {
                 let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
                 let tangent_velocity =
                     Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
-
                 let dp1 = point - world_com1;
                 let dp2 = point - world_com2;
 
@@ -137,10 +135,11 @@ impl WVelocityConstraint {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
                     let imsum = im1 + im2;
-                    let r = delassus_inv_factor
+                    let projected_mass = SimdReal::splat(1.0)
                         / (force_dir1.dot(&imsum.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
+
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
                     let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
@@ -154,8 +153,8 @@ impl WVelocityConstraint {
                         gcross2,
                         rhs: rhs_wo_bias + rhs_bias,
                         rhs_wo_bias,
-                        impulse: na::zero(),
-                        r,
+                        impulse: SimdReal::splat(0.0),
+                        r: projected_mass,
                     };
                 }
 
@@ -202,6 +201,7 @@ impl WVelocityConstraint {
 
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -221,6 +221,7 @@ impl WVelocityConstraint {
         };
 
         VelocityConstraintElement::solve_group(
+            SimdReal::splat(cfm_factor),
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]