Use contact ids instead of contact reordering in order to identify the impulse writeback location.

4 files changed, 37 insertions, 32 deletions

diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 2cac93d..63139cc 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -124,7 +124,7 @@ pub(crate) struct VelocityConstraint {
     pub mj_lambda1: usize,
     pub mj_lambda2: usize,
     pub manifold_id: ContactManifoldIndex,
-    pub manifold_contact_id: usize,
+    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS],
 }
@@ -168,7 +168,7 @@ impl VelocityConstraint {
                 mj_lambda1,
                 mj_lambda2,
                 manifold_id,
-                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
                 num_contacts: manifold_points.len() as u8,
             };
 
@@ -211,7 +211,7 @@ impl VelocityConstraint {
                 constraint.mj_lambda1 = mj_lambda1;
                 constraint.mj_lambda2 = mj_lambda2;
                 constraint.manifold_id = manifold_id;
-                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
                 constraint.num_contacts = manifold_points.len() as u8;
             }
 
@@ -224,6 +224,8 @@ impl VelocityConstraint {
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
                 constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+
                 // Normal part.
                 {
                     let gcross1 = rb1
@@ -382,19 +384,18 @@ impl VelocityConstraint {
 
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
         let manifold = &mut manifolds_all[self.manifold_id];
-        let k_base = self.manifold_contact_id;
 
         for k in 0..self.num_contacts as usize {
-            let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
-            active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
+            let contact_id = self.manifold_contact_id[k];
+            let active_contact = &mut manifold.points[contact_id as usize];
+            active_contact.data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {
-                active_contacts[k_base + k].data.tangent_impulse =
-                    self.elements[k].tangent_part[0].impulse;
+                active_contacts.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
             }
             #[cfg(feature = "dim3")]
             {
-                active_contacts[k_base + k].data.tangent_impulse = [
+                active_contact.data.tangent_impulse = [
                     self.elements[k].tangent_part[0].impulse,
                     self.elements[k].tangent_part[1].impulse,
                 ];
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 3e068e7..8838011 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -55,7 +55,7 @@ pub(crate) struct WVelocityConstraint {
     pub mj_lambda1: [usize; SIMD_WIDTH],
     pub mj_lambda2: [usize; SIMD_WIDTH],
     pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
-    pub manifold_contact_id: usize,
+    pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
 }
 
 impl WVelocityConstraint {
@@ -116,7 +116,7 @@ impl WVelocityConstraint {
                 mj_lambda1,
                 mj_lambda2,
                 manifold_id,
-                manifold_contact_id: l,
+                manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
                 num_contacts: num_points as u8,
             };
 
@@ -141,6 +141,8 @@ impl WVelocityConstraint {
                 let vel2 = linvel2 + angvel2.gcross(dp2);
 
                 constraint.limit = friction;
+                constraint.manifold_contact_id[k] =
+                    array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
 
                 // Normal part.
                 {
@@ -332,17 +334,17 @@ impl WVelocityConstraint {
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
-                let k_base = self.manifold_contact_id;
-                let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
-                active_contacts[k_base + k].data.impulse = impulses[ii];
+                let contact_id = self.manifold_contact_id[k][ii];
+                let active_contact = &mut manifold.points[contact_id as usize];
+                active_contact.data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]
                 {
-                    active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
+                    active_contact.data.tangent_impulse = tangent_impulses[ii];
                 }
                 #[cfg(feature = "dim3")]
                 {
-                    active_contacts[k_base + k].data.tangent_impulse =
+                    active_contact.data.tangent_impulse =
                         [tangent_impulses[ii], bitangent_impulses[ii]];
                 }
             }
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 88f864a..6f61b17 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -49,7 +49,7 @@ pub(crate) struct VelocityGroundConstraint {
     pub limit: Real,
     pub mj_lambda2: usize,
     pub manifold_id: ContactManifoldIndex,
-    pub manifold_contact_id: usize,
+    pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
 }
@@ -92,7 +92,7 @@ impl VelocityGroundConstraint {
                 limit: 0.0,
                 mj_lambda2,
                 manifold_id,
-                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
                 num_contacts: manifold_points.len() as u8,
             };
 
@@ -133,7 +133,7 @@ impl VelocityGroundConstraint {
                 constraint.limit = 0.0;
                 constraint.mj_lambda2 = mj_lambda2;
                 constraint.manifold_id = manifold_id;
-                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
                 constraint.num_contacts = manifold_points.len() as u8;
             }
 
@@ -145,6 +145,7 @@ impl VelocityGroundConstraint {
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
                 constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
 
                 // Normal part.
                 {
@@ -267,19 +268,18 @@ impl VelocityGroundConstraint {
     // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
     pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
         let manifold = &mut manifolds_all[self.manifold_id];
-        let k_base = self.manifold_contact_id;
 
         for k in 0..self.num_contacts as usize {
-            let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
-            active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
+            let contact_id = self.manifold_contact_id[k];
+            let active_contact = &mut manifold.points[contact_id as usize];
+            active_contact.data.impulse = self.elements[k].normal_part.impulse;
             #[cfg(feature = "dim2")]
             {
-                active_contacts[k_base + k].data.tangent_impulse =
-                    self.elements[k].tangent_part[0].impulse;
+                active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
             }
             #[cfg(feature = "dim3")]
             {
-                active_contacts[k_base + k].data.tangent_impulse = [
+                active_contact.data.tangent_impulse = [
                     self.elements[k].tangent_part[0].impulse,
                     self.elements[k].tangent_part[1].impulse,
                 ];
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 29ba5e9..6f279d8 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -51,7 +51,7 @@ pub(crate) struct WVelocityGroundConstraint {
     pub limit: SimdReal,
     pub mj_lambda2: [usize; SIMD_WIDTH],
     pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
-    pub manifold_contact_id: usize,
+    pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
 }
 
 impl WVelocityGroundConstraint {
@@ -111,7 +111,7 @@ impl WVelocityGroundConstraint {
                 limit: SimdReal::splat(0.0),
                 mj_lambda2,
                 manifold_id,
-                manifold_contact_id: l,
+                manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
                 num_contacts: num_points as u8,
             };
 
@@ -135,6 +135,8 @@ impl WVelocityGroundConstraint {
                 let vel2 = linvel2 + angvel2.gcross(dp2);
 
                 constraint.limit = friction;
+                constraint.manifold_contact_id[k] =
+                    array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
 
                 // Normal part.
                 {
@@ -281,17 +283,17 @@ impl WVelocityGroundConstraint {
 
             for ii in 0..SIMD_WIDTH {
                 let manifold = &mut manifolds_all[self.manifold_id[ii]];
-                let k_base = self.manifold_contact_id;
-                let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
-                active_contacts[k_base + k].data.impulse = impulses[ii];
+                let contact_id = self.manifold_contact_id[k][ii];
+                let active_contact = &mut manifold.points[contact_id as usize];
+                active_contact.data.impulse = impulses[ii];
 
                 #[cfg(feature = "dim2")]
                 {
-                    active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
+                    active_contact.data.tangent_impulse = tangent_impulses[ii];
                 }
                 #[cfg(feature = "dim3")]
                 {
-                    active_contacts[k_base + k].data.tangent_impulse =
+                    active_contact.data.tangent_impulse =
                         [tangent_impulses[ii], bitangent_impulses[ii]];
                 }
             }