From 0703e5527fd95d86bb6621e61dbcb1a6e7f9329a Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Sun, 16 Jan 2022 16:40:59 +0100
Subject: Fix some solver issues

- Fix the wrong codepath taken  by the solver for contacts involving a collider without parent.
- Properly adress the non-linear treatment of the friction direction
- Simplify the sleeping strategy
- Add an impulse resolution multiplier
---
 .../solver/velocity_ground_constraint_element.rs   | 40 +++++++++++++---------
 1 file changed, 23 insertions(+), 17 deletions(-)

(limited to 'src/dynamics/solver/velocity_ground_constraint_element.rs')

diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 4d4d3c3..a843905 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -11,17 +11,22 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
     pub impulse: na::Vector1<N>,
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
-    pub r: [N; DIM - 1],
+    #[cfg(feature = "dim2")]
+    pub r: [N; 1],
+    #[cfg(feature = "dim3")]
+    pub r: [N; DIM],
 }
 
 impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     fn zero() -> Self {
         Self {
             gcross2: [na::zero(); DIM - 1],
             rhs: [na::zero(); DIM - 1],
             impulse: na::zero(),
-            r: [na::zero(); DIM - 1],
+            #[cfg(feature = "dim2")]
+            r: [na::zero(); 1],
+            #[cfg(feature = "dim3")]
+            r: [na::zero(); DIM],
         }
     }
 
@@ -38,10 +43,10 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
     {
         #[cfg(feature = "dim2")]
         {
-            let dimpulse = -tangents1[0].dot(&mj_lambda2.linear)
+            let dvel = -tangents1[0].dot(&mj_lambda2.linear)
                 + self.gcross2[0].gdot(mj_lambda2.angular)
                 + self.rhs[0];
-            let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
             let dlambda = new_impulse - self.impulse[0];
             self.impulse[0] = new_impulse;
 
@@ -51,17 +56,22 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
 
         #[cfg(feature = "dim3")]
         {
-            let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear)
+            let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear)
                 + self.gcross2[0].gdot(mj_lambda2.angular)
                 + self.rhs[0];
-            let dimpulse_1 = -tangents1[1].dot(&mj_lambda2.linear)
+            let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear)
                 + self.gcross2[1].gdot(mj_lambda2.angular)
                 + self.rhs[1];
 
-            let new_impulse = na::Vector2::new(
-                self.impulse[0] - self.r[0] * dimpulse_0,
-                self.impulse[1] - self.r[1] * dimpulse_1,
-            );
+            let dvel_00 = dvel_0 * dvel_0;
+            let dvel_11 = dvel_1 * dvel_1;
+            let dvel_01 = dvel_0 * dvel_1;
+            let inv_lhs = (dvel_00 + dvel_11)
+                * crate::utils::simd_inv(
+                    dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
+                );
+            let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
+            let new_impulse = self.impulse - delta_impulse;
             let new_impulse = {
                 let _disable_fe_except =
                     crate::utils::DisableFloatingPointExceptionsFlags::
@@ -69,7 +79,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
                 new_impulse.simd_cap_magnitude(limit)
             };
             let dlambda = new_impulse - self.impulse;
-
             self.impulse = new_impulse;
 
             mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
@@ -89,7 +98,6 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
 }
 
 impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     fn zero() -> Self {
         Self {
             gcross2: na::zero(),
@@ -105,9 +113,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
     where
         AngVector<N>: WDot<AngVector<N>, Result = N>,
     {
-        let dimpulse =
-            -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
-        let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
+        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 dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
@@ -123,7 +130,6 @@ pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> {
 }
 
 impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     pub fn zero() -> Self {
         Self {
             normal_part: VelocityGroundConstraintNormalPart::zero(),
-- 
cgit