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_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,