Allow disabling colliders, rigid-bodies and impulse joints

1 files changed, 144 insertions, 83 deletions

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