aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/dynamics/rigid_body.rs25
-rw-r--r--src/dynamics/solver/categorization.rs6
-rw-r--r--src/dynamics/solver/position_ground_constraint.rs2
-rw-r--r--src/dynamics/solver/position_ground_constraint_wide.rs2
-rw-r--r--src/dynamics/solver/velocity_constraint.rs2
-rw-r--r--src/dynamics/solver/velocity_constraint_wide.rs4
-rw-r--r--src/dynamics/solver/velocity_ground_constraint.rs2
-rw-r--r--src/dynamics/solver/velocity_ground_constraint_wide.rs2
-rw-r--r--src/geometry/contact_pair.rs5
-rw-r--r--src/geometry/narrow_phase.rs2
10 files changed, 43 insertions, 9 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..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;
}
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.
generated by cgit (git 2.34.1) at 2026-02-03 19:28:47 +0000