Merge pull request #427 from dimforge/disable

Add enable/disable, incremental query pipeline, and vehicle character contoller

4 files changed, 253 insertions, 177 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index ac92a04..b086fbe 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -5,7 +5,7 @@ use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase,
 };
 use crate::math::Real;
-use crate::pipeline::{EventHandler, PhysicsHooks};
+use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
 
 /// The collision pipeline, responsible for performing collision detection between colliders.
@@ -111,6 +111,7 @@ impl CollisionPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
+        query_pipeline: Option<&mut QueryPipeline>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -127,9 +128,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,
@@ -143,6 +155,10 @@ impl CollisionPipeline {
             true,
         );
 
+        if let Some(queries) = query_pipeline {
+            queries.update_incremental(colliders, &modified_colliders, &removed_colliders, true);
+        }
+
         self.clear_modified_colliders(colliders, &mut modified_colliders);
         removed_colliders.clear();
     }
@@ -187,6 +203,7 @@ mod tests {
             &mut narrow_phase,
             &mut rigid_body_set,
             &mut collider_set,
+            None,
             &physics_hooks,
             &(),
         );
@@ -238,6 +255,7 @@ mod tests {
             &mut narrow_phase,
             &mut rigid_body_set,
             &mut collider_set,
+            None,
             &physics_hooks,
             &(),
         );
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 391b39a..358efed 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -5,7 +5,7 @@ use crate::counters::Counters;
 use crate::dynamics::IslandSolver;
 use crate::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
-    RigidBodyPosition, RigidBodyType,
+    RigidBodyChanges, RigidBodyHandle, RigidBodyPosition, RigidBodyType,
 };
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
@@ -14,7 +14,7 @@ use crate::geometry::{
     ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex,
 };
 use crate::math::{Real, Vector};
-use crate::pipeline::{EventHandler, PhysicsHooks};
+use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
 use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet};
 
 /// The physics pipeline, responsible for stepping the whole physics simulation.
@@ -77,6 +77,18 @@ impl PhysicsPipeline {
         }
     }
 
+    fn clear_modified_bodies(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        modified_bodies: &mut Vec<RigidBodyHandle>,
+    ) {
+        for handle in modified_bodies.drain(..) {
+            if let Some(rb) = bodies.get_mut_internal(handle) {
+                rb.changes = RigidBodyChanges::empty();
+            }
+        }
+    }
+
     fn detect_collisions(
         &mut self,
         integration_parameters: &IntegrationParameters,
@@ -404,6 +416,7 @@ impl PhysicsPipeline {
         impulse_joints: &mut ImpulseJointSet,
         multibody_joints: &mut MultibodyJointSet,
         ccd_solver: &mut CCDSolver,
+        mut query_pipeline: Option<&mut QueryPipeline>,
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
@@ -422,21 +435,34 @@ 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,
             &modified_colliders[..],
         );
 
-        let modified_bodies = bodies.take_modified();
+        let mut modified_bodies = bodies.take_modified();
         super::user_changes::handle_user_changes_to_rigid_bodies(
             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 {
@@ -455,14 +481,19 @@ impl PhysicsPipeline {
             colliders,
             impulse_joints,
             multibody_joints,
-            &modified_colliders[..],
-            &mut removed_colliders,
+            &modified_colliders,
+            &removed_colliders,
             hooks,
             events,
             true,
         );
 
+        if let Some(queries) = query_pipeline.as_deref_mut() {
+            queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false);
+        }
+
         self.clear_modified_colliders(colliders, &mut modified_colliders);
+        self.clear_modified_bodies(bodies, &mut modified_bodies);
         removed_colliders.clear();
 
         let mut remaining_time = integration_parameters.dt;
@@ -582,13 +613,22 @@ impl PhysicsPipeline {
                 colliders,
                 impulse_joints,
                 multibody_joints,
-                &mut modified_colliders,
-                &mut removed_colliders,
+                &modified_colliders,
+                &[],
                 hooks,
                 events,
                 false,
             );
 
+            if let Some(queries) = query_pipeline.as_deref_mut() {
+                queries.update_incremental(
+                    colliders,
+                    &modified_colliders,
+                    &[],
+                    remaining_substeps == 0,
+                );
+            }
+
             self.clear_modified_colliders(colliders, &mut modified_colliders);
         }
 
@@ -650,6 +690,7 @@ mod test {
             &mut impulse_joints,
             &mut multibody_joints,
             &mut CCDSolver::new(),
+            None,
             &(),
             &(),
         );
@@ -705,6 +746,7 @@ mod test {
             &mut impulse_joints,
             &mut multibody_joints,
             &mut CCDSolver::new(),
+            None,
             &(),
             &(),
         );
@@ -807,6 +849,7 @@ mod test {
                 &mut impulse_joints,
                 &mut multibody_joints,
                 &mut ccd,
+                None,
                 &physics_hooks,
                 &event_handler,
             );
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 00825e2..a248426 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,10 +1,10 @@
-use crate::dynamics::{IslandManager, RigidBodyHandle};
+use crate::dynamics::RigidBodyHandle;
 use crate::geometry::{
     Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection,
 };
 use crate::math::{Isometry, Point, Real, Vector};
 use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
-use parry::partitioning::QbvhDataGenerator;
+use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace};
 use parry::query::details::{
     IntersectionCompositeShapeShapeBestFirstVisitor,
     NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -30,8 +30,9 @@ pub struct QueryPipeline {
     )]
     query_dispatcher: Arc<dyn QueryDispatcher>,
     qbvh: Qbvh<ColliderHandle>,
