Perform contact sorting in the narrow-phase directly.

1 files changed, 7 insertions, 3 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index cbb6bb8..fa2a23e 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -76,9 +76,13 @@ impl VelocityGroundConstraint {
 
         let mj_lambda2 = rb2.active_set_offset;
         let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
-        let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
 
-        for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
+        for (l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
             #[cfg(not(target_arch = "wasm32"))]
             let mut constraint = VelocityGroundConstraint {
                 dir1: force_dir1,
@@ -268,7 +272,7 @@ impl VelocityGroundConstraint {
         let k_base = self.manifold_contact_id;
 
         for k in 0..self.num_contacts as usize {
-            let active_contacts = manifold.active_contacts_mut();
+            let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
             active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {