feat: add exact mlcp solver for pais of 2 constraints

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