diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index dc1fa758c..50492f100 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/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, + &(), + &(), + ); + } + } +} diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index fefa92749..3dd3e364c 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/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) } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 321510a8a..7035f9990 100644 --- a/src_testbed/testbed.rs +++ b/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