From 152ada67ecd122fe38a9cae8b262542b4abf25fc Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Sun, 7 Mar 2021 17:34:49 +0100
Subject: Fix regression causing invalid contacts to be solved.

---
 src/dynamics/solver/velocity_constraint.rs                | 4 ++--
 src/dynamics/solver/velocity_constraint_element.rs        | 4 ++++
 src/dynamics/solver/velocity_constraint_wide.rs           | 4 ++--
 src/dynamics/solver/velocity_ground_constraint.rs         | 4 ++--
 src/dynamics/solver/velocity_ground_constraint_element.rs | 2 ++
 src/dynamics/solver/velocity_ground_constraint_wide.rs    | 4 ++--
 6 files changed, 14 insertions(+), 8 deletions(-)

(limited to 'src')

diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 434e6fb..643facb 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -286,7 +286,7 @@ impl VelocityConstraint {
         let mut mj_lambda2 = DeltaVel::zero();
 
         VelocityConstraintElement::warmstart_group(
-            &self.elements,
+            &self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
@@ -305,7 +305,7 @@ impl VelocityConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         VelocityConstraintElement::solve_group(
-            &mut self.elements,
+            &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index e80d3dd..1324c35 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -30,6 +30,7 @@ impl<N: SimdRealField> VelocityConstraintTangentPart<N> {
         }
     }
 
+    #[inline]
     pub fn warmstart(
         &self,
         tangents1: [&Vector<N>; DIM - 1],
@@ -50,6 +51,7 @@ impl<N: SimdRealField> VelocityConstraintTangentPart<N> {
         }
     }
 
+    #[inline]
     pub fn solve(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
@@ -193,6 +195,7 @@ impl<N: SimdRealField> VelocityConstraintElement<N> {
         }
     }
 
+    #[inline]
     pub fn warmstart_group(
         elements: &[Self],
         dir1: &Vector<N>,
@@ -221,6 +224,7 @@ impl<N: SimdRealField> VelocityConstraintElement<N> {
         }
     }
 
+    #[inline]
     pub fn solve_group(
         elements: &mut [Self],
         dir1: &Vector<N>,
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 691a983..673af54 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -217,7 +217,7 @@ impl WVelocityConstraint {
         };
 
         VelocityConstraintElement::warmstart_group(
-            &self.elements,
+            &self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
@@ -257,7 +257,7 @@ impl WVelocityConstraint {
         };
 
         VelocityConstraintElement::solve_group(
-            &mut self.elements,
+            &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 3509dd5..cabce5e 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -196,7 +196,7 @@ impl VelocityGroundConstraint {
         let mut mj_lambda2 = DeltaVel::zero();
 
         VelocityGroundConstraintElement::warmstart_group(
-            &self.elements,
+            &self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
@@ -212,7 +212,7 @@ impl VelocityGroundConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         VelocityGroundConstraintElement::solve_group(
-            &mut self.elements,
+            &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 43e3e01..8bcdc82 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -28,6 +28,7 @@ impl<N: SimdRealField> VelocityGroundConstraintTangentPart<N> {
         }
     }
 
+    #[inline]
     pub fn warmstart(
         &self,
         tangents1: [&Vector<N>; DIM - 1],
@@ -40,6 +41,7 @@ impl<N: SimdRealField> VelocityGroundConstraintTangentPart<N> {
         }
     }
 
+    #[inline]
     pub fn solve(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 22da895..ba1c46a 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -194,7 +194,7 @@ impl WVelocityGroundConstraint {
         };
 
         VelocityGroundConstraintElement::warmstart_group(
-            &self.elements,
+            &self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
@@ -219,7 +219,7 @@ impl WVelocityGroundConstraint {
         };
 
         VelocityGroundConstraintElement::solve_group(
-            &mut self.elements,
+            &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
             &self.tangent1,
-- 
cgit