Implement dominance.

8 files changed, 11 insertions, 9 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index c920b69..5a00896 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -2,7 +2,6 @@ use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub(crate) fn categorize_contacts(
-    bodies: &RigidBodySet,
     manifolds: &[&mut ContactManifold],
     manifold_indices: &[ContactManifoldIndex],
     out_ground: &mut Vec<ContactManifoldIndex>,
@@ -10,10 +9,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/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
index b9dd497..b3ce8c7 100644
--- a/src/dynamics/solver/solver_constraints.rs
+++ b/src/dynamics/solver/solver_constraints.rs
@@ -51,7 +51,6 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
         self.not_ground_interactions.clear();
         self.ground_interactions.clear();
         categorize_contacts(
-            bodies,
             manifolds,
             manifold_indices,
             &mut self.ground_interactions,
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;
             }