From aca29243a86d704a6c66a2569a0c1695007fc5a8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 6 Jul 2024 15:24:26 +0200 Subject: [PATCH 1/4] Fix out-of-bounds crash when merging two multibodies --- .../joint/multibody_joint/multibody.rs | 32 +++++++++++++++++-- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 8cfe8e5c8..5546f9db3 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -202,7 +202,10 @@ impl Multibody { pub(crate) fn append(&mut self, mut rhs: Multibody, parent: usize, joint: MultibodyJoint) { let rhs_root_ndofs = rhs.links[0].joint.ndofs(); - let rhs_copy_shift = self.ndofs + rhs_root_ndofs; + // Values for rhs will be copied into the buffers of `self` starting at this index. + let rhs_copy_shift = self.ndofs + joint.ndofs(); + // Number of dofs to copy from rhs. The root’s dofs isn’t included because it will be + // replaced by `joint. let rhs_copy_ndofs = rhs.ndofs - rhs_root_ndofs; // Adjust the ids of all the rhs links except the first one. @@ -224,7 +227,7 @@ impl Multibody { rhs.links[0].parent_internal_id = parent; } - // Grow buffers and append data from rhs. + // Grow buffers then append data from rhs. self.grow_buffers(rhs_copy_ndofs + rhs.links[0].joint.ndofs(), rhs.links.len()); if rhs_copy_ndofs > 0 { @@ -1360,9 +1363,32 @@ impl IndexSequence { #[cfg(test)] mod test { use super::IndexSequence; - use crate::math::Real; + use crate::math::{Real, SPATIAL_DIM}; + use crate::prelude::{MultibodyJointSet, RevoluteJoint, RigidBodyBuilder, RigidBodySet}; use na::{DVector, RowDVector}; + #[test] + fn test_multibody_append() { + let mut bodies = RigidBodySet::new(); + let mut joints = MultibodyJointSet::new(); + + let a = bodies.insert(RigidBodyBuilder::dynamic()); + let b = bodies.insert(RigidBodyBuilder::dynamic()); + let c = bodies.insert(RigidBodyBuilder::dynamic()); + let d = bodies.insert(RigidBodyBuilder::dynamic()); + + #[cfg(feature = "dim2")] + let joint = RevoluteJoint::new(); + #[cfg(feature = "dim3")] + let joint = RevoluteJoint::new(na::Vector::x_axis()); + + let mb_handle = joints.insert(a, b, joint, true).unwrap(); + joints.insert(c, d, joint, true).unwrap(); + joints.insert(b, c, joint, true).unwrap(); + + assert_eq!(joints.get(mb_handle).unwrap().0.ndofs, SPATIAL_DIM + 3); + } + fn test_sequence() -> IndexSequence { let mut seq = IndexSequence::new(); seq.remove(2); From dd39333c140b8f3792520acc2bba0fbc732b6bdd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 6 Jul 2024 18:21:52 +0200 Subject: [PATCH 2/4] Fix broken multibody joint removal. --- crates/rapier2d-f64/Cargo.toml | 1 + crates/rapier2d/Cargo.toml | 1 + crates/rapier3d-f64/Cargo.toml | 1 + crates/rapier3d/Cargo.toml | 1 + .../joint/multibody_joint/multibody.rs | 72 ++++++++++++++++++- .../multibody_joint/multibody_joint_set.rs | 17 +++-- 6 files changed, 84 insertions(+), 9 deletions(-) diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index 587f41485..08f36fbc2 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -71,3 +71,4 @@ thiserror = "1" [dev-dependencies] bincode = "1" serde = { version = "1", features = ["derive"] } +oorandom = { version = "11", default-features = false } diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 84bed4f9b..aa7ae717b 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -71,3 +71,4 @@ thiserror = "1" [dev-dependencies] bincode = "1" serde = { version = "1", features = ["derive"] } +oorandom = { version = "11", default-features = false } \ No newline at end of file diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 9f6ef44c0..d8f6fe5f5 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -71,3 +71,4 @@ thiserror = "1" [dev-dependencies] bincode = "1" serde = { version = "1", features = ["derive"] } +oorandom = { version = "11", default-features = false } diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index 880c4968d..d8e61a1ec 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -71,3 +71,4 @@ thiserror = "1" [dev-dependencies] bincode = "1" serde = { version = "1", features = ["derive"] } +oorandom = { version = "11", default-features = false } diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 5546f9db3..6c8344ba2 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -145,6 +145,7 @@ impl Multibody { let mut link2mb = vec![usize::MAX; self.links.len()]; let mut link_id2new_id = vec![usize::MAX; self.links.len()]; + // Split multibody and update the set of links and ndofs. for (i, mut link) in self.links.0.into_iter().enumerate() { let is_new_root = i == 0 || !joint_only && link.parent_internal_id == to_remove @@ -192,7 +193,14 @@ impl Multibody { link.internal_id = i; link.assembly_id = assembly_id; - link.parent_internal_id = link_id2new_id[link.parent_internal_id]; + + // NOTE: for the root, the current`link.parent_internal_id` is invalid since that + // parent lies in a different multibody now. + link.parent_internal_id = if i != 0 { + link_id2new_id[link.parent_internal_id] + } else { + 0 + }; assembly_id += link_ndofs; } } @@ -1363,8 +1371,11 @@ impl IndexSequence { #[cfg(test)] mod test { use super::IndexSequence; + use crate::dynamics::{ImpulseJointSet, IslandManager}; use crate::math::{Real, SPATIAL_DIM}; - use crate::prelude::{MultibodyJointSet, RevoluteJoint, RigidBodyBuilder, RigidBodySet}; + use crate::prelude::{ + ColliderSet, MultibodyJointSet, RevoluteJoint, RigidBodyBuilder, RigidBodySet, + }; use na::{DVector, RowDVector}; #[test] @@ -1389,6 +1400,63 @@ mod test { assert_eq!(joints.get(mb_handle).unwrap().0.ndofs, SPATIAL_DIM + 3); } + #[test] + fn test_multibody_remove() { + 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 num_links = 100; + let mut handles = vec![]; + + for _ in 0..num_links { + 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(); + } + + 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 { + bodies.remove( + handle, + &mut islands, + &mut colliders, + &mut impulse_joints, + &mut multibody_joints, + true, + ); + } + } + } + fn test_sequence() -> IndexSequence { let mut seq = IndexSequence::new(); seq.remove(2); diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 6ffdece00..67df15931 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -209,7 +209,7 @@ impl MultibodyJointSet { Some(MultibodyJointHandle(body2.0)) } - /// Removes an multibody_joint from this set. + /// Removes a multibody_joint from this set. pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) { if let Some(removed) = self.rb2mb.get(handle.0).copied() { let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); @@ -217,10 +217,9 @@ impl MultibodyJointSet { // Remove the edge from the connectivity graph. if let Some(parent_link) = multibody.link(removed.id).unwrap().parent_id() { let parent_rb = multibody.link(parent_link).unwrap().rigid_body; - self.connectivity_graph.remove_edge( - self.rb2mb.get(parent_rb.0).unwrap().graph_id, - removed.graph_id, - ); + let parent_graph_id = self.rb2mb.get(parent_rb.0).unwrap().graph_id; + self.connectivity_graph + .remove_edge(parent_graph_id, removed.graph_id); if wake_up { self.to_wake_up.push(RigidBodyHandle(handle.0)); @@ -236,8 +235,12 @@ impl MultibodyJointSet { for multibody in multibodies { if multibody.num_links() == 1 { // We don’t have any multibody_joint attached to this body, remove it. - if let Some(other) = self.connectivity_graph.remove_node(removed.graph_id) { - self.rb2mb.get_mut(other.0).unwrap().graph_id = removed.graph_id; + 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) + { + self.rb2mb.get_mut(other.0).unwrap().graph_id = isolated_graph_id; } } else { let mb_id = self.multibodies.insert(multibody); From c0934d0dfa284e7fa01d3a2fca10cd24d52f30b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 6 Jul 2024 18:27:36 +0200 Subject: [PATCH 3/4] chore: update changelog --- CHANGELOG.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0cfe853a3..f0026e546 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,7 +1,14 @@ ## Unreleased +### Fix + +- Fix crash when removing a multibody joint, or a rigid-body with a multipody-joint attached to it. +- Fix crash when inserting multibody joints in an arbitrary order (instead of incrementally from root to leaf). + ### Added -- Implement rotation gizmo for Ball 2D shape (as radius line) in Debug renderer if `DebugRenderMode::COLLIDER_SHAPES` enabled + +- Implement rotation gizmo for Ball 2D shape (as radius line) in Debug renderer if `DebugRenderMode::COLLIDER_SHAPES` + enabled ## v0.21.0 (23 June 2024) From b4ff571922c40b4f63c1d94613b627448cddf773 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 6 Jul 2024 18:28:40 +0200 Subject: [PATCH 4/4] chore: add one more test for the multibody insertion out-of-bounds issue --- .../joint/multibody_joint/multibody.rs | 54 ++++++++++++++++++- 1 file changed, 53 insertions(+), 1 deletion(-) diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 6c8344ba2..7832abe20 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1374,7 +1374,8 @@ mod test { use crate::dynamics::{ImpulseJointSet, IslandManager}; use crate::math::{Real, SPATIAL_DIM}; use crate::prelude::{ - ColliderSet, MultibodyJointSet, RevoluteJoint, RigidBodyBuilder, RigidBodySet, + ColliderSet, MultibodyJointHandle, MultibodyJointSet, RevoluteJoint, RigidBodyBuilder, + RigidBodySet, }; use na::{DVector, RowDVector}; @@ -1400,6 +1401,57 @@ mod test { assert_eq!(joints.get(mb_handle).unwrap().0.ndofs, SPATIAL_DIM + 3); } + #[test] + fn test_multibody_insert() { + let mut rnd = oorandom::Rand32::new(1234); + + for k in 0..10 { + let mut bodies = RigidBodySet::new(); + let mut multibody_joints = MultibodyJointSet::new(); + + let num_links = 100; + let mut handles = vec![]; + + for _ in 0..num_links { + handles.push(bodies.insert(RigidBodyBuilder::dynamic())); + } + + let mut insertion_id: Vec<_> = (0..num_links - 1).collect(); + + #[cfg(feature = "dim2")] + let joint = RevoluteJoint::new(); + #[cfg(feature = "dim3")] + let joint = RevoluteJoint::new(na::Vector::x_axis()); + + match k { + 0 => {} // Remove in insertion order. + 1 => { + // Remove from leaf to root. + insertion_id.reverse(); + } + _ => { + // Shuffle the vector a bit. + // (This test checks multiple shuffle arrangements due to k > 2). + for l in 0..num_links - 1 { + insertion_id.swap(l, rnd.rand_range(0..num_links as u32 - 1) as usize); + } + } + } + + let mut mb_handle = MultibodyJointHandle::invalid(); + for i in insertion_id { + mb_handle = multibody_joints + .insert(handles[i], handles[i + 1], joint, true) + .unwrap(); + } + + assert_eq!( + multibody_joints.get(mb_handle).unwrap().0.ndofs, + SPATIAL_DIM + num_links - 1 + ); + } + } + #[test] fn test_multibody_remove() { let mut rnd = oorandom::Rand32::new(1234);