feat: add warmstarting to contact constraints resolution

13 files changed, 694 insertions, 309 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 093ebe9..80ca92c 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -4,7 +4,6 @@ use std::num::NonZeroUsize;
 // TODO: enabling the block solver in 3d introduces a lot of jitters in
 //       the 3D domino demo. So for now we dont enable it in 3D.
 pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2");
-pub(crate) static DISABLE_FRICTION_LIMIT_REAPPLY: bool = false;
 
 /// Parameters for a time-step of the physics engine.
 #[derive(Copy, Clone, Debug)]
@@ -26,12 +25,12 @@ pub struct IntegrationParameters {
 
     /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
     /// will be compensated for during the velocity solve.
-    /// (default `0.8`).
+    /// (default `0.1`).
     pub erp: 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`).
+    /// (default `20.0`).
     pub damping_ratio: Real,
 
     /// 0-1: multiplier for how much of the joint violation
@@ -40,14 +39,21 @@ pub struct IntegrationParameters {
     pub joint_erp: Real,
 
     /// The fraction of critical damping applied to the joint for constraints regularization.
-    /// (default `0.25`).
+    /// (default `1.0`).
     pub joint_damping_ratio: Real,
 
-    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
+    /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the
+    /// initial solution (instead of 0) at the next simulation step.
+    ///
+    /// This should generally be set to 1. Can be set to 0 if using a large [`Self::erp`] value.
+    /// (default `1.0`).
+    pub warmstart_coefficient: Real,
+
+    /// Amount of penetration the engine won’t attempt to correct (default: `0.001m`).
     pub allowed_linear_error: Real,
     /// Maximum amount of penetration the solver will attempt to resolve in one timestep.
     pub max_penetration_correction: Real,
-    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
+    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`).
     pub prediction_distance: Real,
     /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
     pub num_solver_iterations: NonZeroUsize,
