Merge pull request #282 from dimforge/critical-damping

Improve the CFM implementation

12 files changed, 162 insertions, 111 deletions

diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index f1ab0ea..c1d4134 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/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,6 +32,7 @@ impl AnyGenericVelocityConstraint {
     ) {
         match self {
             AnyGenericVelocityConstraint::Nongrouped(c) => c.solve(
+                cfm_factor,
                 jacobians,
                 mj_lambdas,
                 generic_mj_lambdas,
@@ -38,6 +40,7 @@ impl AnyGenericVelocityConstraint {
                 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,
diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs
index e75dd01..19fba43 100644
--- a/src/dynamics/solver/generic_velocity_constraint_element.rs
+++ b/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;
             }
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs
index c9b2c3f..9ce824e 100644
--- a/src/dynamics/solver/generic_velocity_ground_constraint.rs
+++ b/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,
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs
index 80c97ab..750811c 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,
@@ -119,9 +121,9 @@ impl VelocityGroundConstraintElement<Real> {
             let mut nrm_j_id = j_id;
 
             for element in elements.iter_mut() {
-                element
-                    .normal_part
-                    .generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas);
+                element.normal_part.generic_solve(
+                    cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas,
+                );
                 nrm_j_id += j_step;
             }
         }
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index 69ceb03..00668b1 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/src/dynamics/solver/parallel_velocity_solver.rs
@@ -54,6 +54,7 @@ impl ParallelVelocitySolver {
             let joint_descs = &joint_constraints.constraint_descs[..];
             let mut target_num_desc = 0;
             let mut shift = 0;
+            let cfm_factor = params.cfm_factor();
 
             for _ in 0..params.max_velocity_iterations {
                 macro_rules! solve {
@@ -116,7 +117,13 @@ impl ParallelVelocitySolver {
                 );
                 shift += joint_descs.len();
                 start_index -= joint_descs.len();
-                solve!(contact_constraints, &mut self.mj_lambdas, true, true);
+                solve!(
+                    contact_constraints,
+                    cfm_factor,
+                    &mut self.mj_lambdas,
+                    true,
+                    true
+                );
                 shift += contact_descs.len();
                 start_index -= contact_descs.len();
             }
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 6a95492..bb00b66 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/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")]
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);
             }
         }
 
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 7fcb7f4..44b91c6 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/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")]
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 76de3f9..cf7d9eb 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -153,7 +153,7 @@ impl VelocityGroundConstraint {
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = params.delassus_inv_factor
+                    let projected_mass = 1.0
                         / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
                             + gcross2.gdot(gcross2));
 
@@ -172,8 +172,8 @@ impl VelocityGroundConstraint {
                         gcross2,
                         rhs: rhs_wo_bias + rhs_bias,
                         rhs_wo_bias,
-                        impulse: 0.0,
-                        r,
+                        impulse: na::zero(),
+                        r: projected_mass,
                     };
                 }
 
