Allow disabling colliders, rigid-bodies and impulse joints

4 files changed, 171 insertions, 86 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index ac92a04..631388b 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -127,9 +127,20 @@ impl CollisionPipeline {
             None,
             bodies,
             colliders,
+            &mut ImpulseJointSet::new(),
+            &mut MultibodyJointSet::new(),
             &modified_bodies,
             &mut modified_colliders,
         );
+
+        // Disabled colliders are treated as if they were removed.
+        removed_colliders.extend(
+            modified_colliders
+                .iter()
+                .copied()
+                .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)),
+        );
+
         self.detect_collisions(
             prediction_distance,
             broad_phase,
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 391b39a..f05a3f0 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -422,6 +422,7 @@ impl PhysicsPipeline {
         // Apply modifications.
         let mut modified_colliders = colliders.take_modified();
         let mut removed_colliders = colliders.take_removed();
+
         super::user_changes::handle_user_changes_to_colliders(
             bodies,
             colliders,
@@ -433,10 +434,22 @@ impl PhysicsPipeline {
             Some(islands),
             bodies,
             colliders,
+            impulse_joints,
+            multibody_joints,
             &modified_bodies,
             &mut modified_colliders,
         );
 
+        // Disabled colliders are treated as if they were removed.
+        // NOTE: this must be called here, after handle_user_changes_to_rigid_bodies to take into
+        //       account colliders disabled because of their parent rigid-body.
+        removed_colliders.extend(
+            modified_colliders
+                .iter()
+                .copied()
+                .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)),
+        );
+
         // TODO: do this only on user-change.
         // TODO: do we want some kind of automatic inverse kinematics?
         for multibody in &mut multibody_joints.multibodies {
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 00825e2..6682370 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -358,12 +358,12 @@ impl QueryPipeline {
             fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, Aabb)) {
                 match self.mode {
                     QueryPipelineMode::CurrentPosition => {
-                        for (h, co) in self.colliders.iter() {
+                        for (h, co) in self.colliders.iter_enabled() {
                             f(h, co.shape.compute_aabb(&co.pos))
                         }
                     }
                     QueryPipelineMode::SweepTestWithNextPosition => {
-                        for (h, co) in self.colliders.iter() {
+                        for (h, co) in self.colliders.iter_enabled() {
                             if let Some(co_parent) = co.parent {
                                 let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position;
                                 let next_position = rb_next_pos * co_parent.pos_wrt_parent;
@@ -374,7 +374,7 @@ impl QueryPipeline {
                         }
                     }
                     QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
-                        for (h, co) in self.colliders.iter() {
+                        for (h, co) in self.colliders.iter_enabled() {
                             if let Some(co_parent) = co.parent {
                                 let rb = &self.bodies[co_parent.handle];
                                 let predicted_pos = rb.pos.integrate_forces_and_velocities(
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index e7da68a..adfd023 100644
--- a/src/pipeline/user_changes.rs
+++ b/src/pipeline/user_changes.rs
@@ -1,7 +1,10 @@
 use crate::dynamics::{
-    IslandManager, RigidBodyChanges, RigidBodyHandle, RigidBodySet, RigidBodyType,
+    ImpulseJointSet, IslandManager, JointEnabled, MultibodyJointSet, RigidBodyChanges,
+    RigidBodyHandle, RigidBodySet, RigidBodyType,
+};
+use crate::geometry::{
+    ColliderChanges, ColliderEnabled, ColliderHandle, ColliderPosition, ColliderSet,
 };
-use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet};
 
 pub(crate) fn handle_user_changes_to_colliders(
     bodies: &mut RigidBodySet,
@@ -21,10 +24,12 @@ pub(crate) fn handle_user_changes_to_colliders(
                 }
             }
 
-            if co
-                .changes
-                .intersects(ColliderChanges::SHAPE | ColliderChanges::LOCAL_MASS_PROPERTIES)
-            {
+            if co.changes.intersects(
+                ColliderChanges::SHAPE
+                    | ColliderChanges::LOCAL_MASS_PROPERTIES
+                    | ColliderChanges::ENABLED_OR_DISABLED
+                    | ColliderChanges::PARENT,
+            ) {
                 if let Some(rb) = co
                     .parent
                     .and_then(|p| bodies.get_mut_internal_with_modification_tracking(p.handle))
@@ -40,12 +45,15 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
     mut islands: Option<&mut IslandManager>,
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
     modified_bodies: &[RigidBodyHandle],
     modified_colliders: &mut Vec<ColliderHandle>,
 ) {
     enum FinalAction {
-        UpdateActiveKinematicSetId,
-        UpdateActiveDynamicSetId,
+        UpdateActiveKinematicSetId(usize),
+        UpdateActiveDynamicSetId(usize),
+        RemoveFromIsland,
     }
 
     for handle in modified_bodies {
@@ -62,89 +70,95 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
         let mut activation = rb.activation;
 
         {
-            // The body's status changed. We need to make sure
-            // it is on the correct active set.
-            if let Some(islands) = islands.as_deref_mut() {
-                if changes.contains(RigidBodyChanges::TYPE) {
-                    match rb.body_type {
-                        RigidBodyType::Dynamic => {
-                            // Remove from the active kinematic set if it was there.
-                            if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) {
-                                islands.active_kinematic_set.swap_remove(ids.active_set_id);
-                                final_action = Some((
-                                    FinalAction::UpdateActiveKinematicSetId,
-                                    ids.active_set_id,
-                                ));
-                            }
-
-                            // Add to the active dynamic set.
-                            activation.wake_up(true);
-                            // Make sure the sleep change flag is set (even if for some
-                            // reasons the rigid-body was already awake) to make
-                            // sure the code handling sleeping change adds the body to
-                            // the active_dynamic_set.
-                            changes.set(RigidBodyChanges::SLEEP, true);
-                        }
-                        RigidBodyType::KinematicVelocityBased
-                        | RigidBodyType::KinematicPositionBased => {
-                            // Remove from the active dynamic set if it was there.
-                            if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle) {
-                                islands.active_dynamic_set.swap_remove(ids.active_set_id);
-                                final_action = Some((
-                                    FinalAction::UpdateActiveDynamicSetId,
-                                    ids.active_set_id,
-                                ));
+            if rb.is_enabled() {
+                // The body's status changed. We need to make sure
+                // it is on the correct active set.
+                if let Some(islands) = islands.as_deref_mut() {
+                    if changes.contains(RigidBodyChanges::TYPE) {
+                        match rb.body_type {
+                            RigidBodyType::Dynamic => {
+                                // Remove from the active kinematic set if it was there.
+                                if islands.active_kinematic_set.get(ids.active_set_id)
+                                    == Some(handle)
+                                {
+                                    islands.active_kinematic_set.swap_remove(ids.active_set_id);
+                                    final_action = Some(FinalAction::UpdateActiveKinematicSetId(
+                                        ids.active_set_id,
+                                    ));
+                                }
+
+                                // Add to the active dynamic set.
+                                activation.wake_up(true);
+                                // Make sure the sleep change flag is set (even if for some
+                                // reasons the rigid-body was already awake) to make
+                                // sure the code handling sleeping change adds the body to
+                                // the active_dynamic_set.
+                                changes.set(RigidBodyChanges::SLEEP, true);
                             }
-
-                            // Add to the active kinematic set.
-                            if islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) {
-                                ids.active_set_id = islands.active_kinematic_set.len();
-                                islands.active_kinematic_set.push(*handle);
+                            RigidBodyType::KinematicVelocityBased
+                            | RigidBodyType::KinematicPositionBased => {
+                                // Remove from the active dynamic set if it was there.
+                                if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle)
+                                {
+                                    islands.active_dynamic_set.swap_remove(ids.active_set_id);
+                                    final_action = Some(FinalAction::UpdateActiveDynamicSetId(
+                                        ids.active_set_id,
+                                    ));
+                                }
+
+                                // Add to the active kinematic set.
+                                if islands.active_kinematic_set.get(ids.active_set_id)
+                                    != Some(handle)
+                                {
+                                    ids.active_set_id = islands.active_kinematic_set.len();
+                                    islands.active_kinematic_set.push(*handle);
+                                }
                             }
+                            RigidBodyType::Fixed => {}
                         }
-                        RigidBodyType::Fixed => {}
                     }
-                }
 
-                // Update the positions of the colliders.
-                if changes.contains(RigidBodyChanges::POSITION)
-                    || changes.contains(RigidBodyChanges::COLLIDERS)
-                {
-                    rb.colliders
-                        .update_positions(colliders, modified_colliders, &rb.pos.position);
+                    // Update the active kinematic set.
+                    if changes.contains(RigidBodyChanges::POSITION)
+                        || changes.contains(RigidBodyChanges::COLLIDERS)
+                    {
+                        if rb.is_kinematic()
+                            && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle)
+                        {
+                            ids.active_set_id = islands.active_kinematic_set.len();
+                            islands.active_kinematic_set.push(*handle);
+                        }
+                    }
 
-                    if rb.is_kinematic()
-                        && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle)
+                    // Push the body to the active set if it is not
+                    // sleeping and if it is not already inside of the active set.
+                    if changes.contains(RigidBodyChanges::SLEEP)
+                        && rb.is_enabled()
+                        && !activation.sleeping // May happen if the body was put to sleep manually.
+                        && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+                        && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
                     {
-                        ids.active_set_id = islands.active_kinematic_set.len();
-                        islands.active_kinematic_set.push(*handle);
+                        ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+                        islands.active_dynamic_set.push(*handle);
                     }
                 }
+            }
 
-                // Push the body to the active set if it is not
-                // sleeping and if it is not already inside of the active set.
-                if changes.contains(RigidBodyChanges::SLEEP)
-                    && !activation.sleeping // May happen if the body was put to sleep manually.
-                    && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
-                    && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
-                {
-                    ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
-                    islands.active_dynamic_set.push(*handle);
-                }
-            } else {
-                // We don't use islands. So just update the colliders' positions.
-                if changes.contains(RigidBodyChanges::POSITION)
-                    || changes.contains(RigidBodyChanges::COLLIDERS)
-                {
-                    rb.colliders
-                        .update_positions(colliders, modified_colliders, &rb.pos.position);
-                }
+            // Update the colliders' positions.
+            if changes.contains(RigidBodyChanges::POSITION)
+                || changes.contains(RigidBodyChanges::COLLIDERS)
+            {
+                rb.colliders
+                    .update_positions(colliders, modified_colliders, &rb.pos.position);
             }
 
             if changes.contains(RigidBodyChanges::DOMINANCE)
                 || changes.contains(RigidBodyChanges::TYPE)
             {
                 for handle in rb.colliders.0.iter() {
+                    // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking`
+                    // here because that would modify the `modified_colliders` inside of the `ColliderSet`
+                    // instead of the one passed to this method.
                     let co = colliders.index_mut_internal(*handle);
                     if !co.changes.contains(ColliderChanges::MODIFIED) {
                         modified_colliders.push(*handle);
@@ -165,6 +179,43 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                 );
             }
 
+            if changes.contains(RigidBodyChanges::ENABLED_OR_DISABLED) {
+                // Propagate the rigid-body’s enabled/disable status to its colliders.
+                for handle in rb.colliders.0.iter() {
+                    // NOTE: we can’t just use `colliders.get_mut_internal_with_modification_tracking`
+                    // here because that would modify the `modified_colliders` inside of the `ColliderSet`
+                    // instead of the one passed to this method.
+                    let co = colliders.index_mut_internal(*handle);
+                    if !co.changes.contains(ColliderChanges::MODIFIED) {
+                        modified_colliders.push(*handle);
+                    }
+
+                    if rb.enabled && co.flags.enabled == ColliderEnabled::DisabledByParent {
+                        co.flags.enabled = ColliderEnabled::Enabled;
+                    } else if !rb.enabled && co.flags.enabled == ColliderEnabled::Enabled {
+                        co.flags.enabled = ColliderEnabled::DisabledByParent;
+                    }
+
+                    co.changes |= ColliderChanges::MODIFIED | ColliderChanges::ENABLED_OR_DISABLED;
+                }
+
+                // Propagate the rigid-body’s enabled/disable status to its attached impulse joints.
+                impulse_joints.map_attached_joints_mut(*handle, |_, _, _, joint| {
+                    if rb.enabled && joint.data.enabled == JointEnabled::DisabledByAttachedBody {
+                        joint.data.enabled = JointEnabled::Enabled;
+                    } else if !rb.enabled && joint.data.enabled == JointEnabled::Enabled {
+                        joint.data.enabled = JointEnabled::DisabledByAttachedBody;
+                    }
+                });
+
+                // FIXME: Propagate the rigid-body’s enabled/disable status to its attached multibody joints.
+
+                // Remove the rigid-body from the island manager.
+                if !rb.enabled {
+                    final_action = Some(FinalAction::RemoveFromIsland);
+                }
+            }
+
             rb.changes = RigidBodyChanges::empty();
             rb.ids = ids;
             rb.activation = activation;
@@ -172,15 +223,25 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
 
         // Adjust some ids, if needed.
         if let Some(islands) = islands.as_deref_mut() {
-            if let Some((action, id)) = final_action {
-                let active_set = match action {
-                    FinalAction::UpdateActiveKinematicSetId => &mut islands.active_kinematic_set,
-                    FinalAction::UpdateActiveDynamicSetId => &mut islands.active_dynamic_set,
+            if let Some(action) = final_action {
+                match action {
+                    FinalAction::RemoveFromIsland => {
+                        let ids = rb.ids;
+                        islands.rigid_body_removed(*handle, &ids, bodies);
+                    }
+                    FinalAction::UpdateActiveKinematicSetId(id) => {
+                        let active_set = &mut islands.active_kinematic_set;
+                        if id < active_set.len() {
+                            bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
+                        }
+                    }
+                    FinalAction::UpdateActiveDynamicSetId(id) => {
+                        let active_set = &mut islands.active_dynamic_set;
+                        if id < active_set.len() {
+                            bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
+                        }
+                    }
                 };
-
-                if id < active_set.len() {
-                    bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
-                }
             }
         }
     }