Implement dominance.

11 files changed, 42 insertions, 10 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 859beb2..32c0cca 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -92,6 +92,8 @@ pub struct RigidBody {
     pub(crate) changes: RigidBodyChanges,
     /// The status of the body, governing how it is affected by external forces.
     pub body_status: BodyStatus,
+    /// The dominance group this rigid-body is part of.
+    dominance_group: i8,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
 }
@@ -122,6 +124,7 @@ impl RigidBody {
             flags: RigidBodyFlags::empty(),
             changes: RigidBodyChanges::all(),
             body_status: BodyStatus::Dynamic,
+            dominance_group: 0,
             user_data: 0,
         }
     }
@@ -159,6 +162,19 @@ impl RigidBody {
         &self.mass_properties
     }
 
+    /// The dominance group of this rigid-body.
+    ///
+    /// This method always returns `i8::MAX + 1` for non-dynamic
+    /// rigid-bodies.
+    #[inline]
+    pub fn effective_dominance_group(&self) -> i16 {
+        if self.is_dynamic() {
+            self.dominance_group as i16
+        } else {
+            i8::MAX as i16 + 1
+        }
+    }
+
     /// Sets the rigid-body's mass properties.
     ///
     /// If `wake_up` is `true` then the rigid-body will be woken up if it was
@@ -648,6 +664,7 @@ pub struct RigidBodyBuilder {
     mass_properties: MassProperties,
     can_sleep: bool,
     sleeping: bool,
+    dominance_group: i8,
     user_data: u128,
 }
 
@@ -666,6 +683,7 @@ impl RigidBodyBuilder {
             mass_properties: MassProperties::zero(),
             can_sleep: true,
             sleeping: false,
+            dominance_group: 0,
             user_data: 0,
         }
     }
@@ -691,6 +709,12 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// Sets the dominance group of this rigid-body.
+    pub fn dominance_group(mut self, group: i8) -> Self {
+        self.dominance_group = group;
+        self
+    }
+
     /// Sets the initial translation of the rigid-body to be created.
     #[cfg(feature = "dim2")]
     pub fn translation(mut self, x: Real, y: Real) -> Self {
@@ -880,6 +904,7 @@ impl RigidBodyBuilder {
         rb.angular_damping = self.angular_damping;
         rb.gravity_scale = self.gravity_scale;
         rb.flags = self.flags;
+        rb.dominance_group = self.dominance_group;
 
         if self.can_sleep && self.sleeping {
             rb.sleep();
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;
             }
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 50094ca..5ef1ac1 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -113,6 +113,8 @@ pub struct ContactManifoldData {
     /// The contacts that will be seen by the constraints solver for computing forces.
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     pub solver_contacts: Vec<SolverContact>,
+    /// The relative dominance of the bodies involved in this contact manifold.
+    pub relative_dominance: i16,
     /// A user-defined piece of data.
     pub user_data: u32,
 }
@@ -122,7 +124,7 @@ pub struct ContactManifoldData {
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct SolverContact {
     /// The index of the manifold contact used to generate this solver contact.
-    pub contact_id: u8,
+    pub(crate) contact_id: u8,
     /// The world-space contact point.
     pub point: Point<Real>,
     /// The distance between the two original contacts points along the contact normal.
@@ -177,6 +179,7 @@ impl ContactManifoldData {
             solver_flags,
             normal: Vector::zeros(),
             solver_contacts: Vec::new(),
+            relative_dominance: 0,
             user_data: 0,
         }
     }
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index d05b19a..28c53f1 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -555,6 +555,8 @@ impl NarrowPhase {
                 manifold.data.solver_contacts.clear();
                 manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent());
                 manifold.data.solver_flags = solver_flags;
+                manifold.data.relative_dominance =
+                    rb1.effective_dominance_group() - rb2.effective_dominance_group();
                 manifold.data.normal = world_pos1 * manifold.local_n1;
 
                 // Generate solver contacts.