feat: add exact mlcp solver for pais of 2 constraints

11 files changed, 460 insertions, 22 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 13b3fde..2788349 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -1,6 +1,9 @@
 use crate::math::Real;
 use std::num::NonZeroUsize;
 
+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)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -50,6 +53,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,
     /// 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`).
@@ -194,7 +199,7 @@ impl Default for IntegrationParameters {
         Self {
             dt: 1.0 / 60.0,
             min_ccd_dt: 1.0 / 60.0 / 100.0,
-            erp: 0.6,
+            erp: 0.8,
             damping_ratio: 1.0,
             joint_erp: 1.0,
             joint_damping_ratio: 1.0,
@@ -202,6 +207,7 @@ impl Default for IntegrationParameters {
             max_penetration_correction: Real::MAX,
             prediction_distance: 0.002,
             num_internal_pgs_iterations: 1,
+            max_internal_stabilization_iterations: 10,
             num_additional_friction_iterations: 4,
             num_solver_iterations: NonZeroUsize::new(4).unwrap(),
             // TODO: what is the optimal value for min_island_size?
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 cfdff55..d99ad60 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
@@ -155,6 +155,7 @@ impl GenericOneBodyConstraintBuilder {
                         impulse: na::zero(),
                         impulse_accumulator: na::zero(),
                         r,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
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 f0a7d64..4fa8694 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
@@ -204,6 +204,7 @@ impl GenericTwoBodyConstraintBuilder {
                         impulse_accumulator: na::zero(),
                         impulse: na::zero(),
                         r,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
index c9b35f3..357f1c4 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
@@ -3,8 +3,10 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
+use na::Matrix2;
 use parry::math::Isometry;
 
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::solver::SolverVel;
 use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
@@ -126,14 +128,15 @@ impl OneBodyConstraintBuilder {
                 // Normal part.
                 let normal_rhs_wo_bias;
                 {
-                    let gcross2 = mprops2
+                    let mut gcross2 = mprops2
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let projected_mass = utils::inv(
-                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
-                            + gcross2.gdot(gcross2),
-                    );
+                    let projected_lin_mass =
+                        force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1));
+                    let projected_ang_mass = gcross2.gdot(gcross2);
+
+                    let projected_mass = utils::inv(projected_lin_mass + projected_ang_mass);
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
 
@@ -151,6 +154,7 @@ impl OneBodyConstraintBuilder {
                         impulse: na::zero(),
                         impulse_accumulator: na::zero(),
                         r: projected_mass,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
@@ -205,6 +209,44 @@ impl OneBodyConstraintBuilder {
                     builder.infos[k] = infos;
                 }
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..manifold_points.len() / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let mut r_mat = Matrix2::zeros();
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+                    r_mat.m12 = force_dir1
+                        .dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m21 = r_mat.m12;
+                    r_mat.m11 = utils::inv(r0);
+                    r_mat.m22 = utils::inv(r1);
+
+                    if let Some(inv) = r_mat.try_inverse() {
+                        constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
+                        constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
+                    } else {
+                        // If inversion failed, the contacts are redundant.
+                        // Ignore the one with the smallest depth (it is too late to
+                        // have the constraint removed from the constraint set, so just
+                        // set the mass (r) matrix elements to 0.
+                        constraint.elements[k0].normal_part.r_mat_elts =
+                            if manifold_points[k0].dist <= manifold_points[k1].dist {
+                                [r0, 0.0]
+                            } else {
+                                [0.0, r1]
+                            };
+                        constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
+                    }
+                }
+            }
         }
     }
 
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 79b207b..e5ae140 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
@@ -1,6 +1,11 @@
+use crate::dynamics::integration_parameters::{
+    BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY,
+};
+use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
 use crate::dynamics::solver::SolverVel;
 use crate::math::{AngVector, Vector, DIM};
 use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
+use na::Vector2;
 
 #[derive(Copy, Clone, Debug)]
 pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
@@ -60,6 +65,10 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<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);
@@ -153,6 +162,7 @@ pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> {
     pub impulse: N,
     pub impulse_accumulator: N,
     pub r: N,
+    pub r_mat_elts: [N; 2],
 }
 
 impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
@@ -164,6 +174,7 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
             impulse: na::zero(),
             impulse_accumulator: na::zero(),
             r: na::zero(),
+            r_mat_elts: [N::zero(); 2],
         }
     }
 
@@ -192,6 +203,44 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
         solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
         solver_vel2.angular += self.gcross2 * dlambda;
     }