-    tree_built: bool,
     dilation_factor: Real,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    workspace: QbvhUpdateWorkspace,
 }
 
 struct QueryPipelineAsCompositeShape<'a> {
@@ -310,8 +311,8 @@ impl QueryPipeline {
         Self {
             query_dispatcher: Arc::new(d),
             qbvh: Qbvh::new(),
-            tree_built: false,
             dilation_factor: 0.01,
+            workspace: QbvhUpdateWorkspace::default(),
         }
     }
 
@@ -320,25 +321,46 @@ impl QueryPipeline {
         &*self.query_dispatcher
     }
 
-    /// Update the acceleration structure on the query pipeline.
-    pub fn update(
+    /// Update the query pipeline incrementally, avoiding a complete rebuild of its
+    /// internal data-structure.
+    pub fn update_incremental(
         &mut self,
-        islands: &IslandManager,
-        bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        refit_and_rebalance: bool,
     ) {
-        self.update_with_mode(
-            islands,
-            bodies,
-            colliders,
-            QueryPipelineMode::CurrentPosition,
-        )
+        // We remove first. This is needed to avoid the ABA problem: if a collider was removed
+        // and another added right after with the same handle index, we can remove first, and
+        // then update the new one (but only if its actually exists, to address the case where
+        // a collider was added/modified and then removed during the same frame).
+        for removed in removed_colliders {
+            self.qbvh.remove(*removed);
+        }
+
+        for modified in modified_colliders {
+            // Check that the collider still exists as it may have been removed.
+            if colliders.contains(*modified) {
+                self.qbvh.pre_update_or_insert(*modified);
+            }
+        }
+
+        if refit_and_rebalance {
+            let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| {
+                colliders[*handle].compute_aabb()
+            });
+            self.qbvh.rebalance(0.0, &mut self.workspace);
+        }
+    }
+
+    /// Update the acceleration structure on the query pipeline.
+    pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
+        self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition)
     }
 
     /// Update the acceleration structure on the query pipeline.
     pub fn update_with_mode(
         &mut self,
-        islands: &IslandManager,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
         mode: QueryPipelineMode,
@@ -358,12 +380,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 +396,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(
@@ -392,71 +414,12 @@ impl QueryPipeline {
             }
         }
 
-        if !self.tree_built {
-            let generator = DataGenerator {
-                bodies,
-                colliders,
-                mode,
-            };
-            self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
-
-            // FIXME: uncomment this once we handle insertion/removals properly.
-            // self.tree_built = true;
-            return;
-        }
-
-        for handle in islands.iter_active_bodies() {
-            let rb = &bodies[handle];
-            for handle in &rb.colliders.0 {
-                self.qbvh.pre_update(*handle)
-            }
-        }
-
-        match mode {
-            QueryPipelineMode::CurrentPosition => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        co.shape.compute_aabb(&co.pos)
-                    },
-                    self.dilation_factor,
-                );
-            }
-            QueryPipelineMode::SweepTestWithNextPosition => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        if let Some(parent) = &co.parent {
-                            let rb_next_pos = &bodies[parent.handle].pos.next_position;
-                            let next_position = rb_next_pos * parent.pos_wrt_parent;
-                            co.shape.compute_swept_aabb(&co.pos, &next_position)
-                        } else {
-                            co.shape.compute_aabb(&co.pos)
-                        }
-                    },
-                    self.dilation_factor,
-                );
-            }
-            QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
-                self.qbvh.update(
-                    |handle| {
-                        let co = &colliders[*handle];
-                        if let Some(parent) = co.parent {
-                            let rb = &bodies[parent.handle];
-                            let predicted_pos = rb.pos.integrate_forces_and_velocities(
-                                dt, &rb.forces, &rb.vels, &rb.mprops,
-                            );
-
-                            let next_position = predicted_pos * parent.pos_wrt_parent;
-                            co.shape.compute_swept_aabb(&co.pos, &next_position)
-                        } else {
-                            co.shape.compute_aabb(&co.pos)
-                        }
-                    },
-                    self.dilation_factor,
-                );
-            }
-        }
+        let generator = DataGenerator {
+            bodies,
+            colliders,
+            mode,
+        };
+        self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
     }
 
     /// Find the closest intersection between a ray and a set of collider.
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index e7da68a..eeda97a 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, // FIXME: propagate disabled state to multibodies
     modified_bodies: &[RigidBodyHandle],
     modified_colliders: &mut Vec<ColliderHandle>,
 ) {
     enum FinalAction {
-        UpdateActiveKinematicSetId,
-        UpdateActiveDynamicSetId,
+        UpdateActiveKinematicSetId(usize),
+        UpdateActiveDynamicSetId(usize),
+        RemoveFromIsland,
     }
 
     for handle in modified_bodies {
@@ -57,94 +65,92 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
         }
 
         let rb = bodies.index_mut_internal(*handle);
-        let mut changes = rb.changes;
         let mut ids = rb.ids;
-        let mut activation = rb.activation;
+        let changes = rb.changes;
+        let 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 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()
+                        && !rb.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,22 +171,68 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                 );
             }
 
-            rb.changes = RigidBodyChanges::empty();
+            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.ids = ids;
             rb.activation = activation;
         }
 
         // 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;
-                }
             }
         }
     }