Complete the parallel solver fix

1 files changed, 5 insertions, 6 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index d09df63..04acc88 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -33,7 +33,7 @@ impl VelocityGroundConstraint {
         manifold: &ContactManifold,
         bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -104,7 +104,7 @@ impl VelocityGroundConstraint {
             // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
             // for the moment.
             #[cfg(target_arch = "wasm32")]
-            let constraint = if push {
+            let constraint = if insert_at.is_none() {
                 let new_len = out_constraints.len() + 1;
                 unsafe {
                     out_constraints.resize_with(new_len, || {
@@ -212,11 +212,10 @@ impl VelocityGroundConstraint {
             }
 
             #[cfg(not(target_arch = "wasm32"))]
-            if push {
-                out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
+            if let Some(at) = insert_at {
+                out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint);
             } else {
-                out_constraints[manifold.data.constraint_index + _l] =
-                    AnyVelocityConstraint::NongroupedGround(constraint);
+                out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
             }
         }
     }