+
+    #[inline]
+    pub fn solve_pair(
+        constraint_a: &mut Self,
+        constraint_b: &mut Self,
+        cfm_factor: N,
+        dir1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        let dvel_a = -dir1.dot(&solver_vel2.linear)
+            + constraint_a.gcross2.gdot(solver_vel2.angular)
+            + constraint_a.rhs;
+        let dvel_b = -dir1.dot(&solver_vel2.linear)
+            + constraint_b.gcross2.gdot(solver_vel2.angular)
+            + constraint_b.rhs;
+
+        let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
+        let new_impulse = TwoBodyConstraintNormalPart::solve_mlcp_two_constraints(
+            Vector2::new(dvel_a, dvel_b),
+            prev_impulse,
+            constraint_a.r,
+            constraint_b.r,
+            constraint_a.r_mat_elts,
+            constraint_b.r_mat_elts,
+            cfm_factor,
+        );
+
+        let dlambda = new_impulse - prev_impulse;
+
+        constraint_a.impulse = new_impulse.x;
+        constraint_b.impulse = new_impulse.y;
+
+        solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
+        solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
+    }
 }
 
 #[derive(Copy, Clone, Debug)]
@@ -230,13 +279,47 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
 
         // Solve penetration.
         if solve_normal {
-            for element in elements.iter_mut() {
-                element
-                    .normal_part
-                    .solve(cfm_factor, dir1, im2, solver_vel2);
-                let limit = limit * element.normal_part.impulse;
-                let part = &mut element.tangent_part;
-                part.apply_limit(tangents1, im2, limit, solver_vel2);
+            if BLOCK_SOLVER_ENABLED {
+                for elements in elements.chunks_exact_mut(2) {
+                    let [element_a, element_b] = elements else {
+                        unreachable!()
+                    };
+
+                    OneBodyConstraintNormalPart::solve_pair(
+                        &mut element_a.normal_part,
+                        &mut element_b.normal_part,
+                        cfm_factor,
+                        dir1,
+                        im2,
+                        solver_vel2,
+                    );
+
+                    // There is one constraint left to solve if there isn’t an even number.
+                    for i in 0..2 {
+                        let limit = limit * elements[i].normal_part.impulse;
+                        let part = &mut elements[i].tangent_part;
+                        part.apply_limit(tangents1, im2, limit, solver_vel2);
+                    }
+                }
+
+                if elements.len() % 2 == 1 {
+                    let element = elements.last_mut().unwrap();
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im2, solver_vel2);
+                    let limit = limit * element.normal_part.impulse;
+                    let part = &mut element.tangent_part;
+                    part.apply_limit(tangents1, im2, limit, solver_vel2);
+                }
+            } else {
+                for element in elements.iter_mut() {
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im2, solver_vel2);
+                    let limit = limit * element.normal_part.impulse;
+                    let part = &mut element.tangent_part;
+                    part.apply_limit(tangents1, im2, limit, solver_vel2);
+                }
             }
         }
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
index 03c1abe..8819cea 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
@@ -1,4 +1,5 @@
 use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::solver::{ContactPointInfos, SolverVel};
 use crate::dynamics::{
@@ -15,6 +16,7 @@ use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
 use parry::math::SimdBool;
+use parry::utils::SdpMatrix2;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
@@ -156,6 +158,7 @@ impl SimdOneBodyConstraintBuilder {
                         impulse: na::zero(),
                         impulse_accumulator: na::zero(),
                         r: projected_mass,
+                        r_mat_elts: [SimdReal::zero(); 2],
                     };
                 }
 