@@ -219,6 +219,7 @@ impl VelocityGroundConstraint {
 
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -226,6 +227,7 @@ impl VelocityGroundConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         VelocityGroundConstraintElement::solve_group(
+            cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index a843905..8057030 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -109,12 +109,17 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
     }
 
     #[inline]
-    pub fn solve(&mut self, dir1: &Vector<N>, im2: &Vector<N>, mj_lambda2: &mut DeltaVel<N>)
-    where
+    pub fn solve(
+        &mut self,
+        cfm_factor: N,
+        dir1: &Vector<N>,
+        im2: &Vector<N>,
+        mj_lambda2: &mut DeltaVel<N>,
+    ) where
         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 = (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;
 
@@ -139,6 +144,7 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
 
     #[inline]
     pub fn solve_group(
+        cfm_factor: N,
         elements: &mut [Self],
         dir1: &Vector<N>,
         #[cfg(feature = "dim3")] tangent1: &Vector<N>,
@@ -155,7 +161,9 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
         // Solve penetration.
         if solve_normal {
             for element in elements.iter_mut() {
-                element.normal_part.solve(&dir1, im2, mj_lambda2);
+                element
+                    .normal_part
+                    .solve(cfm_factor, &dir1, im2, mj_lambda2);
             }
         }
 
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 4771469..65ac46e 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -43,9 +43,8 @@ impl WVelocityGroundConstraint {
     {
         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 mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
         let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
@@ -143,8 +142,9 @@ impl WVelocityGroundConstraint {
                 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = delassus_inv_factor
+                    let projected_mass = SimdReal::splat(1.0)
                         / (force_dir1.dot(&im2.component_mul(&force_dir1)) + 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;
@@ -158,7 +158,7 @@ impl WVelocityGroundConstraint {
                         rhs: rhs_wo_bias + rhs_bias,
                         rhs_wo_bias,
                         impulse: na::zero(),
-                        r,
+                        r: projected_mass,
                     };
                 }
 
@@ -199,6 +199,7 @@ impl WVelocityGroundConstraint {
 
     pub fn solve(
         &mut self,
+        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -211,6 +212,7 @@ impl WVelocityGroundConstraint {
         };
 
         VelocityGroundConstraintElement::solve_group(
+            SimdReal::splat(cfm_factor),
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index 1cc43ac..47275ed 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -50,6 +50,7 @@ impl VelocitySolver {
             + ComponentSetMut<RigidBodyActivation>
             + ComponentSet<RigidBodyDamping>,
     {
+        let cfm_factor = params.cfm_factor();
         self.mj_lambdas.clear();
         self.mj_lambdas
             .resize(islands.active_island(island_id).len(), DeltaVel::zero());
@@ -93,18 +94,36 @@ impl VelocitySolver {
             }
 
             for constraint in &mut *contact_constraints {
-                constraint.solve(&mut self.mj_lambdas[..], true, solve_friction);
+                constraint.solve(cfm_factor, &mut self.mj_lambdas[..], true, false);
             }
 
             for constraint in &mut *generic_contact_constraints {
                 constraint.solve(
+                    cfm_factor,
                     generic_contact_jacobians,
                     &mut self.mj_lambdas[..],
                     &mut self.generic_mj_lambdas,
                     true,
-                    solve_friction,
+                    false,
                 );
             }
+
+            if solve_friction {
+                for constraint in &mut *contact_constraints {
+                    constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
+                }
+
+                for constraint in &mut *generic_contact_constraints {
+                    constraint.solve(
+                        cfm_factor,
+                        generic_contact_jacobians,
+                        &mut self.mj_lambdas[..],
+                        &mut self.generic_mj_lambdas,
+                        false,
+                        true,
+                    );
+                }
+            }
         }
 
         let remaining_friction_iterations =
@@ -118,11 +137,12 @@ impl VelocitySolver {
 
         for _ in 0..remaining_friction_iterations {
             for constraint in &mut *contact_constraints {
-                constraint.solve(&mut self.mj_lambdas[..], false, true);
+                constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
             }
 
             for constraint in &mut *generic_contact_constraints {
                 constraint.solve(
+                    cfm_factor,
                     generic_contact_jacobians,
                     &mut self.mj_lambdas[..],
                     &mut self.generic_mj_lambdas,
@@ -147,10 +167,7 @@ impl VelocitySolver {
                     multibody.velocities += mj_lambdas;
                     multibody.integrate(params.dt);
                     multibody.forward_kinematics(bodies, false);
-
-                    if params.max_stabilization_iterations > 0 {
-                        multibody.velocities = prev_vels;
-                    }
+                    multibody.velocities = prev_vels;
                 }
             } else {
                 let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
@@ -177,88 +194,85 @@ impl VelocitySolver {
                 new_poss.next_position =
                     new_vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
                 bodies.set_internal(handle.0, new_poss);
-
-                if params.max_stabilization_iterations == 0 {
-                    bodies.set_internal(handle.0, new_vels);
-                }
             }
         }
 
-        if params.max_stabilization_iterations > 0 {
-            for joint in &mut *joint_constraints {
-                joint.remove_bias_from_rhs();
+        for joint in &mut *joint_constraints {
+            joint.remove_bias_from_rhs();
+        }
+        for constraint in &mut *contact_constraints {
+            constraint.remove_bias_from_rhs();
+        }
+        for constraint in &mut *generic_contact_constraints {
+            constraint.remove_bias_from_rhs();
+        }
+
+        for _ in 0..params.max_stabilization_iterations {
+            for constraint in &mut *joint_constraints {
+                constraint.solve(
+                    generic_joint_jacobians,
+                    &mut self.mj_lambdas[..],
+                    &mut self.generic_mj_lambdas,
+                );
             }
+
             for constraint in &mut *contact_constraints {
-                constraint.remove_bias_from_rhs();
+                constraint.solve(1.0, &mut self.mj_lambdas[..], true, false);
             }
+
             for constraint in &mut *generic_contact_constraints {
-                constraint.remove_bias_from_rhs();
+                constraint.solve(
+                    1.0,
+                    generic_contact_jacobians,
+                    &mut self.mj_lambdas[..],
+                    &mut self.generic_mj_lambdas,
+                    true,
+                    false,
+                );
             }
 
-            for _ in 0..params.max_stabilization_iterations {
-                for constraint in &mut *joint_constraints {
-                    constraint.solve(
-                        generic_joint_jacobians,
-                        &mut self.mj_lambdas[..],
-                        &mut self.generic_mj_lambdas,
-                    );
-                }
-
-                for constraint in &mut *contact_constraints {
-                    constraint.solve(&mut self.mj_lambdas[..], true, true);
-                }
+            for constraint in &mut *contact_constraints {
+                constraint.solve(1.0, &mut self.mj_lambdas[..], false, true);
+            }
 
-                for constraint in &mut *generic_contact_constraints {
-                    constraint.solve(
-                        generic_contact_jacobians,
-                        &mut self.mj_lambdas[..],
-                        &mut self.generic_mj_lambdas,
-                        true,
-                        true,
-                    );
-                }
+            for constraint in &mut *generic_contact_constraints {
+                constraint.solve(
+                    1.0,
+                    generic_contact_jacobians,
+                    &mut self.mj_lambdas[..],
+                    &mut self.generic_mj_lambdas,
+                    false,
+                    true,
+                );
             }
+        }
 
-            // Update velocities.
-            for handle in islands.active_island(island_id) {
-                if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
-                    let multibody = multibodies
-                        .get_multibody_mut_internal(link.multibody)
-                        .unwrap();
-
-                    if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                        let mj_lambdas = self
-                            .generic_mj_lambdas
-                            .rows(multibody.solver_id, multibody.ndofs());
-                        multibody.velocities += mj_lambdas;
-                    }
-                } else {
-                    let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
-                        bodies.index_bundle(handle.0);
-
-                    let dvel = self.mj_lambdas[ids.active_set_offset];
-                    let dangvel = mprops
-                        .effective_world_inv_inertia_sqrt
-                        .transform_vector(dvel.angular);
-
-                    // let mut curr_vel_pseudo_energy = 0.0;
-                    bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
-                        // curr_vel_pseudo_energy = vels.pseudo_kinetic_energy();
-                        vels.linvel += dvel.linear;
-                        vels.angvel += dangvel;
-                    });
-
-                    // let impulse_vel_pseudo_energy = RigidBodyVelocity {
-                    //     linvel: dvel.linear,
-                    //     angvel: dangvel,
-                    // }
-                    // .pseudo_kinetic_energy();
-                    //
-                    // bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
-                    //     activation.energy =
-                    //         impulse_vel_pseudo_energy.max(curr_vel_pseudo_energy / 5.0);
-                    // });
+        // Update velocities.
+        for handle in islands.active_island(island_id) {
+            if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
+                let multibody = multibodies
+                    .get_multibody_mut_internal(link.multibody)
+                    .unwrap();
+
+                if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
+                    let mj_lambdas = self
+                        .generic_mj_lambdas
+                        .rows(multibody.solver_id, multibody.ndofs());
+                    multibody.velocities += mj_lambdas;
                 }
+            } else {
+                let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
+                    bodies.index_bundle(handle.0);
+
+                let dvel = self.mj_lambdas[ids.active_set_offset];
+                let dangvel = mprops
+                    .effective_world_inv_inertia_sqrt
+                    .transform_vector(dvel.angular);
+
+                bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
+                    vels.linvel += dvel.linear;
+                    vels.angvel += dangvel;
+                });
             }
         }