More experiments with velocity-based error correction.

16 files changed, 120 insertions, 42 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 4840ec0..8c773ff 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -20,10 +20,10 @@ pub struct IntegrationParameters {
     pub return_after_ccd_substep: bool,
     /// The Error Reduction Parameter in `[0, 1]` is the proportion of
     /// the positional error to be corrected at the position level each time step (default: `0.0`).
-    pub positionErp: Real,
+    pub position_erp: Real,
     /// The Error Reduction Parameter in `[0, 1]` is the proportion of
     /// the positional error to be corrected at the velocity level at each time step (default: `0.005`).
-    pub velocityErp: Real,
+    pub velocity_erp: Real,
     /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of
     /// the positional error to be corrected at each time step (default: `0.2`).
     pub joint_erp: Real,
@@ -116,7 +116,7 @@ impl IntegrationParameters {
         IntegrationParameters {
             dt,
             //            multithreading_enabled,
-            positionErp: erp,
+            position_erp: erp,
             joint_erp,
             warmstart_coeff,
             restitution_velocity_threshold,
@@ -139,7 +139,7 @@ impl IntegrationParameters {
             return_after_ccd_substep,
             multiple_ccd_substep_sensor_events_enabled,
             ccd_on_penetration_enabled,
-            velocityErp: 0.005,
+            velocity_erp: 0.005,
         }
     }
 
@@ -189,8 +189,8 @@ impl Default for IntegrationParameters {
             dt: 1.0 / 60.0,
             //        multithreading_enabled:             true,
             return_after_ccd_substep: false,
-            positionErp: 0.0,
-            velocityErp: 0.005,
+            position_erp: 0.0,
+            velocity_erp: 0.005,
             joint_erp: 0.2,
             warmstart_coeff: 1.0,
             restitution_velocity_threshold: 1.0,
diff --git a/src/dynamics/solver/constraint_regularization.rs b/src/dynamics/solver/constraint_regularization.rs
new file mode 100644
index 0000000..4c67145
--- /dev/null
+++ b/src/dynamics/solver/constraint_regularization.rs
@@ -0,0 +1,34 @@
+use crate::math::Real;
+use na::RealField;
+
+pub struct SpringRegularization {
+    pub angular_frequency: Real,
+    pub damping: Real,
+}
+
+impl Default for SpringRegularization {
+    fn default() -> Self {
+        SpringRegularization {
+            angular_frequency: 30.0 * Real::two_pi(),
+            damping: 1.0,
+        }
+    }
+}
+
+impl SpringRegularization {
+    pub fn erp_cfm_impulse_scale(&self, dt: Real) -> (Real, Real, Real) {
+        let freq_dt = self.angular_frequency * dt;
+        let erp = self.angular_frequency / (freq_dt + self.damping * 2.0);
+        let extra = 1.0 / (freq_dt * (freq_dt + self.damping * 2.0));
+        let cfm = 1.0 / (1.0 + extra);
+        let impulse_scale = extra * cfm;
+
+        let kd = 1.0;
+        let kp = 10.0;
+        let erp = 0.2 / dt; // kp / (dt * kp + kd);
+        let cfm = 1.0e-5 / dt; // 1.0 / (dt * kp + kd);
+        let impulse_scale = 0.9;
+
+        (erp, cfm, impulse_scale)
+    }
+}
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index 68074e1..8b7e587 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -60,7 +60,7 @@ impl IslandSolver {
         bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt));
         counters.solver.velocity_update_time.pause();
 
-        if params.positionErp != 0.0 {
+        if params.position_erp != 0.0 {
             if manifold_indices.len() != 0 || joint_indices.len() != 0 {
                 counters.solver.position_resolution_time.resume();
                 self.position_solver.solve(
diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs
index 2291615..5a0d0c7 100644
--- a/src/dynamics/solver/mod.rs
+++ b/src/dynamics/solver/mod.rs
@@ -14,6 +14,7 @@ pub(self) use self::position_solver::PositionSolver;
 pub(self) use self::solver_constraints::SolverConstraints;
 #[cfg(not(feature = "parallel"))]
 pub(self) use self::velocity_solver::VelocitySolver;
+pub(self) use constraint_regularization::*;
 pub(self) use delta_vel::DeltaVel;
 pub(self) use interaction_groups::*;
 pub(self) use joint_constraint::*;
@@ -37,6 +38,7 @@ pub(self) use velocity_ground_constraint_wide_with_manifold_friction::*;
 pub(self) use velocity_ground_constraint_with_manifold_friction::*;
 
 mod categorization;
+mod constraint_regularization;
 mod delta_vel;
 mod interaction_groups;
 #[cfg(not(feature = "parallel"))]
diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
index dc5b218..ecf8286 100644
--- a/src/dynamics/solver/position_constraint.rs
+++ b/src/dynamics/solver/position_constraint.rs
@@ -93,7 +93,7 @@ impl PositionConstraint {
                 ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 num_contacts: manifold_points.len() as u8,
-                erp: params.positionErp,
+                erp: params.position_erp,
                 max_linear_correction: params.max_linear_correction,
             };
 
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
index 8701bfc..b7918c5 100644
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ b/src/dynamics/solver/position_constraint_wide.rs
@@ -74,7 +74,7 @@ impl WPositionConstraint {
                 im2,
                 ii1: sqrt_ii1.squared(),
                 ii2: sqrt_ii2.squared(),
-                erp: SimdReal::splat(params.positionErp),
+                erp: SimdReal::splat(params.position_erp),
                 max_linear_correction: SimdReal::splat(params.max_linear_correction),
                 num_contacts: num_points as u8,
             };
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index e69d8ea..1e5a2ff 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs
@@ -66,7 +66,7 @@ impl PositionGroundConstraint {
                 im2: rb2.effective_inv_mass,
                 ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
                 num_contacts: manifold_contacts.len() as u8,
-                erp: params.positionErp,
+                erp: params.position_erp,
                 max_linear_correction: params.max_linear_correction,
             };
 
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
index 99a469f..531eead 100644
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ b/src/dynamics/solver/position_ground_constraint_wide.rs
@@ -71,7 +71,7 @@ impl WPositionGroundConstraint {
                 dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
                 im2,
                 ii2: sqrt_ii2.squared(),
-                erp: SimdReal::splat(params.positionErp),
+                erp: SimdReal::splat(params.position_erp),
                 max_linear_correction: SimdReal::splat(params.max_linear_correction),
                 num_contacts: num_points as u8,
             };
diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
index ab8c1e6..1f4cec5 100644
--- a/src/dynamics/solver/solver_constraints.rs
+++ b/src/dynamics/solver/solver_constraints.rs
@@ -135,7 +135,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 true,
             );
 
-            if params.positionErp != 0.0 {
+            if params.position_erp != 0.0 {
                 WPositionConstraint::generate(
                     params,
                     manifolds,
@@ -164,7 +164,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 true,
             );
 
-            if params.positionErp != 0.0 {
+            if params.position_erp != 0.0 {
                 PositionConstraint::generate(
                     params,
                     manifold,
@@ -199,7 +199,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 true,
             );
 
-            if params.positionErp != 0.0 {
+            if params.position_erp != 0.0 {
                 WPositionGroundConstraint::generate(
                     params,
                     manifolds,
@@ -228,7 +228,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
                 true,
             );
 
-            if params.positionErp != 0.0 {
+            if params.position_erp != 0.0 {
                 PositionGroundConstraint::generate(
                     params,
                     manifold,
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index a8384de..97aa0eb 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -152,7 +152,11 @@ impl WVelocityConstraint {
                     let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
                     let rhs_with_restitution = rhs + rhs * restitution;
                     rhs = rhs_with_restitution.select(use_restitution, rhs);
-                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
+
+                    rhs += ((dist.simd_min(SimdReal::splat(0.0))
+                        * SimdReal::splat(params.velocity_erp))
+                        + dist.simd_max(SimdReal::splat(0.0)))
+                        * inv_dt;
 
                     constraint.elements[k].normal_part = WVelocityConstraintElementPart {
                         gcross1,
diff --git a/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs
index d9d3e53..4a1ad82 100644
--- a/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs
+++ b/src/dynamics/solver/velocity_constraint_wide_with_manifold_friction.rs
@@ -1,4 +1,4 @@
-use super::{AnyVelocityConstraint, DeltaVel};
+use super::{AnyVelocityConstraint, DeltaVel, SpringRegularization};
 use crate::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
@@ -44,6 +44,7 @@ pub(crate) struct WVelocityConstraintWithManifoldFriction {
     mj_lambda2: [usize; SIMD_WIDTH],
     manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
     manifold_contact_id: usize,
+    impulse_scale: SimdReal,
 }
 
 impl WVelocityConstraintWithManifoldFriction {
@@ -55,6 +56,9 @@ impl WVelocityConstraintWithManifoldFriction {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
     ) {
+        let (erp, cfm, impulse_scale) =
+            SpringRegularization::default().erp_cfm_impulse_scale(params.dt);
+
         let inv_dt = SimdReal::splat(params.inv_dt());
         let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
         let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
@@ -111,6 +115,7 @@ impl WVelocityConstraintWithManifoldFriction {
                 manifold_id,
                 manifold_contact_id: l,
                 num_contacts: num_points as u8,
+                impulse_scale: SimdReal::splat(impulse_scale),
             };
 
             let mut manifold_center = Point::origin();
@@ -150,17 +155,18 @@ impl WVelocityConstraintWithManifoldFriction {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
                     let r = SimdReal::splat(1.0)
-                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                        / (SimdReal::splat(cfm)
+                            + im1
+                            + im2
+                            + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2));
                     let mut rhs = (vel1 - vel2).dot(&force_dir1);
                     let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
                     let rhs_with_restitution = rhs + rhs * restitution;
                     rhs = rhs_with_restitution.select(use_restitution, rhs);
 
-                    rhs += ((dist.simd_min(SimdReal::splat(0.0))
-                        * SimdReal::splat(params.velocityErp))
-                        + dist.simd_max(SimdReal::splat(0.0)))
-                        * inv_dt;
-                    // rhs += dist.simd_max(SimdReal::splat(-0.01)) * inv_dt;
+                    rhs += dist.simd_min(SimdReal::splat(0.0)) * SimdReal::splat(erp)
+                        + dist.simd_max(SimdReal::splat(0.0)) * inv_dt;
 
                     constraint.normal_parts[k] = WVelocityConstraintPart {
                         gcross1,
@@ -340,7 +346,8 @@ impl WVelocityConstraintWithManifoldFriction {
                 - self.dir1.dot(&mj_lambda2.linear)
                 + elt.gcross2.gdot(mj_lambda2.angular)
                 + elt.rhs;
-            let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero());
+            let new_impulse =
+                (elt.impulse * self.impulse_scale - elt.r * dimpulse).simd_max(SimdReal::zero());
             let dlambda = new_impulse - elt.impulse;
             elt.impulse = new_impulse;
 
diff --git a/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs b/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs
index 66269a4..de88f09 100644
--- a/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs
+++ b/src/dynamics/solver/velocity_constraint_with_manifold_friction.rs
@@ -1,4 +1,4 @@
-use super::DeltaVel;
+use super::{DeltaVel, SpringRegularization};
 use crate::dynamics::solver::{AnyVelocityConstraint, VelocityGroundConstraint};
 use crate::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
@@ -43,6 +43,7 @@ pub(crate) struct VelocityConstraintWithManifoldFriction {
     tangent_parts: [VelocityConstraintPart; DIM - 1],
     twist_part: VelocityConstraintPart,
     twist_weights: [Real; MAX_MANIFOLD_POINTS],
+    impulse_scale: Real,
 }
 
 impl VelocityConstraintWithManifoldFriction {
@@ -60,6 +61,9 @@ impl VelocityConstraintWithManifoldFriction {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
     ) {
+        let (erp, cfm, impulse_scale) =
+            SpringRegularization::default().erp_cfm_impulse_scale(params.dt);
+
         let inv_dt = params.inv_dt();
         let rb1 = &bodies[manifold.data.body_pair.body1];
         let rb2 = &bodies[manifold.data.body_pair.body2];
@@ -89,6 +93,7 @@ impl VelocityConstraintWithManifoldFriction {
                 tangent_parts: [VelocityConstraintPart::zero(); DIM - 1],
                 twist_part: VelocityConstraintPart::zero(),
                 twist_weights: [0.0; MAX_MANIFOLD_POINTS],
+                impulse_scale,
             };
 
             let mut manifold_center = Point::origin();
@@ -115,7 +120,8 @@ impl VelocityConstraintWithManifoldFriction {
                     .transform_vector(dp2.gcross(-force_dir1));
 
                 let r = 1.0
-                    / (rb1.effective_inv_mass
+                    / (cfm
+                        + rb1.effective_inv_mass
                         + rb2.effective_inv_mass
                         + gcross1.gdot(gcross1)
                         + gcross2.gdot(gcross2));
@@ -126,7 +132,11 @@ impl VelocityConstraintWithManifoldFriction {
                     rhs += manifold_point.restitution * rhs
                 }
 
-                rhs += manifold_point.dist.max(0.0) * inv_dt;
+                if manifold_point.dist < 0.0 {
+                    rhs += manifold_point.dist * erp;
+                } else {
+                    rhs += manifold_point.dist * inv_dt;
+                }
 
                 let impulse = manifold_point.data.impulse * warmstart_coeff;
                 tangent_impulses[0] += manifold_point.data.tangent_impulse[0];
@@ -285,7 +295,7 @@ impl VelocityConstraintWithManifoldFriction {
                 - self.dir1.dot(&mj_lambda2.linear)
                 + elt.gcross2.gdot(mj_lambda2.angular)
                 + elt.rhs;
-            let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
+            let new_impulse = (elt.impulse * self.impulse_scale - elt.r * dimpulse).max(0.0);
             let dlambda = new_impulse - elt.impulse;
             elt.impulse = new_impulse;
 
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 20642ec..e773d5e 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -160,7 +160,11 @@ impl VelocityGroundConstraint {
                         rhs += manifold_point.restitution * rhs
                     }
 
-                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    if manifold_point.dist < 0.0 {
+                        rhs += manifold_point.dist * params.velocity_erp * inv_dt;
+                    } else {
+                        rhs += manifold_point.dist * inv_dt;
+                    }
 
                     let impulse = manifold_points[k].data.impulse * warmstart_coeff;
 
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index efb542f..d7221fc 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -144,7 +144,11 @@ impl WVelocityGroundConstraint {
                     let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
                     let rhs_with_restitution = rhs + rhs * restitution;
                     rhs = rhs_with_restitution.select(use_restitution, rhs);
-                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
+
+                    rhs += ((dist.simd_min(SimdReal::splat(0.0))
+                        * SimdReal::splat(params.velocity_erp))
+                        + dist.simd_max(SimdReal::splat(0.0)))
+                        * inv_dt;
 
                     constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
                         gcross2,
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs b/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs
index bf7fecc..39f8d72 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide_with_manifold_friction.rs
@@ -1,4 +1,4 @@
-use super::{AnyVelocityConstraint, DeltaVel};
+use super::{AnyVelocityConstraint, DeltaVel, SpringRegularization};
 use crate::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
@@ -40,6 +40,7 @@ pub(crate) struct WVelocityGroundConstraintWithManifoldFriction {
     mj_lambda2: [usize; SIMD_WIDTH],
     manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
     manifold_contact_id: usize,
+    impulse_scale: SimdReal,
 }
 
 impl WVelocityGroundConstraintWithManifoldFriction {
@@ -51,6 +52,9 @@ impl WVelocityGroundConstraintWithManifoldFriction {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
     ) {
+        let (erp, cfm, impulse_scale) =
+            SpringRegularization::default().erp_cfm_impulse_scale(params.dt);
+
         let inv_dt = SimdReal::splat(params.inv_dt());
         let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
         let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
@@ -109,6 +113,7 @@ impl WVelocityGroundConstraintWithManifoldFriction {
                 manifold_id,
                 manifold_contact_id: l,
                 num_contacts: num_points as u8,
+                impulse_scale: SimdReal::splat(impulse_scale),
             };
 
             let mut manifold_center = Point::origin();
@@ -146,17 +151,15 @@ impl WVelocityGroundConstraintWithManifoldFriction {
                 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
+                    let r =
+                        SimdReal::splat(1.0) / (SimdReal::splat(cfm) + im2 + gcross2.gdot(gcross2));
                     let mut rhs = (vel1 - vel2).dot(&force_dir1);
                     let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
                     let rhs_with_restitution = rhs + rhs * restitution;
                     rhs = rhs_with_restitution.select(use_restitution, rhs);
 
-                    rhs += ((dist.simd_min(SimdReal::splat(0.0))
-                        * SimdReal::splat(params.velocityErp))
-                        + dist.simd_max(SimdReal::splat(0.0)))
-                        * inv_dt;
-                    // rhs += dist.simd_max(SimdReal::splat(-0.1)) * inv_dt;
+                    rhs += dist.simd_min(SimdReal::splat(0.0)) * SimdReal::splat(erp)
+                        + dist.simd_max(SimdReal::splat(0.0)) * inv_dt;
 
                     constraint.normal_parts[k] = WVelocityConstraintPart {
                         gcross2,
@@ -295,7 +298,8 @@ impl WVelocityGroundConstraintWithManifoldFriction {
             let elt = &mut self.normal_parts[i];
             let dimpulse =
                 -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
-            let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdReal::zero());
+            let new_impulse =
+                (elt.impulse * self.impulse_scale - elt.r * dimpulse).simd_max(SimdReal::zero());
             let dlambda = new_impulse - elt.impulse;
             elt.impulse = new_impulse;
 
diff --git a/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs b/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs
index b6a45b1..30ad214 100644
--- a/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_with_manifold_friction.rs
@@ -1,4 +1,4 @@
-use super::DeltaVel;
+use super::{DeltaVel, SpringRegularization};
 use crate::dynamics::solver::{AnyVelocityConstraint, VelocityGroundConstraint};
 use crate::dynamics::{IntegrationParameters, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
@@ -39,6 +39,7 @@ pub(crate) struct VelocityGroundConstraintWithManifoldFriction {
     tangent_parts: [VelocityConstraintElementPart; DIM - 1],
     twist_part: VelocityConstraintElementPart,
     twist_weights: [Real; MAX_MANIFOLD_POINTS],
+    impulse_scale: Real,
 }
 
 impl VelocityGroundConstraintWithManifoldFriction {
@@ -56,6 +57,9 @@ impl VelocityGroundConstraintWithManifoldFriction {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
     ) {
+        let (erp, cfm, impulse_scale) =
+            SpringRegularization::default().erp_cfm_impulse_scale(params.dt);
+
         let inv_dt = params.inv_dt();
         let mut rb1 = &bodies[manifold.data.body_pair.body1];
         let mut rb2 = &bodies[manifold.data.body_pair.body2];
@@ -90,6 +94,7 @@ impl VelocityGroundConstraintWithManifoldFriction {
                 tangent_parts: [VelocityConstraintElementPart::zero(); DIM - 1],
                 twist_part: VelocityConstraintElementPart::zero(),
                 twist_weights: [0.0; MAX_MANIFOLD_POINTS],
+                impulse_scale,
             };
 
             let mut manifold_center = Point::origin();
@@ -112,7 +117,7 @@ impl VelocityGroundConstraintWithManifoldFriction {
                     .effective_world_inv_inertia_sqrt
                     .transform_vector(dp2.gcross(-force_dir1));
 
-                let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
+                let r = 1.0 / (cfm + rb2.effective_inv_mass + gcross2.gdot(gcross2));
 
                 let mut rhs = (vel1 - vel2).dot(&force_dir1);
 
@@ -120,7 +125,11 @@ impl VelocityGroundConstraintWithManifoldFriction {
                     rhs += manifold_point.restitution * rhs
                 }
 
-                rhs += manifold_point.dist.max(0.0) * inv_dt;
+                if manifold_point.dist < 0.0 {
+                    rhs += manifold_point.dist * erp;
+                } else {
+                    rhs += manifold_point.dist * inv_dt;
+                }
 
                 let impulse = manifold_point.data.impulse * warmstart_coeff;
                 tangent_impulses[0] += manifold_point.data.tangent_impulse[0];
@@ -247,7 +256,7 @@ impl VelocityGroundConstraintWithManifoldFriction {
             let elt = &mut self.normal_parts[i];
             let dimpulse =
                 -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
-            let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
+            let new_impulse = (elt.impulse * self.impulse_scale - elt.r * dimpulse).max(0.0);
             let dlambda = new_impulse - elt.impulse;
             elt.impulse = new_impulse;