@@ -200,6 +203,47 @@ impl SimdOneBodyConstraintBuilder {
                     builder.infos[k] = infos;
                 }
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..num_points / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+
+                    let mut r_mat = SdpMatrix2::zero();
+                    r_mat.m12 = force_dir1.dot(&im2.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m11 = utils::simd_inv(r0);
+                    r_mat.m22 = utils::simd_inv(r1);
+
+                    let (inv, det) = {
+                        let _disable_fe_except =
+                            crate::utils::DisableFloatingPointExceptionsFlags::
+                            disable_floating_point_exceptions();
+                        r_mat.inverse_and_get_determinant_unchecked()
+                    };
+                    let is_invertible = det.simd_gt(SimdReal::zero());
+
+                    // If inversion failed, the contacts are redundant.
+                    // Ignore the one with the smallest depth (it is too late to
+                    // have the constraint removed from the constraint set, so just
+                    // set the mass (r) matrix elements to 0.
+                    constraint.elements[k0].normal_part.r_mat_elts = [
+                        inv.m11.select(is_invertible, r0),
+                        inv.m22.select(is_invertible, SimdReal::zero()),
+                    ];
+                    constraint.elements[k1].normal_part.r_mat_elts = [
+                        inv.m12.select(is_invertible, SimdReal::zero()),
+                        r_mat.m12.select(is_invertible, SimdReal::zero()),
+                    ];
+                }
+            }
         }
     }
 
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
index 8abf5c5..24a5730 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
@@ -2,11 +2,13 @@ use super::{ContactConstraintTypes, ContactPointInfos};
 use crate::dynamics::solver::SolverVel;
 use crate::dynamics::solver::{AnyConstraintMut, SolverBody};
 
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Isometry, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot};
-use na::DVector;
+use na::{DVector, Matrix2};
+use num::Pow;
 
 use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
 
@@ -225,6 +227,7 @@ impl TwoBodyConstraintBuilder {
                         impulse: na::zero(),
                         impulse_accumulator: na::zero(),
                         r: projected_mass,
+                        r_mat_elts: [0.0; 2],
                     };
                 }
 
@@ -284,6 +287,48 @@ impl TwoBodyConstraintBuilder {
                 builder.infos[k] = infos;
                 constraint.manifold_contact_id[k] = manifold_point.contact_id;
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..manifold_points.len() / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let mut r_mat = Matrix2::zeros();
+                    let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+                    r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross1
+                            .gdot(constraint.elements[k1].normal_part.gcross1)
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m21 = r_mat.m12;
+                    r_mat.m11 = utils::inv(r0);
+                    r_mat.m22 = utils::inv(r1);
+
+                    if let Some(inv) = r_mat.try_inverse() {
+                        constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
+                        constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
+                    } else {
+                        // If inversion failed, the contacts are redundant.
+                        // Ignore the one with the smallest depth (it is too late to
+                        // have the constraint removed from the constraint set, so just
+                        // set the mass (r) matrix elements to 0.
+                        constraint.elements[k0].normal_part.r_mat_elts =
+                            if manifold_points[k0].dist <= manifold_points[k1].dist {
+                                [r0, 0.0]
+                            } else {
+                                [0.0, r1]
+                            };
+                        constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
+                    }
+                }
+            }
         }
     }
 
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
index 75fc739..e5cd6d5 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs
@@ -1,6 +1,13 @@
+use crate::dynamics::integration_parameters::{
+    BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY,
+};
+use crate::dynamics::solver::contact_constraint::OneBodyConstraintNormalPart;
 use crate::dynamics::solver::SolverVel;
 use crate::math::{AngVector, Vector, DIM};
 use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
+use na::{Matrix2, Vector2};
+use num::Zero;
+use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
 pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
@@ -64,6 +71,10 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<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);
@@ -182,6 +193,11 @@ pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
     pub impulse: N,
     pub impulse_accumulator: N,
     pub r: N,
+    // For coupled constraint pairs, even constraints store the
+    // diagonal of the projected mass matrix. Odd constraints
+    // store the off-diagonal element of the projected mass matrix,
+    // as well as the off-diagonal element of the inverse projected mass matrix.
+    pub r_mat_elts: [N; 2],
 }
 
 impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
@@ -194,6 +210,7 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
             impulse: na::zero(),
             impulse_accumulator: na::zero(),
             r: na::zero(),
+            r_mat_elts: [N::zero(); 2],
         }
     }
 
@@ -229,6 +246,83 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
         solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
         solver_vel2.angular += self.gcross2 * dlambda;
     }
