From 668fb660c86a81848eaace98f184a7d169b55b71 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 25 Jun 2025 10:39:38 +0200 Subject: [PATCH 1/4] add failing test for removing multibody joints + stepping This currently crashes when removing last multibody joint, it seems that a link in MultibodyJointSet::multibodies should be removed --- src/dynamics/joint/multibody_joint/mod.rs | 103 ++++++++++++++++++++++ 1 file changed, 103 insertions(+) diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index dc1fa758c..686a2f2fd 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -17,3 +17,106 @@ 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 rnd = oorandom::Rand32::new(1234); + + for k in 0..10 { + 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 = 100; + 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, + &(), + &(), + ); + match k { + 0 => {} // Remove in insertion order. + 1 => { + // Remove from leaf to root. + handles.reverse(); + } + _ => { + // Shuffle the vector a bit. + // (This test checks multiple shuffle arrangements due to k > 2). + for l in 0..num_links { + handles.swap(l, rnd.rand_range(0..num_links as u32) as usize); + } + } + } + + for (i, handle) in handles.iter().enumerate() { + dbg!(i); + bodies.remove( + *handle, + &mut islands, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + true, + ); + 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, + &(), + &(), + ); + } + } + } +} From 941d2a5954ffd3b4c1819ab345888bab64e1987b Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Wed, 25 Jun 2025 11:49:03 +0200 Subject: [PATCH 2/4] more test info --- src/dynamics/joint/multibody_joint/mod.rs | 21 ++++++++++++++++--- .../multibody_joint/multibody_joint_set.rs | 2 +- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index 686a2f2fd..e1f4c3a36 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -42,7 +42,7 @@ mod test { let mut bf = BroadPhaseMultiSap::new(); let mut nf = NarrowPhase::new(); - let num_links = 100; + let num_links = 2; let mut handles = vec![]; for _ in 0..num_links { @@ -91,8 +91,8 @@ mod test { } } - for (i, handle) in handles.iter().enumerate() { - dbg!(i); + for (i, handle) in handles.clone().iter().enumerate() { + dbg!(i, handle); bodies.remove( *handle, &mut islands, @@ -101,6 +101,21 @@ mod test { &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() + { + dbg!(handle, link); + panic!("multibody_joints should be able to get all links returned from rigid_body_link."); + } + } + } pipeline.step( &Vector::zeros(), &IntegrationParameters::default(), diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index fefa92749..e14de245c 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -308,7 +308,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) } From d357433b2da9da12c48264ad3acd2fb6c9411b6d Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 3 Jul 2025 15:49:44 +0200 Subject: [PATCH 3/4] fix removing last multibody joint of a set + allow testbed to remove all joints (previously was keeping 1 --- src/dynamics/joint/multibody_joint/mod.rs | 5 ++--- .../joint/multibody_joint/multibody_joint_set.rs | 14 ++++++++++---- src_testbed/testbed.rs | 10 ++++++---- 3 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index e1f4c3a36..cefee635d 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -91,8 +91,7 @@ mod test { } } - for (i, handle) in handles.clone().iter().enumerate() { - dbg!(i, handle); + for handle in handles.clone().iter() { bodies.remove( *handle, &mut islands, @@ -111,9 +110,9 @@ mod test { .get_multibody_mut_internal(link.multibody) .is_none() { - dbg!(handle, link); 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( diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index e14de245c..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); 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 From 11904cc4b937619173adbfa14b9facdabaf62057 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Thu, 17 Jul 2025 10:33:11 +0200 Subject: [PATCH 4/4] simpler test because a vec of 1 doesn't need random ordering --- src/dynamics/joint/multibody_joint/mod.rs | 140 ++++++++++------------ 1 file changed, 61 insertions(+), 79 deletions(-) diff --git a/src/dynamics/joint/multibody_joint/mod.rs b/src/dynamics/joint/multibody_joint/mod.rs index cefee635d..50492f100 100644 --- a/src/dynamics/joint/multibody_joint/mod.rs +++ b/src/dynamics/joint/multibody_joint/mod.rs @@ -29,37 +29,74 @@ mod test { #[test] fn multibody_joint_remove_and_step() { - let mut rnd = oorandom::Rand32::new(1234); + 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(); - for k in 0..10 { - 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 mut pipeline = PhysicsPipeline::new(); - let mut bf = BroadPhaseMultiSap::new(); - let mut nf = NarrowPhase::new(); + let num_links = 2; + let mut handles = vec![]; - let num_links = 2; - let mut handles = vec![]; + for _ in 0..num_links { + use crate::prelude::RigidBodyBuilder; - for _ in 0..num_links { - use crate::prelude::RigidBodyBuilder; + handles.push(bodies.insert(RigidBodyBuilder::dynamic())); + } - handles.push(bodies.insert(RigidBodyBuilder::dynamic())); - } + #[cfg(feature = "dim2")] + let joint = RevoluteJoint::new(); + #[cfg(feature = "dim3")] + let joint = RevoluteJoint::new(na::Vector::x_axis()); - #[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 i in 0..num_links - 1 { - multibody_joints - .insert(handles[i], handles[i + 1], joint, true) - .unwrap(); + 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(), @@ -76,61 +113,6 @@ mod test { &(), &(), ); - match k { - 0 => {} // Remove in insertion order. - 1 => { - // Remove from leaf to root. - handles.reverse(); - } - _ => { - // Shuffle the vector a bit. - // (This test checks multiple shuffle arrangements due to k > 2). - for l in 0..num_links { - handles.swap(l, rnd.rand_range(0..num_links as u32) as usize); - } - } - } - - 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, - &(), - &(), - ); - } } } }