Complete the parallel solver fix

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -35,7 +35,7 @@ impl WVelocityGroundConstraint {
         manifolds: [&ContactManifold; SIMD_WIDTH],
         bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -188,11 +188,11 @@ impl WVelocityGroundConstraint {
                 }
             }
 
-            if push {
-                out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
-            } else {
-                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
+            if let Some(at) = insert_at {
+                out_constraints[at + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::GroupedGround(constraint);
+            } else {
+                out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
             }
         }
     }