aboutsummaryrefslogtreecommitdiff
path: root/src/dynamics
diff options
context:
space:
mode:
Diffstat (limited to 'src/dynamics')
-rw-r--r--src/dynamics/rigid_body.rs25
-rw-r--r--src/dynamics/solver/categorization.rs5
-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/solver_constraints.rs1
-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
9 files changed, 36 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..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;
}
generated by cgit (git 2.34.1) at 2026-02-03 19:25:34 +0000