Add the ability to disable contacts between two rigid-bodies attached by joints

12 files changed, 197 insertions, 14 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index 6a86a2a..6d3acc5 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -134,7 +134,7 @@ impl IntegrationParameters {
         1.0 / (1.0 + cfm_coeff)
     }
 
-    /// The CFM (constranits force mixing) coefficient applied to all joints for constraints regularization
+    /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization
     pub fn joint_cfm_coeff(&self) -> Real {
         // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
         let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs
index 92f0d7c..75c8228 100644
--- a/src/dynamics/joint/fixed_joint.rs
+++ b/src/dynamics/joint/fixed_joint.rs
@@ -6,7 +6,8 @@ use crate::math::{Isometry, Point, Real};
 #[repr(transparent)]
 /// A fixed joint, locks all relative motion between two bodies.
 pub struct FixedJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 impl Default for FixedJoint {
@@ -23,6 +24,17 @@ impl FixedJoint {
         Self { data }
     }
 
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub fn contacts_enabled(&self) -> bool {
+        self.data.contacts_enabled
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
+        self.data.set_contacts_enabled(enabled);
+        self
+    }
+
     /// The joint’s frame, expressed in the first rigid-body’s local-space.
     #[must_use]
     pub fn local_frame1(&self) -> &Isometry<Real> {
@@ -81,7 +93,7 @@ impl Into<GenericJoint> for FixedJoint {
 /// Create fixed joints using the builder pattern.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq, Default)]
-pub struct FixedJointBuilder(FixedJoint);
+pub struct FixedJointBuilder(pub FixedJoint);
 
 impl FixedJointBuilder {
     /// Creates a new builder for fixed joints.
@@ -89,6 +101,13 @@ impl FixedJointBuilder {
         Self(FixedJoint::new())
     }
 
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    #[must_use]
+    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
+        self.0.set_contacts_enabled(enabled);
+        self
+    }
+
     /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
     #[must_use]
     pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index b3277e9..1cf96cb 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -206,6 +206,8 @@ pub struct GenericJoint {
     ///
     /// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
     pub motors: [JointMotor; SPATIAL_DIM],
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub contacts_enabled: bool,
 }
 
 impl Default for GenericJoint {
@@ -219,6 +221,7 @@ impl Default for GenericJoint {
             coupled_axes: JointAxesMask::empty(),
             limits: [JointLimits::default(); SPATIAL_DIM],
             motors: [JointMotor::default(); SPATIAL_DIM],
+            contacts_enabled: true,
         }
     }
 }
@@ -275,6 +278,17 @@ impl GenericJoint {
         self
     }
 
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub fn contacts_enabled(&self) -> bool {
+        self.contacts_enabled
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
+        self.contacts_enabled = enabled;
+        self
+    }
+
     /// The principal (local X) axis of this joint, expressed in the first rigid-body’s local-space.
     #[must_use]
     pub fn local_axis1(&self) -> UnitVector<Real> {
@@ -481,8 +495,9 @@ impl GenericJoint {
 }
 
 /// Create generic joints using the builder pattern.
-#[derive(Copy, Clone, Debug)]
-pub struct GenericJointBuilder(GenericJoint);
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq)]
+pub struct GenericJointBuilder(pub GenericJoint);
 
 impl GenericJointBuilder {
     /// Creates a new generic joint builder.
@@ -498,6 +513,13 @@ impl GenericJointBuilder {
         self
     }
 
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    #[must_use]
+    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
+        self.0.contacts_enabled = enabled;
+        self
+    }
+
     /// Sets the joint’s frame, expressed in the first rigid-body’s local-space.
     #[must_use]
     pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index c4ec734..7409172 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -71,6 +71,20 @@ impl ImpulseJointSet {
         &self.joint_graph
     }
 
+    /// Iterates through all the joints between two rigid-bodies.
+    pub fn joints_between<'a>(
+        &'a self,
+        body1: RigidBodyHandle,
+        body2: RigidBodyHandle,
+    ) -> impl Iterator<Item = (ImpulseJointHandle, &'a ImpulseJoint)> {
+        self.rb_graph_ids
+            .get(body1.0)
+            .zip(self.rb_graph_ids.get(body2.0))
+            .into_iter()
+            .flat_map(move |(id1, id2)| self.joint_graph.interaction_pair(*id1, *id2).into_iter())
+            .map(|inter| (inter.2.handle, inter.2))
+    }
+
     /// Iterates through all the impulse joints attached to the given rigid-body.
     pub fn attached_joints<'a>(
         &'a self,
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index f1b2ffe..c25de73 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -321,6 +321,41 @@ impl MultibodyJointSet {
         ))
     }
 
