nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix broken multibody joint removal.

Browse Source
This commit is contained in:
Sébastien Crozet
2024-07-06 18:21:52 +02:00
committed by Sébastien Crozet
parent f34152e78b
commit 9e699e0315
6 changed files with 84 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -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);