diff options
| -rw-r--r-- | src/dynamics/joint/multibody_joint/multibody.rs | 29 | ||||
| -rw-r--r-- | src/geometry/narrow_phase.rs | 30 |
2 files changed, 50 insertions, 9 deletions
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index fb56087..2088148 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -75,6 +75,7 @@ pub struct Multibody { ndofs: usize, pub(crate) root_is_dynamic: bool, pub(crate) solver_id: usize, + self_contacts_enabled: bool, /* * Workspaces. @@ -93,6 +94,10 @@ impl Default for Multibody { impl Multibody { /// Creates a new multibody with no link. pub fn new() -> Self { + Self::with_self_contacts(true) + } + + pub(crate) fn with_self_contacts(self_contacts_enabled: bool) -> Self { Multibody { links: MultibodyLinkVec(Vec::new()), velocities: DVector::zeros(0), @@ -103,6 +108,7 @@ impl Multibody { inv_augmented_mass: LU::new(DMatrix::zeros(0, 0)), acc_augmented_mass: DMatrix::zeros(0, 0), acc_inv_augmented_mass: LU::new(DMatrix::zeros(0, 0)), + augmented_mass_indices: IndexSequence::new(), ndofs: 0, solver_id: 0, workspace: MultibodyWorkspace::new(), @@ -110,12 +116,13 @@ impl Multibody { coriolis_w: Vec::new(), i_coriolis_dt: Jacobian::zeros(0), root_is_dynamic: false, + self_contacts_enabled, // solver_workspace: Some(SolverWorkspace::new()), } } - pub(crate) fn with_root(handle: RigidBodyHandle) -> Self { - let mut mb = Multibody::new(); + pub(crate) fn with_root(handle: RigidBodyHandle, self_contacts_enabled: bool) -> Self { + let mut mb = Multibody::with_self_contacts(self_contacts_enabled); // NOTE: we have no way of knowing if the root in fixed at this point, so // we mark it as dynamic and will fixe later with `Self::update_root_type`. mb.root_is_dynamic = true; @@ -138,7 +145,7 @@ impl Multibody { continue; } else if is_new_root { link2mb[i] = result.len(); - result.push(Multibody::new()); + result.push(Multibody::with_self_contacts(self.self_contacts_enabled)); } else { link2mb[i] = link2mb[link.parent_internal_id] } @@ -232,6 +239,22 @@ impl Multibody { self.workspace.resize(self.links.len(), self.ndofs); } + /// Whether self-contacts are enabled on this multibody. + /// + /// If set to `false` no two link from this multibody can generate contacts, even + /// if the contact is enabled on the individual joint with [`GenericJoint::contacts_enabled`]. + pub fn self_contacts_enabled(&self) -> bool { + self.self_contacts_enabled + } + + /// Sets whether self-contacts are enabled on this multibody. + /// + /// If set to `false` no two link from this multibody can generate contacts, even + /// if the contact is enabled on the individual joint with [`GenericJoint::contacts_enabled`]. + pub fn set_self_contacts_enabled(&mut self, enabled: bool) { + self.self_contacts_enabled = enabled; + } + /// The inverse augmented mass matrix of this multibody. pub fn inv_augmented_mass(&self) -> &LU<Real, Dyn, Dyn> { &self.inv_augmented_mass diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 1eddd73..717fa25 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -835,12 +835,30 @@ impl NarrowPhase { } } - if let Some((_, _, mb_link)) = - multibody_joints.joint_between(co_parent1.handle, co_parent2.handle) - { - if !mb_link.joint.data.contacts_enabled { - pair.clear(); - break 'emit_events; + let link1 = multibody_joints.rigid_body_link(co_parent1.handle); + let link2 = multibody_joints.rigid_body_link(co_parent2.handle); + + if let (Some(link1),Some(link2)) = (link1, link2) { + // If both bodies belong to the same multibody, apply some additional built-in + // contact filtering rules. + if link1.multibody == link2.multibody { + // 1) check if self-contacts is enabled. + if let Some(mb) = multibody_joints.get_multibody(link1.multibody) { + if !mb.self_contacts_enabled() { + pair.clear(); + break 'emit_events; + } + } + + // 2) if they are attached by a joint, check if contacts is disabled. + if let Some((_, _, mb_link)) = + multibody_joints.joint_between(co_parent1.handle, co_parent2.handle) + { + if !mb_link.joint.data.contacts_enabled { + pair.clear(); + break 'emit_events; + } + } } } } |