+    /// Returns the the joint between two rigid-bodies (if it exists).
+    pub fn joint_between(
+        &self,
+        rb1: RigidBodyHandle,
+        rb2: RigidBodyHandle,
+    ) -> Option<(MultibodyJointHandle, &Multibody, &MultibodyLink)> {
+        let id1 = self.rb2mb.get(rb1.0)?;
+        let id2 = self.rb2mb.get(rb2.0)?;
+
+        // Both bodies must be part of the same multibody.
+        if id1.multibody != id2.multibody {
+            return None;
+        }
+
+        let mb = self.multibodies.get(id1.multibody.0)?;
+
+        // NOTE: if there is a joint between these two bodies, then
+        //       one of the bodies must be the parent of the other.
+        let link1 = mb.link(id1.id)?;
+        let parent1 = link1.parent_id()?;
+
+        if parent1 == id2.id {
+            Some((MultibodyJointHandle(rb1.0), mb, &link1))
+        } else {
+            let link2 = mb.link(id2.id)?;
+            let parent2 = link2.parent_id()?;
+
+            if parent2 == id1.id {
+                Some((MultibodyJointHandle(rb2.0), mb, &link2))
+            } else {
+                None
+            }
+        }
+    }
+
     /// Iterates through all the joints attached to the given rigid-body.
     pub fn attached_joints(
         &self,
diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs
index 667999f..1e7a016 100644
--- a/src/dynamics/joint/prismatic_joint.rs
+++ b/src/dynamics/joint/prismatic_joint.rs
@@ -9,7 +9,8 @@ use super::{JointLimits, JointMotor};
 #[repr(transparent)]
 /// A prismatic joint, locks all relative motion between two bodies except for translation along the joint’s principal axis.
 pub struct PrismaticJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 impl PrismaticJoint {
@@ -29,6 +30,17 @@ impl PrismaticJoint {
         &self.data
     }
 
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub fn contacts_enabled(&self) -> bool {
+        self.data.contacts_enabled
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
+        self.data.set_contacts_enabled(enabled);
+        self
+    }
+
     /// The joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
     pub fn local_anchor1(&self) -> Point<Real> {
@@ -149,8 +161,9 @@ impl Into<GenericJoint> for PrismaticJoint {
 /// Create prismatic joints using the builder pattern.
 ///
 /// A prismatic joint locks all relative motion except for translations along the joint’s principal axis.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
-pub struct PrismaticJointBuilder(PrismaticJoint);
+pub struct PrismaticJointBuilder(pub PrismaticJoint);
 
 impl PrismaticJointBuilder {
     /// Creates a new builder for prismatic joints.
@@ -160,6 +173,13 @@ impl PrismaticJointBuilder {
         Self(PrismaticJoint::new(axis))
     }
 
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    #[must_use]
+    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
+        self.0.set_contacts_enabled(enabled);
+        self
+    }
+
     /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
     pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs
index 28e3968..9400ba8 100644
--- a/src/dynamics/joint/revolute_joint.rs
+++ b/src/dynamics/joint/revolute_joint.rs
@@ -10,7 +10,8 @@ use crate::math::UnitVector;
 #[repr(transparent)]
 /// A revolute joint, locks all relative motion except for rotation along the joint’s principal axis.
 pub struct RevoluteJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 impl RevoluteJoint {
@@ -38,6 +39,17 @@ impl RevoluteJoint {
         &self.data
     }
 
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub fn contacts_enabled(&self) -> bool {
+        self.data.contacts_enabled
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
+        self.data.set_contacts_enabled(enabled);
+        self
+    }
+
     /// The joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
     pub fn local_anchor1(&self) -> Point<Real> {
@@ -136,7 +148,7 @@ impl Into<GenericJoint> for RevoluteJoint {
 /// A revolute joint locks all relative motion except for rotations along the joint’s principal axis.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
-pub struct RevoluteJointBuilder(RevoluteJoint);
+pub struct RevoluteJointBuilder(pub RevoluteJoint);
 
 impl RevoluteJointBuilder {
     /// Creates a new revolute joint builder.
@@ -153,6 +165,13 @@ impl RevoluteJointBuilder {
         Self(RevoluteJoint::new(axis))
     }
 
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    #[must_use]
+    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
+        self.0.set_contacts_enabled(enabled);
+        self
+    }
+
     /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
     pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs
index 89ab7b1..bce393c 100644
--- a/src/dynamics/joint/spherical_joint.rs
+++ b/src/dynamics/joint/spherical_joint.rs
@@ -9,7 +9,8 @@ use super::JointLimits;
 #[repr(transparent)]
 /// A spherical joint, locks all relative translations between two bodies.
 pub struct SphericalJoint {
-    data: GenericJoint,
+    /// The underlying joint data.
+    pub data: GenericJoint,
 }
 
 impl Default for SphericalJoint {
@@ -30,6 +31,17 @@ impl SphericalJoint {
         &self.data
     }
 
+    /// Are contacts between the attached rigid-bodies enabled?
+    pub fn contacts_enabled(&self) -> bool {
+        self.data.contacts_enabled
+    }
+
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    pub fn set_contacts_enabled(&mut self, enabled: bool) -> &mut Self {
+        self.data.set_contacts_enabled(enabled);
+        self
+    }
+
     /// The joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
     pub fn local_anchor1(&self) -> Point<Real> {
@@ -132,7 +144,7 @@ impl Into<GenericJoint> for SphericalJoint {
 /// Create spherical joints using the builder pattern.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
-pub struct SphericalJointBuilder(SphericalJoint);
+pub struct SphericalJointBuilder(pub SphericalJoint);
 
 impl Default for SphericalJointBuilder {
     fn default() -> Self {
@@ -146,6 +158,13 @@ impl SphericalJointBuilder {
         Self(SphericalJoint::new())
     }
 
+    /// Sets whether contacts between the attached rigid-bodies are enabled.
+    #[must_use]
+    pub fn contacts_enabled(mut self, enabled: bool) -> Self {
+        self.0.set_contacts_enabled(enabled);
+        self
+    }
+
     /// Sets the joint’s anchor, expressed in the local-space of the first rigid-body.
     #[must_use]
     pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 5eca5a2..eaf3967 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -20,7 +20,7 @@ pub struct RigidBody {
     pub(crate) pos: RigidBodyPosition,
     pub(crate) mprops: RigidBodyMassProps,
     // NOTE: we need this so that the CCD can use the actual velocities obtained
-    //       by the velocity solver with bias. If we switch to intepolation, we
+    //       by the velocity solver with bias. If we switch to interpolation, we
     //       should remove this field.
     pub(crate) integrated_vels: RigidBodyVelocity,
     pub(crate) vels: RigidBodyVelocity,
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index a1256b8..ecd1623 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -4,7 +4,8 @@ use rayon::prelude::*;
 use crate::data::graph::EdgeIndex;
 use crate::data::Coarena;
 use crate::dynamics::{
-    CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType,
+    CoefficientCombineRule, ImpulseJointSet, IslandManager, RigidBodyDominance, RigidBodySet,
+    RigidBodyType,
 };
 use crate::geometry::{
     BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
@@ -16,7 +17,7 @@ use crate::pipeline::{
     ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext,
     PhysicsHooks,
 };
-use crate::prelude::CollisionEventFlags;
+use crate::prelude::{CollisionEventFlags, MultibodyJointSet};
 use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
 use parry::utils::IsometryOpt;
 use std::collections::HashMap;
@@ -774,6 +775,8 @@ impl NarrowPhase {
         prediction_distance: Real,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
         modified_colliders: &[ColliderHandle],
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
@@ -812,6 +815,27 @@ impl NarrowPhase {
                     rb_type2 = bodies[co_parent2.handle].body_type;
                 }
 
+                // Deal with contacts disabled between bodies attached by joints.
+                if let (Some(co_parent1), Some(co_parent2)) = (&co1.parent, &co2.parent) {
+                    for (_, joint) in
+                        impulse_joints.joints_between(co_parent1.handle, co_parent2.handle)
+                    {
+                        if !joint.data.contacts_enabled {
+                            pair.clear();
+                            break 'emit_events;
+                        }
+                    }
+
+                    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;
+                        }
+                    }
+                }
+
                 // Filter based on the rigid-body types.
                 if !co1.flags.active_collision_types.test(rb_type1, rb_type2)
                     && !co2.flags.active_collision_types.test(rb_type1, rb_type2)
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index a27e5a8..ac92a04 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -1,5 +1,6 @@
 //! Physics pipeline structures.
 
+use crate::dynamics::{ImpulseJointSet, MultibodyJointSet};
 use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase,
 };
@@ -81,6 +82,8 @@ impl CollisionPipeline {
             prediction_distance,
             bodies,
             colliders,
+            &ImpulseJointSet::new(),
+            &MultibodyJointSet::new(),
             modified_colliders,
             hooks,
             events,
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index d1c3b65..d347637 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -85,6 +85,8 @@ impl PhysicsPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
+        impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
         modified_colliders: &[ColliderHandle],
         removed_colliders: &[ColliderHandle],
         hooks: &dyn PhysicsHooks,
@@ -130,6 +132,8 @@ impl PhysicsPipeline {
             integration_parameters.prediction_distance,
             bodies,
             colliders,
+            impulse_joints,
+            multibody_joints,
             modified_colliders,
             hooks,
             events,
@@ -449,6 +453,8 @@ impl PhysicsPipeline {
             narrow_phase,
             bodies,
             colliders,
+            impulse_joints,
+            multibody_joints,
             &modified_colliders[..],
             &mut removed_colliders,
             hooks,
@@ -574,6 +580,8 @@ impl PhysicsPipeline {
                 narrow_phase,
                 bodies,
                 colliders,
+                impulse_joints,
+                multibody_joints,
                 &mut modified_colliders,
                 &mut removed_colliders,
                 hooks,