ImpulseJointSet::get_mut option to wake up connected bodies (#716)

1 files changed, 11 insertions, 9 deletions

diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index a9bf521..de1748c 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -1,3 +1,5 @@
+use parry::utils::hashmap::HashMap;
+
 use crate::data::{Arena, Coarena, Index};
 use crate::dynamics::joint::MultibodyLink;
 use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle};
@@ -94,7 +96,7 @@ pub struct MultibodyJointSet {
     // NOTE: this is mostly for the island extraction. So perhaps we won’t need
     //       that any more in the future when we improve our island builder.
     pub(crate) connectivity_graph: InteractionGraph<RigidBodyHandle, ()>,
-    pub(crate) to_wake_up: Vec<RigidBodyHandle>,
+    pub(crate) to_wake_up: HashMap<RigidBodyHandle, ()>,
 }
 
 impl MultibodyJointSet {
@@ -104,7 +106,7 @@ impl MultibodyJointSet {
             multibodies: Arena::new(),
             rb2mb: Coarena::new(),
             connectivity_graph: InteractionGraph::new(),
-            to_wake_up: vec![],
+            to_wake_up: HashMap::default(),
         }
     }
 
@@ -200,8 +202,8 @@ impl MultibodyJointSet {
         multibody1.append(mb2, link1.id, MultibodyJoint::new(data.into(), kinematic));
 
         if wake_up {
-            self.to_wake_up.push(body1);
-            self.to_wake_up.push(body2);
+            self.to_wake_up.insert(body1, ());
+            self.to_wake_up.insert(body2, ());
         }
 
         // Because each rigid-body can only have one parent link,
@@ -223,8 +225,8 @@ impl MultibodyJointSet {
                     .remove_edge(parent_graph_id, removed.graph_id);
 
                 if wake_up {
-                    self.to_wake_up.push(RigidBodyHandle(handle.0));
-                    self.to_wake_up.push(parent_rb);
+                    self.to_wake_up.insert(RigidBodyHandle(handle.0), ());
+                    self.to_wake_up.insert(parent_rb, ());
                 }
 
                 // TODO: remove the node if it no longer has any attached edges?
@@ -265,7 +267,7 @@ impl MultibodyJointSet {
                 let rb_handle = link.rigid_body;
 
                 if wake_up {
-                    self.to_wake_up.push(rb_handle);
+                    self.to_wake_up.insert(rb_handle, ());
                 }
 
                 // Remove the rigid-body <-> multibody mapping for this link.
@@ -290,8 +292,8 @@ impl MultibodyJointSet {
                 // There is a multibody_joint handle is equal to the second rigid-body’s handle.
                 articulations_to_remove.push(MultibodyJointHandle(rb2.0));
 
-                self.to_wake_up.push(rb1);
-                self.to_wake_up.push(rb2);
+                self.to_wake_up.insert(rb1, ());
+                self.to_wake_up.insert(rb2, ());
             }
 
             for articulation_handle in articulations_to_remove {