+
+    #[inline(always)]
+    pub(crate) fn solve_mlcp_two_constraints(
+        dvel: Vector2<N>,
+        prev_impulse: Vector2<N>,
+        r_a: N,
+        r_b: N,
+        [r_mat11, r_mat22]: [N; 2],
+        [r_mat12, r_mat_inv12]: [N; 2],
+        cfm_factor: N,
+    ) -> Vector2<N> {
+        let r_dvel = Vector2::new(
+            r_mat11 * dvel.x + r_mat12 * dvel.y,
+            r_mat12 * dvel.x + r_mat22 * dvel.y,
+        );
+        let new_impulse0 = prev_impulse - r_dvel;
+        let new_impulse1 = Vector2::new(prev_impulse.x - r_a * dvel.x, N::zero());
+        let new_impulse2 = Vector2::new(N::zero(), prev_impulse.y - r_b * dvel.y);
+        let new_impulse3 = Vector2::new(N::zero(), N::zero());
+
+        let keep0 = new_impulse0.x.simd_ge(N::zero()) & new_impulse0.y.simd_ge(N::zero());
+        let keep1 = new_impulse1.x.simd_ge(N::zero())
+            & (dvel.y + r_mat_inv12 * new_impulse1.x).simd_ge(N::zero());
+        let keep2 = new_impulse2.y.simd_ge(N::zero())
+            & (dvel.x + r_mat_inv12 * new_impulse2.y).simd_ge(N::zero());
+        let keep3 = dvel.x.simd_ge(N::zero()) & dvel.y.simd_ge(N::zero());
+
+        let selected3 = (new_impulse3 * cfm_factor).select(keep3, prev_impulse);
+        let selected2 = (new_impulse2 * cfm_factor).select(keep2, selected3);
+        let selected1 = (new_impulse1 * cfm_factor).select(keep1, selected2);
+        (new_impulse0 * cfm_factor).select(keep0, selected1)
+    }
+
+    #[inline]
+    pub fn solve_pair(
+        constraint_a: &mut Self,
+        constraint_b: &mut Self,
+        cfm_factor: N,
+        dir1: &Vector<N>,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, Result = N>,
+    {
+        let dvel_lin = dir1.dot(&solver_vel1.linear) - dir1.dot(&solver_vel2.linear);
+        let dvel_a = dvel_lin
+            + constraint_a.gcross1.gdot(solver_vel1.angular)
+            + constraint_a.gcross2.gdot(solver_vel2.angular)
+            + constraint_a.rhs;
+        let dvel_b = dvel_lin
+            + constraint_b.gcross1.gdot(solver_vel1.angular)
+            + constraint_b.gcross2.gdot(solver_vel2.angular)
+            + constraint_b.rhs;
+
+        let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
+        let new_impulse = Self::solve_mlcp_two_constraints(
+            Vector2::new(dvel_a, dvel_b),
+            prev_impulse,
+            constraint_a.r,
+            constraint_b.r,
+            constraint_a.r_mat_elts,
+            constraint_b.r_mat_elts,
+            cfm_factor,
+        );
+
+        let dlambda = new_impulse - prev_impulse;
+
+        constraint_a.impulse = new_impulse.x;
+        constraint_b.impulse = new_impulse.y;
+
+        solver_vel1.linear += dir1.component_mul(im1) * (dlambda.x + dlambda.y);
+        solver_vel1.angular += constraint_a.gcross1 * dlambda.x + constraint_b.gcross1 * dlambda.y;
+        solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
+        solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
+    }
 }
 
 #[derive(Copy, Clone, Debug)]
