Allow disabling colliders, rigid-bodies and impulse joints

3 files changed, 92 insertions, 9 deletions

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index bb1598d..4f0a791 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -5,6 +5,7 @@ use crate::utils::{WBasis, WReal};
 
 #[cfg(feature = "dim3")]
 use crate::dynamics::SphericalJoint;
+use crate::geometry::ColliderEnabled;
 
 #[cfg(feature = "dim3")]
 bitflags::bitflags! {
@@ -182,6 +183,19 @@ impl JointMotor {
     }
 }
 
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// Enum indicating whether or not a joint is enabled.
+pub enum JointEnabled {
+    /// The joint is enabled.
+    Enabled,
+    /// The joint wasn’t disabled by the user explicitly but it is attached to
+    /// a disabled rigid-body.
+    DisabledByAttachedBody,
+    /// The joint is disabled by the user explicitly.
+    Disabled,
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq)]
 /// A generic joint.
@@ -208,6 +222,8 @@ pub struct GenericJoint {
     pub motors: [JointMotor; SPATIAL_DIM],
     /// Are contacts between the attached rigid-bodies enabled?
     pub contacts_enabled: bool,
+    /// Whether or not the joint is enabled.
+    pub enabled: JointEnabled,
 }
 
 impl Default for GenericJoint {
@@ -222,6 +238,7 @@ impl Default for GenericJoint {
             limits: [JointLimits::default(); SPATIAL_DIM],
             motors: [JointMotor::default(); SPATIAL_DIM],
             contacts_enabled: true,
+            enabled: JointEnabled::Enabled,
         }
     }
 }
@@ -260,6 +277,27 @@ impl GenericJoint {
         }
     }
 
+    /// Is this joint enabled?
+    pub fn is_enabled(&self) -> bool {
+        self.enabled == JointEnabled::Enabled
+    }
+
+    /// Set whether this joint is enabled or not.
+    pub fn set_enabled(&mut self, enabled: bool) {
+        match self.enabled {
+            JointEnabled::Enabled | JointEnabled::DisabledByAttachedBody => {
+                if !enabled {
+                    self.enabled = JointEnabled::Disabled;
+                }
+            }
+            JointEnabled::Disabled => {
+                if enabled {
+                    self.enabled = JointEnabled::Enabled;
+                }
+            }
+        }
+    }
+
     /// Add the specified axes to the set of axes locked by this joint.
     pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self {
         self.locked_axes |= axes;
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 7409172..f21f950 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -72,11 +72,11 @@ impl ImpulseJointSet {
     }
 
     /// Iterates through all the joints between two rigid-bodies.
-    pub fn joints_between<'a>(
-        &'a self,
+    pub fn joints_between(
+        &self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
-    ) -> impl Iterator<Item = (ImpulseJointHandle, &'a ImpulseJoint)> {
+    ) -> impl Iterator<Item = (ImpulseJointHandle, &ImpulseJoint)> {
         self.rb_graph_ids
             .get(body1.0)
             .zip(self.rb_graph_ids.get(body2.0))
@@ -86,15 +86,15 @@ impl ImpulseJointSet {
     }
 
     /// Iterates through all the impulse joints attached to the given rigid-body.
-    pub fn attached_joints<'a>(
-        &'a self,
+    pub fn attached_joints(
+        &self,
         body: RigidBodyHandle,
     ) -> impl Iterator<
         Item = (
             RigidBodyHandle,
             RigidBodyHandle,
             ImpulseJointHandle,
-            &'a ImpulseJoint,
+            &ImpulseJoint,
         ),
     > {
         self.rb_graph_ids
@@ -104,6 +104,35 @@ impl ImpulseJointSet {
             .map(|inter| (inter.0, inter.1, inter.2.handle, inter.2))
     }
 
+    /// Iterates through all the impulse joints attached to the given rigid-body.
+    pub fn map_attached_joints_mut<'a>(
+        &'a mut self,
+        body: RigidBodyHandle,
+        mut f: impl FnMut(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, &mut ImpulseJoint),
+    ) {
+        self.rb_graph_ids.get(body.0).into_iter().for_each(|id| {
+            for inter in self.joint_graph.interactions_with_mut(*id) {
+                (f)(inter.0, inter.1, inter.3.handle, inter.3)
+            }
+        })
+    }
+
+    /// Iterates through all the enabled impulse joints attached to the given rigid-body.
+    pub fn attached_enabled_joints(
+        &self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<
+        Item = (
+            RigidBodyHandle,
+            RigidBodyHandle,
+            ImpulseJointHandle,
+            &ImpulseJoint,
+        ),
+    > {
+        self.attached_joints(body)
+            .filter(|inter| inter.3.data.is_enabled())
+    }
+
     /// Is the given joint handle valid?
     pub fn contains(&self, handle: ImpulseJointHandle) -> bool {
         self.joint_ids.contains(handle.0)
@@ -246,7 +275,7 @@ impl ImpulseJointSet {
         ImpulseJointHandle(handle)
     }
 
-    /// Retrieve all the impulse_joints happening between two active bodies.
+    /// Retrieve all the enabled impulse joints happening between two active bodies.
     // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
     pub(crate) fn select_active_interactions(
         &self,
@@ -264,7 +293,8 @@ impl ImpulseJointSet {
             let rb1 = &bodies[joint.body1];
             let rb2 = &bodies[joint.body2];
 
-            if (rb1.is_dynamic() || rb2.is_dynamic())
+            if joint.data.is_enabled()
+                && (rb1.is_dynamic() || rb2.is_dynamic())
                 && (!rb1.is_dynamic() || !rb1.is_sleeping())
                 && (!rb2.is_dynamic() || !rb2.is_sleeping())
             {
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index c25de73..a4b338a 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -372,7 +372,7 @@ impl MultibodyJointSet {
     }
 
     /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
-    /// by an multibody_joint.
+    /// by a multibody_joint.
     pub fn attached_bodies<'a>(
         &'a self,
         body: RigidBodyHandle,
@@ -384,6 +384,21 @@ impl MultibodyJointSet {
             .map(move |inter| crate::utils::select_other((inter.0, inter.1), body))
     }
 
+    /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
+    /// by an enabled multibody_joint.
+    pub fn bodies_attached_with_enabled_joint<'a>(
+        &'a self,
+        body: RigidBodyHandle,
+    ) -> impl Iterator<Item = RigidBodyHandle> + 'a {
+        self.attached_bodies(body).filter(move |other| {
+            if let Some((_, _, link)) = self.joint_between(body, *other) {
+                link.joint.data.is_enabled()
+            } else {
+                false
+            }
+        })
+    }
+
     /// Iterates through all the multibodies on this set.
     pub fn multibodies(&self) -> impl Iterator<Item = &Multibody> {
         self.multibodies.iter().map(|e| e.1)