feat: add the ability to disable all contacts between two links belon…

…ging to the same multibody

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,19 +108,21 @@ 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(),
             coriolis_v: Vec::new(),
             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

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;
+                                }
+                            }
                         }
                     }
                 }