@@ -55,8 +61,8 @@ pub struct IntegrationParameters {
     pub num_additional_friction_iterations: usize,
     /// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`).
     pub num_internal_pgs_iterations: usize,
-    /// The maximum number of stabilization iterations run at each solver iterations (default: `10`).
-    pub max_internal_stabilization_iterations: usize,
+    /// The number of stabilization iterations run at each solver iterations (default: `2`).
+    pub num_internal_stabilization_iterations: usize,
     /// Minimum number of dynamic bodies in each active island (default: `128`).
     pub min_island_size: usize,
     /// Maximum number of substeps performed by the  solver (default: `1`).
@@ -64,51 +70,6 @@ pub struct IntegrationParameters {
 }
 
 impl IntegrationParameters {
-    /// Configures the integration parameters to match the old PGS solver
-    /// from Rapier version <= 0.17.
-    ///
-    /// This solver was slightly faster than the new one but resulted
-    /// in less stable joints and worse convergence rates.
-    ///
-    /// This should only be used for comparison purpose or if you are
-    /// experiencing problems with the new solver.
-    ///
-    /// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will
-    ///       still create solver iterations based on the new "small-steps" PGS solver.
-    /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
-    ///       [`Self::joint_damping_ratio`] to their former default values.
-    pub fn switch_to_standard_pgs_solver(&mut self) {
-        self.num_internal_pgs_iterations *= self.num_solver_iterations.get();
-        self.num_solver_iterations = NonZeroUsize::new(1).unwrap();
-        self.erp = 0.8;
-        self.damping_ratio = 0.25;
-        self.joint_erp = 1.0;
-        self.joint_damping_ratio = 1.0;
-    }
-
-    /// Configures the integration parameters to match the new "small-steps" PGS solver
-    /// from Rapier version >= 0.18.
-    ///
-    /// The "small-steps" PGS solver is the default one given by [`Self::default()`] so
-    /// calling this function is generally not needed unless
-    /// [`Self::switch_to_standard_pgs_solver()`] was called.
-    ///
-    /// This solver results in more stable joints and significantly better convergence
-    /// rates but is slightly slower in its default settings.
-    ///
-    /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
-    ///       [`Self::joint_damping_ratio`] to their default values.
-    pub fn switch_to_small_steps_pgs_solver(&mut self) {
-        self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap();
-        self.num_internal_pgs_iterations = 1;
-
-        let default = Self::default();
-        self.erp = default.erp;
-        self.damping_ratio = default.damping_ratio;
-        self.joint_erp = default.joint_erp;
-        self.joint_damping_ratio = default.joint_damping_ratio;
-    }
-
     /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
     ///
     /// This is zero if `self.dt` is zero.
@@ -161,7 +122,7 @@ impl IntegrationParameters {
         // 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:
+        // NOTE: This simplifies to cfm = cfm_coeff / 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);
 
@@ -180,13 +141,14 @@ impl IntegrationParameters {
         // 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.
+        // in the constraint solver.
         1.0 / (1.0 + cfm_coeff)
     }
 
     /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization
     pub fn joint_cfm_coeff(&self) -> Real {
         // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
+        // The logic is similar to `Self::cfm_factor`.
         let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
         inv_erp_minus_one * inv_erp_minus_one
             / ((1.0 + inv_erp_minus_one)
@@ -194,23 +156,23 @@ impl IntegrationParameters {
                 * self.joint_damping_ratio
                 * self.joint_damping_ratio)
     }
-}
 
-impl Default for IntegrationParameters {
-    fn default() -> Self {
+    /// Initialize the simulation paramaters with settings matching the TGS-soft solver
+    /// with warmstarting.
+    ///
+    /// This is the default configuration, equivalent to [`IntegrationParameters::default()`].
+    pub fn tgs_soft() -> Self {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            erp: 0.8,
-            damping_ratio: 1.0,
+            erp: 0.1,
+            damping_ratio: 20.0,
             joint_erp: 1.0,
             joint_damping_ratio: 1.0,
-            allowed_linear_error: 0.001,
-            max_penetration_correction: Real::MAX,
-            prediction_distance: 0.002,
+            warmstart_coefficient: 1.0,
             num_internal_pgs_iterations: 1,
-            max_internal_stabilization_iterations: 10,
-            num_additional_friction_iterations: 4,
+            num_internal_stabilization_iterations: 2,
+            num_additional_friction_iterations: 0,
             num_solver_iterations: NonZeroUsize::new(4).unwrap(),
             // TODO: what is the optimal value for min_island_size?
             // It should not be too big so that we don't end up with
@@ -218,7 +180,45 @@ impl Default for IntegrationParameters {
             // However we don't want it to be too small and end up with
             // tons of islands, reducing SIMD parallelism opportunities.
             min_island_size: 128,
+            allowed_linear_error: 0.001,
+            max_penetration_correction: Real::MAX,
+            prediction_distance: 0.002,
             max_ccd_substeps: 1,
         }
     }
+
+    /// Initialize the simulation paramaters with settings matching the TGS-soft solver
+    /// **without** warmstarting.
+    ///
+    /// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless
+    /// warmstarting proves to be undesirable for your use-case.
+    pub fn tgs_soft_without_warmstart() -> Self {
+        Self {
+            erp: 0.8,
+            damping_ratio: 1.0,
+            warmstart_coefficient: 0.0,
+            num_additional_friction_iterations: 4,
+            ..Self::tgs_soft()
+        }
+    }
+
+    /// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17.
+    ///
+    /// This exists mainly for testing and comparison purpose.
+    pub fn pgs_legacy() -> Self {
+        Self {
+            erp: 0.8,
+            damping_ratio: 0.25,
+            warmstart_coefficient: 0.0,
+            num_additional_friction_iterations: 4,
+            num_solver_iterations: NonZeroUsize::new(1).unwrap(),
+            ..Self::tgs_soft()
+        }
+    }
+}
+
+impl Default for IntegrationParameters {
+    fn default() -> Self {
+        Self::tgs_soft()
+    }
 }
diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
index 4d88949..6809c37 100644
--- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
+++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs
@@ -442,6 +442,17 @@ impl ContactConstraintsSet {
         assert_eq!(curr_start, total_num_constraints);
     }
 
+    pub fn warmstart(
+        &mut self,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let (jac, constraints) = self.iter_constraints_mut();
+        for mut c in constraints {
+            c.warmstart(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
     pub fn solve_restitution(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
index d99ad60..301ce71 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
@@ -259,6 +259,24 @@ impl GenericOneBodyConstraint {
         }
     }
 
+    pub fn warmstart(
+        &mut self,
+        jacobians: &DVector<Real>,
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let solver_vel2 = self.inner.solver_vel2;
+
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        OneBodyConstraintElement::generic_warmstart_group(
+            elements,
+            jacobians,
+            self.ndofs2,
+            self.j_id,
+            solver_vel2,
+            generic_solver_vels,
+        );
+    }
+
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,
diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
index 1003a9a..4928f36 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
@@ -8,6 +8,40 @@ use na::SimdPartialOrd;
 
 impl OneBodyConstraintTangentPart<Real> {
     #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id2: usize,
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        #[cfg(feature = "dim2")]
+        {
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+                self.impulse[0],
+                &jacobians.rows(j_id2 + ndofs2, ndofs2),
+                1.0,
+            );
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let j_step = ndofs2 * 2;
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+                self.impulse[0],
+                &jacobians.rows(j_id2 + ndofs2, ndofs2),
+                1.0,
+            );
+            solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+                self.impulse[1],
+                &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
+                1.0,
+            );
+        }
+    }
+
+    #[inline]
     pub fn generic_solve(
         &mut self,
         j_id2: usize,
@@ -72,6 +106,22 @@ impl OneBodyConstraintTangentPart<Real> {
 
 impl OneBodyConstraintNormalPart<Real> {
     #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id2: usize,
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
+            self.impulse,
+            &jacobians.rows(j_id2 + ndofs2, ndofs2),
+            1.0,
+        );
+    }
+
+    #[inline]
     pub fn generic_solve(
         &mut self,
         cfm_factor: Real,
@@ -100,6 +150,42 @@ impl OneBodyConstraintNormalPart<Real> {
 
 impl OneBodyConstraintElement<Real> {
     #[inline]
+    pub fn generic_warmstart_group(
+        elements: &mut [Self],
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        // Jacobian index of the first constraint.
+        j_id: usize,
+        solver_vel2: usize,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_step = ndofs2 * 2 * DIM;
+
+        // Solve penetration.
+        let mut nrm_j_id = j_id;
+
+        for element in elements.iter_mut() {
+            element.normal_part.generic_warmstart(
+                nrm_j_id,
+                jacobians,
+                ndofs2,
+                solver_vel2,
+                solver_vels,
+            );
+            nrm_j_id += j_step;
+        }
+
+        // Solve friction.
+        let mut tng_j_id = j_id + ndofs2 * 2;
+
+        for element in elements.iter_mut() {
+            let part = &mut element.tangent_part;
+            part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels);
+            tng_j_id += j_step;
+        }
+    }
+
+    #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,
         elements: &mut [Self],
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
index 4fa8694..be839f0 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
@@ -202,7 +202,7 @@ impl GenericTwoBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse_accumulator: na::zero(),
-                        impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
                         r,
                         r_mat_elts: [0.0; 2],
                     };
@@ -210,7 +210,8 @@ impl GenericTwoBodyConstraintBuilder {
 
                 // Tangent parts.
                 {
-                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
+                    constraint.inner.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -374,6 +375,50 @@ impl GenericTwoBodyConstraint {
         }
     }
 
+    pub fn warmstart(
+        &mut self,
+        jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel1)
+        };
+
+        let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel2)
+        };
+
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        TwoBodyConstraintElement::generic_warmstart_group(
+            elements,
+            jacobians,
+            &self.inner.dir1,
+            #[cfg(feature = "dim3")]
+            &self.inner.tangent1,
+            &self.inner.im1,
+            &self.inner.im2,
+            self.ndofs1,
+            self.ndofs2,
+            self.j_id,
+            &mut solver_vel1,
+            &mut solver_vel2,
+            generic_solver_vels,
+        );
+
+        if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
+            solver_vels[self.inner.solver_vel1] = solver_vel1;
+        }
+
+        if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
+            solver_vels[self.inner.solver_vel2] = solver_vel2;
+        }
+    }
+
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
index 40740a0..389b603 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs
@@ -89,6 +89,95 @@ impl GenericRhs {
 
 impl TwoBodyConstraintTangentPart<Real> {
     #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        tangents1: [&Vector<Real>; DIM - 1],
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        ndofs1: usize,
+        ndofs2: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+        #[cfg(feature = "dim3")]
+        let j_step = j_step(ndofs1, ndofs2);
+
+        #[cfg(feature = "dim2")]
+        {
+            solver_vel1.apply_impulse(
+                j_id1,
+                ndofs1,
+                self.impulse[0],
+                jacobians,
+                tangents1[0],
+                &self.gcross1[0],
+                solver_vels,
+                im1,
+            );
+            solver_vel2.apply_impulse(
+                j_id2,
+                ndofs2,
+                self.impulse[0],
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                solver_vels,
+                im2,
+            );
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            solver_vel1.apply_impulse(
+                j_id1,
+                ndofs1,
+                self.impulse[0],
+                jacobians,
+                tangents1[0],
+                &self.gcross1[0],
+                solver_vels,
+                im1,
+            );
+            solver_vel1.apply_impulse(
+                j_id1 + j_step,
+                ndofs1,
+                self.impulse[1],
+                jacobians,
+                tangents1[1],
+                &self.gcross1[1],
+                solver_vels,
+                im1,
+            );
+
+            solver_vel2.apply_impulse(
+                j_id2,
+                ndofs2,
+                self.impulse[0],
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                solver_vels,
+                im2,
+            );
+            solver_vel2.apply_impulse(
+                j_id2 + j_step,
+                ndofs2,
+                self.impulse[1],
+                jacobians,
+                &-tangents1[1],
+                &self.gcross2[1],
+                solver_vels,
+                im2,
+            );
+        }
+    }
+
+    #[inline]
     pub fn generic_solve(
         &mut self,
         j_id: usize,
@@ -241,6 +330,45 @@ impl TwoBodyConstraintTangentPart<Real> {
 
 impl TwoBodyConstraintNormalPart<Real> {
     #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        ndofs1: usize,
+        ndofs2: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+
+        solver_vel1.apply_impulse(
+            j_id1,
+            ndofs1,
+            self.impulse,
+            jacobians,
+            dir1,
+            &self.gcross1,
+            solver_vels,
+            im1,
+        );
+        solver_vel2.apply_impulse(
+            j_id2,
+            ndofs2,
+            self.impulse,
+            jacobians,
+            &-dir1,
+            &self.gcross2,
+            solver_vels,
+            im2,
+        );
+    }
+
+    #[inline]
     pub fn generic_solve(
         &mut self,
         cfm_factor: Real,
@@ -291,6 +419,74 @@ impl TwoBodyConstraintNormalPart<Real> {
 
 impl TwoBodyConstraintElement<Real> {
     #[inline]
+    pub fn generic_warmstart_group(
+        elements: &mut [Self],
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        // ndofs is 0 for a non-multibody body, or a multibody with zero
+        // degrees of freedom.
+        ndofs1: usize,
+        ndofs2: usize,
+        // Jacobian index of the first constraint.
+        j_id: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_step = j_step(ndofs1, ndofs2) * DIM;
+
+        // Solve penetration.
+        {
+            let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
+
+            for element in elements.iter_mut() {
+                element.normal_part.generic_warmstart(
+                    nrm_j_id,
+                    jacobians,
+                    dir1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
+                );
+                nrm_j_id += j_step;
+            }
+        }
+
+        // Solve friction.
+        {
+            #[cfg(feature = "dim3")]
+            let tangents1 = [tangent1, &dir1.cross(tangent1)];
+            #[cfg(feature = "dim2")]
+            let tangents1 = [&dir1.orthonormal_vector()];
+            let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
+
+            for element in elements.iter_mut() {
+                let part = &mut element.tangent_part;
+                part.generic_warmstart(
+                    tng_j_id,
+                    jacobians,
+                    tangents1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
+                );
+                tng_j_id += j_step;
+            }
+        }
+    }
+
+    #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,
         elements: &mut [Self],
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
index 357f1c4..c31d2aa 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
@@ -151,7 +151,7 @@ impl OneBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
                         impulse_accumulator: na::zero(),
                         r: projected_mass,
                         r_mat_elts: [0.0; 2],
@@ -160,7 +160,8 @@ impl OneBodyConstraintBuilder {
 
                 // Tangent parts.
                 {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
+                    constraint.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let gcross2 = mprops2
@@ -317,13 +318,13 @@ impl OneBodyConstraintBuilder {
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
                 element.normal_part.impulse_accumulator += element.normal_part.impulse;
-                element.normal_part.impulse = na::zero();
+                element.normal_part.impulse *= params.warmstart_coefficient;
             }
 
             // Tangent part.
             {
                 element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse *= params.warmstart_coefficient;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -369,6 +370,21 @@ impl OneBodyConstraint {
         }
     }
 
+    pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+        let mut solver_vel2 = solver_vels[self.solver_vel2];
+
+        OneBodyConstraintElement::warmstart_group(
+            &mut self.elements[..self.num_contacts as usize],
+            &self.dir1,
+            #[cfg(feature = "dim3")]
+            &self.tangent1,
+            &self.im2,
+            &mut solver_vel2,
+        );
+
+        solver_vels[self.solver_vel2] = solver_vel2;
+    }
+
     pub fn solve(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -400,17 +416,11 @@ impl OneBodyConstraint {
         for k in 0..self.num_contacts as usize {
             let contact_id = self.manifold_contact_id[k];
             let active_contact = &mut manifold.points[contact_id as usize];
-            active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
 
-            #[cfg(feature = "dim2")]
-            {
-                active_contact.data.tangent_impulse =
-                    self.elements[k].tangent_part.total_impulse()[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
-            }
+            active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
+            active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
+            active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
+            active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
         }
     }
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
index e5ae140..7ec8c5d 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
@@ -1,9 +1,7 @@
-use crate::dynamics::integration_parameters::{
-    BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY,
-};
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
 use crate::dynamics::solver::SolverVel;
-use crate::math::{AngVector, Vector, DIM};
+use crate::math::{AngVector, TangentImpulse, Vector, DIM};
 use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
 use na::Vector2;
 
@@ -12,14 +10,8 @@ pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
     pub rhs_wo_bias: [N; DIM - 1],
-    #[cfg(feature = "dim2")]
-    pub impulse: na::Vector1<N>,
-    #[cfg(feature = "dim3")]
-    pub impulse: na::Vector2<N>,
-    #[cfg(feature = "dim2")]
-    pub impulse_accumulator: na::Vector1<N>,
-    #[cfg(feature = "dim3")]
-    pub impulse_accumulator: na::Vector2<N>,
+    pub impulse: TangentImpulse<N>,
+    pub impulse_accumulator: TangentImpulse<N>,
     #[cfg(feature = "dim2")]
     pub r: [N; 1],
     #[cfg(feature = "dim3")]
@@ -43,57 +35,29 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
 
     /// Total impulse applied across all the solver substeps.
     #[inline]
-    #[cfg(feature = "dim2")]
-    pub fn total_impulse(&self) -> na::Vector1<N> {
+    pub fn total_impulse(&self) -> TangentImpulse<N> {
         self.impulse_accumulator + self.impulse
     }
 
-    /// Total impulse applied across all the solver substeps.
     #[inline]
-    #[cfg(feature = "dim3")]
-    pub fn total_impulse(&self) -> na::Vector2<N> {
-        self.impulse_accumulator + self.impulse
-    }
-
-    #[inline]
-    pub fn apply_limit(
+    pub fn warmstart(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
         im2: &Vector<N>,
-        limit: N,
         solver_vel2: &mut SolverVel<N>,
-    ) where
-        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
-    {
-        if DISABLE_FRICTION_LIMIT_REAPPLY {
-            return;
-        }
-
+    ) {
         #[cfg(feature = "dim2")]
         {
-            let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
-            let dlam