Properly take initial sleeping state set by the user when creating a rigid-body

5 files changed, 41 insertions, 21 deletions

diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index edd2579..e196503 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -1,6 +1,6 @@
 use crate::dynamics::{
-    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle,
-    RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
+    ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders,
+    RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::math::Real;
@@ -96,12 +96,18 @@ impl IslandManager {
         //       attempting to wake-up a rigid-body that has already been deleted.
         if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
             let rb = bodies.index_mut_internal(handle);
-            rb.activation.wake_up(strong);
 
-            if rb.is_enabled() && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle)
-            {
-                rb.ids.active_set_id = self.active_dynamic_set.len();
-                self.active_dynamic_set.push(handle);
+            // Check that the user didn’t change the sleeping state explicitly, in which
+            // case we don’t overwrite it.
+            if !rb.changes.contains(RigidBodyChanges::SLEEP) {
+                rb.activation.wake_up(strong);
+
+                if rb.is_enabled()
+                    && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle)
+                {
+                    rb.ids.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
             }
         }
     }
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 26be983..bb1e9bf 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -135,7 +135,7 @@ impl RigidBody {
     }
 
     /// Sets the type of this rigid-body.
-    pub fn set_body_type(&mut self, status: RigidBodyType) {
+    pub fn set_body_type(&mut self, status: RigidBodyType, wake_up: bool) {
         if status != self.body_type {
             self.changes.insert(RigidBodyChanges::TYPE);
             self.body_type = status;
@@ -143,6 +143,10 @@ impl RigidBody {
             if status == RigidBodyType::Fixed {
                 self.vels = RigidBodyVelocity::zero();
             }
+
+            if self.is_dynamic() && wake_up {
+                self.wake_up(true);
+            }
         }
     }
 
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index ecd1623..dacca9e 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -299,7 +299,13 @@ impl NarrowPhase {
             }
         }
 
-        self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events);
+        self.handle_user_changes_on_colliders(
+            islands,
+            modified_colliders,
+            colliders,
+            bodies,
+            events,
+        );
     }
 
     pub(crate) fn remove_collider(
@@ -393,7 +399,7 @@ impl NarrowPhase {
         }
     }
 
-    pub(crate) fn handle_modified_colliders(
+    pub(crate) fn handle_user_changes_on_colliders(
         &mut self,
         mut islands: Option<&mut IslandManager>,
         modified_colliders: &[ColliderHandle],
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 9ef4177..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};
@@ -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,
@@ -430,7 +442,7 @@ impl PhysicsPipeline {
             &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,
@@ -481,6 +493,7 @@ impl PhysicsPipeline {
         }
 
         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;
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index 00096de..0f4058c 100644
--- a/src/pipeline/user_changes.rs
+++ b/src/pipeline/user_changes.rs
@@ -86,14 +86,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                                         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 => {
@@ -216,7 +208,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                 }
             }
 
-            rb.changes = RigidBodyChanges::empty();
             rb.ids = ids;
             rb.activation = activation;
         }