Fix regression causing invalid contacts to be solved.

2021-03-07 17:34:49 +01:00
parent 0e4393ba9e
commit 152ada67ec
6 changed files with 14 additions and 8 deletions
--- 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,