From f74b8401ad9ef50b8cdbf1f43a2b21f6c42b0ebc Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Sun, 2 Jan 2022 14:47:40 +0100
Subject: Implement multibody joints and the new solver

---
 src/dynamics/solver/velocity_constraint_element.rs | 114 +++++----------------
 1 file changed, 24 insertions(+), 90 deletions(-)

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

diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index cb6d476..406f68e 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -9,48 +9,23 @@ pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
     #[cfg(feature = "dim2")]
-    pub impulse: [N; DIM - 1],
+    pub impulse: na::Vector1<N>,
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
     pub r: [N; DIM - 1],
 }
 
 impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     fn zero() -> Self {
         Self {
             gcross1: [na::zero(); DIM - 1],
             gcross2: [na::zero(); DIM - 1],
             rhs: [na::zero(); DIM - 1],
-            #[cfg(feature = "dim2")]
-            impulse: [na::zero(); DIM - 1],
-            #[cfg(feature = "dim3")]
             impulse: na::zero(),
             r: [na::zero(); DIM - 1],
         }
     }
 
-    #[inline]
-    pub fn warmstart(
-        &self,
-        tangents1: [&Vector<N>; DIM - 1],
-        im1: N,
-        im2: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
-    {
-        for j in 0..DIM - 1 {
-            mj_lambda1.linear += tangents1[j] * (im1 * self.impulse[j]);
-            mj_lambda1.angular += self.gcross1[j] * self.impulse[j];
-
-            mj_lambda2.linear += tangents1[j] * (-im2 * self.impulse[j]);
-            mj_lambda2.angular += self.gcross2[j] * self.impulse[j];
-        }
-    }
-
     #[inline]
     pub fn solve(
         &mut self,
@@ -125,40 +100,23 @@ pub(crate) struct VelocityConstraintNormalPart<N: SimdRealField + Copy> {
     pub gcross1: AngVector<N>,
     pub gcross2: AngVector<N>,
     pub rhs: N,
+    pub rhs_wo_bias: N,
     pub impulse: N,
     pub r: N,
 }
 
 impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     fn zero() -> Self {
         Self {
             gcross1: na::zero(),
             gcross2: na::zero(),
             rhs: na::zero(),
+            rhs_wo_bias: na::zero(),
             impulse: na::zero(),
             r: na::zero(),
         }
     }
 
-    #[inline]
-    pub fn warmstart(
-        &self,
-        dir1: &Vector<N>,
-        im1: N,
-        im2: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-    {
-        mj_lambda1.linear += dir1 * (im1 * self.impulse);
-        mj_lambda1.angular += self.gcross1 * self.impulse;
-
-        mj_lambda2.linear += dir1 * (-im2 * self.impulse);
-        mj_lambda2.angular += self.gcross2 * self.impulse;
-    }
-
     #[inline]
     pub fn solve(
         &mut self,
@@ -193,7 +151,6 @@ pub(crate) struct VelocityConstraintElement<N: SimdRealField + Copy> {
 }
 
 impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     pub fn zero() -> Self {
         Self {
             normal_part: VelocityConstraintNormalPart::zero(),
@@ -201,35 +158,6 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
         }
     }
 
-    #[inline]
-    pub fn warmstart_group(
-        elements: &[Self],
-        dir1: &Vector<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im1: N,
-        im2: N,
-        mj_lambda1: &mut DeltaVel<N>,
-        mj_lambda2: &mut DeltaVel<N>,
-    ) where
-        Vector<N>: WBasis,
-        AngVector<N>: WDot<AngVector<N>, Result = N>,
-        N::Element: SimdRealField + Copy,
-    {
-        #[cfg(feature = "dim3")]
-        let tangents1 = [tangent1, &dir1.cross(&tangent1)];
-        #[cfg(feature = "dim2")]
-        let tangents1 = [&dir1.orthonormal_vector()];
-
-        for element in elements {
-            element
-                .tangent_part
-                .warmstart(tangents1, im1, im2, mj_lambda1, mj_lambda2);
-            element
-                .normal_part
-                .warmstart(dir1, im1, im2, mj_lambda1, mj_lambda2);
-        }
-    }
-
     #[inline]
     pub fn solve_group(
         elements: &mut [Self],
@@ -240,28 +168,34 @@ impl<N: SimdRealField + Copy> VelocityConstraintElement<N> {
         limit: N,
         mj_lambda1: &mut DeltaVel<N>,
         mj_lambda2: &mut DeltaVel<N>,
+        solve_normal: bool,
+        solve_friction: bool,
     ) where
         Vector<N>: WBasis,
         AngVector<N>: WDot<AngVector<N>, Result = N>,
         N::Element: SimdRealField + Copy,
     {
-        // Solve friction.
-        #[cfg(feature = "dim3")]
-        let tangents1 = [tangent1, &dir1.cross(&tangent1)];
-        #[cfg(feature = "dim2")]
-        let tangents1 = [&dir1.orthonormal_vector()];
-
-        for element in elements.iter_mut() {
-            let limit = limit * element.normal_part.impulse;
-            let part = &mut element.tangent_part;
-            part.solve(tangents1, im1, im2, limit, mj_lambda1, mj_lambda2);
+        // Solve penetration.
+        if solve_normal {
+            for element in elements.iter_mut() {
+                element
+                    .normal_part
+                    .solve(&dir1, im1, im2, mj_lambda1, mj_lambda2);
+            }
         }
 
-        // Solve penetration.
-        for element in elements.iter_mut() {
-            element
-                .normal_part
-                .solve(&dir1, im1, im2, mj_lambda1, mj_lambda2);
+        // Solve friction.
+        if solve_friction {
+            #[cfg(feature = "dim3")]
+            let tangents1 = [tangent1, &dir1.cross(&tangent1)];
+            #[cfg(feature = "dim2")]
+            let tangents1 = [&dir1.orthonormal_vector()];
+
+            for element in elements.iter_mut() {
+                let limit = limit * element.normal_part.impulse;
+                let part = &mut element.tangent_part;
+                part.solve(tangents1, im1, im2, limit, mj_lambda1, mj_lambda2);
+            }
         }
     }
 }
-- 
cgit