Merge pull request #122 from dimforge/dominance

Implement dominance groups

7 files changed, 12 insertions, 8 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index c920b69..e3327c6 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -2,7 +2,7 @@ use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub(crate) fn categorize_contacts(
-    bodies: &RigidBodySet,
+    _bodies: &RigidBodySet, // Unused but useful to simplify the parallel code.
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
     out_ground: &mut Vec<ContactManifoldIndex>,
@@ -10,10 +10,8 @@ pub(crate) fn categorize_contacts(
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
-        let rb1 = &bodies[manifold.data.body_pair.body1];
-        let rb2 = &bodies[manifold.data.body_pair.body2];
 
-        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+        if manifold.data.relative_dominance != 0 {
             out_ground.push(*manifold_i)
         } else {
             out_not_ground.push(*manifold_i)
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index 4ab07eb..e1a4016 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs
@@ -30,7 +30,7 @@ impl PositionGroundConstraint {
     ) {
         let mut rb1 = &bodies[manifold.data.body_pair.body1];
         let mut rb2 = &bodies[manifold.data.body_pair.body2];
-        let flip = !rb2.is_dynamic();
+        let flip = manifold.data.relative_dominance < 0;
 
         let n1 = if flip {
             std::mem::swap(&mut rb1, &mut rb2);
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
index f52b3f4..1869c9c 100644
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ b/src/dynamics/solver/position_ground_constraint_wide.rs
@@ -39,7 +39,7 @@ impl WPositionGroundConstraint {
         let mut flipped = [false; SIMD_WIDTH];
 
         for ii in 0..SIMD_WIDTH {
-            if !rbs2[ii].is_dynamic() {
+            if manifolds[ii].data.relative_dominance < 0 {
                 flipped[ii] = true;
                 std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
             }
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 2de9807..abc46c9 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -144,6 +144,8 @@ impl VelocityConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
     ) {
+        assert_eq!(manifold.data.relative_dominance, 0);
+
         let inv_dt = params.inv_dt();
         let rb1 = &bodies[manifold.data.body_pair.body1];
         let rb2 = &bodies[manifold.data.body_pair.body2];
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 97fa261..82a764e 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -67,6 +67,10 @@ impl WVelocityConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         push: bool,
     ) {
+        for ii in 0..SIMD_WIDTH {
+            assert_eq!(manifolds[ii].data.relative_dominance, 0);
+        }
+
         let inv_dt = SimdReal::splat(params.inv_dt());
         let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
         let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index beb07ed..a3a5563 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -66,7 +66,7 @@ impl VelocityGroundConstraint {
         let inv_dt = params.inv_dt();
         let mut rb1 = &bodies[manifold.data.body_pair.body1];
         let mut rb2 = &bodies[manifold.data.body_pair.body2];
-        let flipped = !rb2.is_dynamic();
+        let flipped = manifold.data.relative_dominance < 0;
 
         let (force_dir1, flipped_multiplier) = if flipped {
             std::mem::swap(&mut rb1, &mut rb2);
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 6e7216a..5339d8a 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -69,7 +69,7 @@ impl WVelocityGroundConstraint {
         let mut flipped = [1.0; SIMD_WIDTH];
 
         for ii in 0..SIMD_WIDTH {
-            if !rbs2[ii].is_dynamic() {
+            if manifolds[ii].data.relative_dominance < 0 {
                 std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
                 flipped[ii] = -1.0;
             }