@@ -269,13 +363,49 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
 
         // Solve penetration.
         if solve_normal {
-            for element in elements.iter_mut() {
-                element
-                    .normal_part
-                    .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
-                let limit = limit * element.normal_part.impulse;
-                let part = &mut element.tangent_part;
-                part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
+            if BLOCK_SOLVER_ENABLED {
+                for elements in elements.chunks_exact_mut(2) {
+                    let [element_a, element_b] = elements else {
+                        unreachable!()
+                    };
+
+                    TwoBodyConstraintNormalPart::solve_pair(
+                        &mut element_a.normal_part,
+                        &mut element_b.normal_part,
+                        cfm_factor,
+                        dir1,
+                        im1,
+                        im2,
+                        solver_vel1,
+                        solver_vel2,
+                    );
+
+                    for i in 0..2 {
+                        let limit = limit * elements[i].normal_part.impulse;
+                        let part = &mut elements[i].tangent_part;
+                        part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
+                    }
+                }
+
+                // There is one constraint left to solve if there isn’t an even number.
+                if elements.len() % 2 == 1 {
+                    let element = elements.last_mut().unwrap();
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
+                    let limit = limit * element.normal_part.impulse;
+                    let part = &mut element.tangent_part;
+                    part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
+                }
+            } else {
+                for element in elements.iter_mut() {
+                    element
+                        .normal_part
+                        .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
+                    let limit = limit * element.normal_part.impulse;
+                    let part = &mut element.tangent_part;
+                    part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
+                }
             }
         }
 
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
index 2de6ee2..9b05fd3 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
@@ -1,4 +1,5 @@
 use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::solver::{ContactPointInfos, SolverVel};
 use crate::dynamics::{
@@ -13,8 +14,10 @@ use crate::math::{
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
+use na::Matrix2;
 use num::Zero;
 use parry::math::SimdBool;
+use parry::utils::SdpMatrix2;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
@@ -140,6 +143,7 @@ impl TwoBodyConstraintBuilderSimd {
                         impulse: SimdReal::splat(0.0),
                         impulse_accumulator: SimdReal::splat(0.0),
                         r: projected_mass,
+                        r_mat_elts: [SimdReal::zero(); 2],
                     };
                 }
 
@@ -186,6 +190,52 @@ impl TwoBodyConstraintBuilderSimd {
 
                 builder.infos[k] = infos;
             }
+
+            if BLOCK_SOLVER_ENABLED {
+                // Coupling between consecutive pairs.
+                for k in 0..num_points / 2 {
+                    let k0 = k * 2;
+                    let k1 = k * 2 + 1;
+
+                    let imsum = im1 + im2;
+                    let r0 = constraint.elements[k0].normal_part.r;
+                    let r1 = constraint.elements[k1].normal_part.r;
+
+                    let mut r_mat = SdpMatrix2::zero();
+                    r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross1
+                            .gdot(constraint.elements[k1].normal_part.gcross1)
+                        + constraint.elements[k0]
+                            .normal_part
+                            .gcross2
+                            .gdot(constraint.elements[k1].normal_part.gcross2);
+                    r_mat.m11 = utils::simd_inv(r0);
+                    r_mat.m22 = utils::simd_inv(r1);
+
+                    let (inv, det) = {
+                        let _disable_fe_except =
+                            crate::utils::DisableFloatingPointExceptionsFlags::
+                            disable_floating_point_exceptions();
+                        r_mat.inverse_and_get_determinant_unchecked()
+                    };
+                    let is_invertible = det.simd_gt(SimdReal::zero());
+
+                    // If inversion failed, the contacts are redundant.
+                    // Ignore the one with the smallest depth (it is too late to
+                    // have the constraint removed from the constraint set, so just
+                    // set the mass (r) matrix elements to 0.
+                    constraint.elements[k0].normal_part.r_mat_elts = [
+                        inv.m11.select(is_invertible, r0),
+                        inv.m22.select(is_invertible, SimdReal::zero()),
+                    ];
+                    constraint.elements[k1].normal_part.r_mat_elts = [
+                        inv.m12.select(is_invertible, SimdReal::zero()),
+                        r_mat.m12.select(is_invertible, SimdReal::zero()),
+                    ];
+                }
+            }
         }
     }
 
diff --git a/src/dynamics/solver/solver_vel.rs b/src/dynamics/solver/solver_vel.rs
index 48c76b8..da69fb8 100644
--- a/src/dynamics/solver/solver_vel.rs
+++ b/src/dynamics/solver/solver_vel.rs
@@ -1,7 +1,7 @@
 use crate::math::{AngVector, Vector, SPATIAL_DIM};
 use crate::utils::SimdRealCopy;
 use na::{DVectorView, DVectorViewMut, Scalar};
-use std::ops::{AddAssign, Sub};
+use std::ops::{AddAssign, Sub, SubAssign};
 
 #[derive(Copy, Clone, Debug, Default)]
 #[repr(C)]
@@ -47,6 +47,13 @@ impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
     }
 }
 
+impl<N: SimdRealCopy> SubAssign for SolverVel<N> {
+    fn sub_assign(&mut self, rhs: Self) {
+        self.linear -= rhs.linear;
+        self.angular -= rhs.angular;
+    }
+}
+
 impl<N: SimdRealCopy> Sub for SolverVel<N> {
     type Output = Self;
 
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index bcf31b0..1452864 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -997,6 +997,35 @@ impl NarrowPhase {
                         manifold.data.normal = modifiable_normal;
                         manifold.data.user_data = modifiable_user_data;
                     }
+
+                    /*
+                     * TODO: When using the block solver in 3D, I’d expect this sort to help, but
+                     *       it makes the domino demo worse. Needs more investigation.
+                    fn sort_solver_contacts(mut contacts: &mut [SolverContact]) {
+                        while contacts.len() > 2 {
+                            let first = contacts[0];
+                            let mut furthest_id = 1;
+                            let mut furthest_dist = na::distance(&first.point, &contacts[1].point);
+
+                            for (candidate_id, candidate) in contacts.iter().enumerate().skip(2) {
+                                let candidate_dist = na::distance(&first.point, &candidate.point);
+
+