Fix crash caused by the removal of a kinematic body.

3 files changed, 87 insertions, 30 deletions

diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs
index 3836baf..bb71b76 100644
--- a/examples3d/platform3.rs
+++ b/examples3d/platform3.rs
@@ -65,21 +65,22 @@ pub fn init_world(testbed: &mut Testbed) {
      * Setup a callback to control the platform.
      */
     testbed.add_callback(move |_, physics, _, _, time| {
-        let mut platform = physics.bodies.get_mut(platform_handle).unwrap();
-        let mut next_pos = platform.position;
+        if let Some(mut platform) = physics.bodies.get_mut(platform_handle) {
+            let mut next_pos = platform.position;
 
-        let dt = 0.016;
-        next_pos.translation.vector.y += (time * 5.0).sin() * dt;
-        next_pos.translation.vector.z += time.sin() * 5.0 * dt;
+            let dt = 0.016;
+            next_pos.translation.vector.y += (time * 5.0).sin() * dt;
+            next_pos.translation.vector.z += time.sin() * 5.0 * dt;
 
-        if next_pos.translation.vector.z >= rad * 10.0 {
-            next_pos.translation.vector.z -= dt;
-        }
-        if next_pos.translation.vector.z <= -rad * 10.0 {
-            next_pos.translation.vector.z += dt;
-        }
+            if next_pos.translation.vector.z >= rad * 10.0 {
+                next_pos.translation.vector.z -= dt;
+            }
+            if next_pos.translation.vector.z <= -rad * 10.0 {
+                next_pos.translation.vector.z += dt;
+            }
 
-        platform.set_next_kinematic_position(next_pos);
+            platform.set_next_kinematic_position(next_pos);
+        }
     });
 
     /*
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 99d6f10..14a6471 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -2,7 +2,7 @@
 use rayon::prelude::*;
 
 use crate::data::arena::Arena;
-use crate::dynamics::{Joint, RigidBody};
+use crate::dynamics::{BodyStatus, Joint, RigidBody};
 use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
 use crossbeam::channel::{Receiver, Sender};
 use std::ops::{Deref, DerefMut, Index, IndexMut};
@@ -128,9 +128,23 @@ impl RigidBodySet {
 
     pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
         let mut rb = &mut self.bodies[handle];
-        if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
-            rb.active_set_id = self.active_dynamic_set.len();
-            self.active_dynamic_set.push(handle);
+        match rb.body_status {
+            // XXX: this case should only concern the dynamic bodies.
+            // For static bodies we should use the modified_inactive_set, or something
+            // similar. Right now we do this for static bodies as well so the broad-phase
+            // takes them into account the first time they are inserted.
+            BodyStatus::Dynamic | BodyStatus::Static => {
+                if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
+                    rb.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
+            }
+            BodyStatus::Kinematic => {
+                if self.active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
+                    rb.active_set_id = self.active_kinematic_set.len();
+                    self.active_kinematic_set.push(handle);
+                }
+            }
         }
     }
 
@@ -143,13 +157,14 @@ impl RigidBodySet {
     pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
         let handle = self.bodies.insert(rb);
         let rb = &mut self.bodies[handle];
-        rb.active_set_id = self.active_dynamic_set.len();
 
         if !rb.is_sleeping() && rb.is_dynamic() {
+            rb.active_set_id = self.active_dynamic_set.len();
             self.active_dynamic_set.push(handle);
         }
 
         if rb.is_kinematic() {
+            rb.active_set_id = self.active_kinematic_set.len();
             self.active_kinematic_set.push(handle);
         }
 
@@ -166,12 +181,15 @@ impl RigidBodySet {
 
     pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option<RigidBody> {
         let rb = self.bodies.remove(handle)?;
-        // Remove this body from the active dynamic set.
-        if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
-            self.active_dynamic_set.swap_remove(rb.active_set_id);
+        let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
+
+        for active_set in &mut active_sets {
+            if active_set.get(rb.active_set_id) == Some(&handle) {
+                active_set.swap_remove(rb.active_set_id);
 
-            if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) {
-                self.bodies[*replacement].active_set_id = rb.active_set_id;
+                if let Some(replacement) = active_set.get(rb.active_set_id) {
+                    self.bodies[*replacement].active_set_id = rb.active_set_id;
+                }
             }
         }
 
@@ -181,6 +199,7 @@ impl RigidBodySet {
     /// Forces the specified rigid-body to wake up if it is dynamic.
     pub fn wake_up(&mut self, handle: RigidBodyHandle) {
         if let Some(rb) = self.bodies.get_mut(handle) {
+            // TODO: what about kinematic bodies?
             if rb.is_dynamic() {
                 rb.wake_up();
 
@@ -214,8 +233,11 @@ impl RigidBodySet {
     ///
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
-    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
-        self.bodies.get_unknown_gen_mut(i)
+    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(RigidBodyMut, RigidBodyHandle)> {
+        let sender = &self.activation_channel.0;
+        self.bodies
+            .get_unknown_gen_mut(i)
+            .map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle))
     }
 
     /// Gets the rigid-body with the given handle.
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 8449477..192ca9a 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -298,8 +298,9 @@ impl PhysicsPipeline {
 
 #[cfg(test)]
 mod test {
-    use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-    use crate::geometry::{BroadPhase, ColliderSet, NarrowPhase};
+    use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet};
+    use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
+    use crate::math::Vector;
     use crate::pipeline::PhysicsPipeline;
 
     #[test]
@@ -310,12 +311,45 @@ mod test {
         let mut bf = BroadPhase::new();
         let mut nf = NarrowPhase::new();
 
-        let mut set = RigidBodySet::new();
-        let rb = RigidBodyBuilder::new_dynamic().build();
+        let mut bodies = RigidBodySet::new();
 
         // Check that removing the body right after inserting it works.
-        let h1 = set.insert(rb.clone());
-        pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints);
+        // We add two dynamic bodies, one kinematic body and one static body before removing
+        // them. This include a non-regression test where deleting a kimenatic body crashes.
+        let rb = RigidBodyBuilder::new_dynamic().build();
+        let h1 = bodies.insert(rb.clone());
+        let h2 = bodies.insert(rb.clone());
+
+        // The same but with a kinematic body.
+        let rb = RigidBodyBuilder::new_kinematic().build();
+        let h3 = bodies.insert(rb.clone());
+
+        // The same but with a static body.
+        let rb = RigidBodyBuilder::new_static().build();
+        let h4 = bodies.insert(rb.clone());
+
+        let to_delete = [h1, h2, h3, h4];
+        for h in &to_delete {
+            pipeline.remove_rigid_body(
+                *h,
+                &mut bf,
+                &mut nf,
+                &mut bodies,
+                &mut colliders,
+                &mut joints,
+            );
+        }
+
+        pipeline.step(
+            &Vector::zeros(),
+            &IntegrationParameters::default(),
+            &mut bf,
+            &mut nf,
+            &mut bodies,
+            &mut colliders,
+            &mut joints,
+            &(),
+        );
     }
 
     #[test]