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

5 files changed, 51 insertions, 27 deletions

diff --git a/CHANGELOG.md b/CHANGELOG.md
index e6a110a..070b778 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -8,6 +8,7 @@
 ### Modified
 
 - `InteractionGroups` default value for `memberships` is now `GROUP_1` (#706)
+- `ImpulseJointSet::get_mut` has a new parameter `wake_up: bool`, to wake up connected bodies.
 
 ## v0.22.0 (20 July 2024)
 
diff --git a/examples3d/vehicle_joints3.rs b/examples3d/vehicle_joints3.rs
index c284175..517bd9c 100644
--- a/examples3d/vehicle_joints3.rs
+++ b/examples3d/vehicle_joints3.rs
@@ -158,14 +158,17 @@ pub fn init_world(testbed: &mut Testbed) {
                 _ => {}
             }
         }
-
+        let mut should_wake_up = false;
         if thrust != 0.0 || steering != 0.0 {
-            physics.bodies.get_mut(body_handle).unwrap().wake_up(true);
+            should_wake_up = true;
         }
 
         // Apply steering to the axles.
         for steering_handle in &steering_joints {
-            let steering_joint = physics.impulse_joints.get_mut(*steering_handle).unwrap();
+            let steering_joint = physics
+                .impulse_joints
+                .get_mut(*steering_handle, should_wake_up)
+                .unwrap();
             steering_joint.data.set_motor_position(
                 JointAxis::AngY,
                 max_steering_angle * steering,
@@ -187,7 +190,10 @@ pub fn init_world(testbed: &mut Testbed) {
 
         let ms = [1.0 / speed_diff, speed_diff];
         for (motor_handle, &ms) in motor_joints.iter().copied().zip(ms.iter()) {
-            let motor_joint = physics.impulse_joints.get_mut(motor_handle).unwrap();
+            let motor_joint = physics
+                .impulse_joints
+                .get_mut(motor_handle, should_wake_up)
+                .unwrap();
             motor_joint.data.set_motor_velocity(
                 JointAxis::AngX,
                 -30.0 * thrust * ms * boost,
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 3331822..8f47e0e 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -1,3 +1,5 @@
+use parry::utils::hashmap::HashMap;
+
 use super::ImpulseJoint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
@@ -40,9 +42,11 @@ pub(crate) type JointGraphEdge = crate::data::graph::Edge<ImpulseJoint>;
 /// A set of impulse_joints that can be handled by a physics `World`.
 pub struct ImpulseJointSet {
     rb_graph_ids: Coarena<RigidBodyGraphIndex>,
-    joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
+    /// Map joint handles to edge ids on the graph.
+    joint_ids: Arena<TemporaryInteractionIndex>,
     joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
-    pub(crate) to_wake_up: Vec<RigidBodyHandle>, // A set of rigid-body handles to wake-up during the next timestep.
+    /// A set of rigid-body handles to wake-up during the next timestep.
+    pub(crate) to_wake_up: HashMap<RigidBodyHandle, ()>,
 }
 
 impl ImpulseJointSet {
@@ -52,7 +56,7 @@ impl ImpulseJointSet {
             rb_graph_ids: Coarena::new(),
             joint_ids: Arena::new(),
             joint_graph: InteractionGraph::new(),
-            to_wake_up: vec![],
+            to_wake_up: HashMap::default(),
         }
     }
 
@@ -145,9 +149,20 @@ impl ImpulseJointSet {
     }
 
     /// Gets a mutable reference to the joint with the given handle.
-    pub fn get_mut(&mut self, handle: ImpulseJointHandle) -> Option<&mut ImpulseJoint> {
+    pub fn get_mut(
+        &mut self,
+        handle: ImpulseJointHandle,
+        wake_up_connected_bodies: bool,
+    ) -> Option<&mut ImpulseJoint> {
         let id = self.joint_ids.get(handle.0)?;
-        self.joint_graph.graph.edge_weight_mut(*id)
+        let joint = self.joint_graph.graph.edge_weight_mut(*id);
+        if wake_up_connected_bodies {
+            if let Some(joint) = &joint {
+                self.to_wake_up.insert(joint.body1, ());
+                self.to_wake_up.insert(joint.body2, ());
+            }
+        }
+        joint
     }
 
     /// Gets the joint with the given handle without a known generation.
@@ -269,8 +284,8 @@ impl ImpulseJointSet {
         self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint);
 
         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, ());
         }
 
         ImpulseJointHandle(handle)
@@ -320,10 +335,10 @@ impl ImpulseJointSet {
 
         if wake_up {
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
-                self.to_wake_up.push(*rb_handle);
+                self.to_wake_up.insert(*rb_handle, ());
             }
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
-                self.to_wake_up.push(*rb_handle);
+                self.to_wake_up.insert(*rb_handle, ());
             }
         }
 
@@ -372,8 +387,8 @@ impl ImpulseJointSet {
                     }
 
                     // Wake up the attached bodies.
-                    self.to_wake_up.push(h1);
-                    self.to_wake_up.push(h2);
+                    self.to_wake_up.insert(h1, ());
+                    self.to_wake_up.insert(h2, ());
                 }
 
                 if let Some(other) = self.joint_graph.remove_node(deleted_id) {
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 {
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 8d1fec4..ab4b119 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -428,10 +428,10 @@ impl PhysicsPipeline {
         self.counters.stages.user_changes.start();
         for handle in impulse_joints
             .to_wake_up
-            .drain(..)
-            .chain(multibody_joints.to_wake_up.drain(..))
+            .drain()
+            .chain(multibody_joints.to_wake_up.drain())
         {
-            islands.wake_up(bodies, handle, true);
+            islands.wake_up(bodies, handle.0, true);
         }
 
         // Apply modifications.