Crash when removing multibody

src/dynamics/joint/multibody_joint/mod.rs

@@ -17,3 +17,102 @@ mod multibody_workspace;
 mod multibody_ik;
 mod multibody_joint;
 mod unit_multibody_joint;
+
+#[cfg(test)]
+mod test {
+    use crate::prelude::{
+        BroadPhaseMultiSap, CCDSolver, ColliderSet, ImpulseJointSet, IntegrationParameters,
+        IslandManager, NarrowPhase, PhysicsPipeline, RigidBodySet,
+    };
+    use crate::prelude::{MultibodyJointSet, RevoluteJoint};
+    use parry::math::Vector;
+
+    #[test]
+    fn multibody_joint_remove_and_step() {
+        let mut bodies = RigidBodySet::new();
+        let mut multibody_joints = MultibodyJointSet::new();
+        let mut colliders = ColliderSet::new();
+        let mut impulse_joints = ImpulseJointSet::new();
+        let mut islands = IslandManager::new();
+
+        let mut pipeline = PhysicsPipeline::new();
+        let mut bf = BroadPhaseMultiSap::new();
+        let mut nf = NarrowPhase::new();
+
+        let num_links = 2;
+        let mut handles = vec![];
+
+        for _ in 0..num_links {
+            use crate::prelude::RigidBodyBuilder;
+
+            handles.push(bodies.insert(RigidBodyBuilder::dynamic()));
+        }
+
+        #[cfg(feature = "dim2")]
+        let joint = RevoluteJoint::new();
+        #[cfg(feature = "dim3")]
+        let joint = RevoluteJoint::new(na::Vector::x_axis());
+
+        for i in 0..num_links - 1 {
+            multibody_joints
+                .insert(handles[i], handles[i + 1], joint, true)
+                .unwrap();
+        }
+        pipeline.step(
+            &Vector::zeros(),
+            &IntegrationParameters::default(),
+            &mut islands,
+            &mut bf,
+            &mut nf,
+            &mut bodies,
+            &mut colliders,
+            &mut impulse_joints,
+            &mut multibody_joints,
+            &mut CCDSolver::new(),
+            None,
+            &(),
+            &(),
+        );
+
+        for handle in handles.clone().iter() {
+            bodies.remove(
+                *handle,
+                &mut islands,
+                &mut colliders,
+                &mut impulse_joints,
+                &mut multibody_joints,
+                true,
+            );
+            // Remove from our handles the one we just removed.
+            handles.retain(|value| value != handle);
+            // All handles left should be correctly set up.
+            for handle in handles.iter() {
+                // Note debug #847: rigid_body_link should return none when attempting to remove the last (second) body.
+                if let Some(link) = multibody_joints.rigid_body_link(*handle).copied() {
+                    if multibody_joints
+                        .get_multibody_mut_internal(link.multibody)
+                        .is_none()
+                    {
+                        panic!("multibody_joints should be able to get all links returned from rigid_body_link.");
+                    }
+                    panic!("This test has only 1 link, it should lead to rigid_body_link returning `None` immediately after first removal.");
+                }
+            }
+            pipeline.step(
+                &Vector::zeros(),
+                &IntegrationParameters::default(),
+                &mut islands,
+                &mut bf,
+                &mut nf,
+                &mut bodies,
+                &mut colliders,
+                &mut impulse_joints,
+                &mut multibody_joints,
+                &mut CCDSolver::new(),
+                None,
+                &(),
+                &(),
+            );
+        }
+    }
+}

src/dynamics/joint/multibody_joint/multibody_joint_set.rs

@@ -241,11 +241,17 @@ impl MultibodyJointSet {
                     if multibody.num_links() == 1 {
                         // We don’t have any multibody_joint attached to this body, remove it.
                         let isolated_link = multibody.link(0).unwrap();
-                        let isolated_graph_id =
-                            self.rb2mb.get(isolated_link.rigid_body.0).unwrap().graph_id;
-                        if let Some(other) = self.connectivity_graph.remove_node(isolated_graph_id)
+                        let isolated_graph_id = self
+                            .rb2mb
+                            .remove(isolated_link.rigid_body.0, Default::default())
+                            .unwrap();
+
+                        if let Some(other) = self
+                            .connectivity_graph
+                            .remove_node(isolated_graph_id.graph_id)
                         {
-                            self.rb2mb.get_mut(other.0).unwrap().graph_id = isolated_graph_id;
+                            self.rb2mb.get_mut(other.0).unwrap().graph_id =
+                                isolated_graph_id.graph_id;
                         }
                     } else {
                         let mb_id = self.multibodies.insert(multibody);
@@ -308,7 +314,7 @@ impl MultibodyJointSet {
 
     /// Returns the link of this multibody attached to the given rigid-body.
     ///
-    /// Returns `None` if `rb` isn’t part of any rigid-body.
+    /// Returns `None` if `rb` isn’t part of any multibody.
     pub fn rigid_body_link(&self, rb: RigidBodyHandle) -> Option<&MultibodyLinkId> {
         self.rb2mb.get(rb.0)
     }

src_testbed/testbed.rs

@@ -848,7 +848,7 @@ impl Testbed<'_, '_, '_, '_, '_, '_, '_, '_, '_, '_, '_> {
                         .collect();
                     colliders.sort_by_key(|co| -(co.len() as isize));
 
-                    let num_to_delete = (colliders.len() / 10).max(0);
+                    let num_to_delete = (colliders.len() / 10).clamp(1, colliders.len());
                     for to_delete in &colliders[..num_to_delete] {
                         if let Some(graphics) = self.graphics.as_mut() {
                             graphics.remove_collider(to_delete[0], &self.harness.physics.colliders);
@@ -871,7 +871,7 @@ impl Testbed<'_, '_, '_, '_, '_, '_, '_, '_, '_, '_, '_> {
                         .filter(|e| e.1.is_dynamic())
                         .map(|e| e.0)
                         .collect();
-                    let num_to_delete = (dynamic_bodies.len() / 10).max(0);
+                    let num_to_delete = (dynamic_bodies.len() / 10).clamp(1, dynamic_bodies.len());
                     for to_delete in &dynamic_bodies[..num_to_delete] {
                         if let Some(graphics) = self.graphics.as_mut() {
                             graphics.remove_body(*to_delete);
@@ -895,7 +895,8 @@ impl Testbed<'_, '_, '_, '_, '_, '_, '_, '_, '_, '_, '_> {
                         .iter()
                         .map(|e| e.0)
                         .collect();
-                    let num_to_delete = (impulse_joints.len() / 10).max(0);
+                    let num_to_delete =
+                        (impulse_joints.len() / 10).max(1).min(impulse_joints.len());
                     for to_delete in &impulse_joints[..num_to_delete] {
                         self.harness.physics.impulse_joints.remove(*to_delete, true);
                     }
@@ -909,7 +910,8 @@ impl Testbed<'_, '_, '_, '_, '_, '_, '_, '_, '_, '_, '_> {
                         .iter()
                         .map(|e| e.0)
                         .collect();
-                    let num_to_delete = (multibody_joints.len() / 10).max(0);
+                    let num_to_delete =
+                        (multibody_joints.len() / 10).clamp(1, multibody_joints.len());
                     for to_delete in &multibody_joints[..num_to_delete] {
                         self.harness
                             .physics