From 4682f6cdaccd03f284854850d0e2f73fefacb337 Mon Sep 17 00:00:00 2001
From: gavlig <gavlig@gmail.com>
Date: Sat, 14 May 2022 10:33:11 +0200
Subject: [PATCH] implemented wake_up_bodies_with_active_motor for impulse
 joints

---
 src/data/graph.rs                             |  7 +++++
 src/dynamics/joint/generic_joint.rs           |  4 +++
 .../joint/impulse_joint/impulse_joint_set.rs  | 31 +++++++++++++++++++
 src/pipeline/physics_pipeline.rs              |  5 +++
 4 files changed, 47 insertions(+)

diff --git a/src/data/graph.rs b/src/data/graph.rs
index dd2c7607e..ab7d0676e 100644
--- a/src/data/graph.rs
+++ b/src/data/graph.rs
@@ -189,6 +189,13 @@ impl<N, E> Graph<N, E> {
         self.nodes.get(a.index()).map(|n| &n.weight)
     }
 
+    /// Access the weight for node `a` mutably.
+    ///
+    /// Also available with indexing syntax: `&mut graph[a]`.
+    pub fn node_weight_mut(&mut self, a: NodeIndex) -> Option<&mut N> {
+        self.nodes.get_mut(a.index()).map(|n| &mut n.weight)
+    }
+
     /// Access the weight for edge `a`.
     ///
     /// Also available with indexing syntax: `&graph[a]`.
diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
index b3277e905..1e304c613 100644
--- a/src/dynamics/joint/generic_joint.rs
+++ b/src/dynamics/joint/generic_joint.rs
@@ -206,6 +206,8 @@ pub struct GenericJoint {
     ///
     /// Note that the mostor must also be explicitly enabled by the `motors` bitmask.
     pub motors: [JointMotor; SPATIAL_DIM],
+    /// The flag indicating that motor settings were changed from outside
+    pub motor_changed: bool,
 }
 
 impl Default for GenericJoint {
@@ -219,6 +221,7 @@ impl Default for GenericJoint {
             coupled_axes: JointAxesMask::empty(),
             limits: [JointLimits::default(); SPATIAL_DIM],
             motors: [JointMotor::default(); SPATIAL_DIM],
+            motor_changed: false,
         }
     }
 }
@@ -419,6 +422,7 @@ impl GenericJoint {
         self.motors[i].target_pos = target_pos;
         self.motors[i].stiffness = stiffness;
         self.motors[i].damping = damping;
+        self.motor_changed = true;
         self
     }
 }
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 1cd177d67..6f686fda1 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -245,6 +245,37 @@ impl ImpulseJointSet {
         }
     }
 
+    /// Wake up bodies that are attached with joint that has a motor and it was changed in this frame
+    pub(crate) fn wake_up_bodies_with_active_motor(
+        &mut self,
+        islands: &mut IslandManager,
+        bodies: &mut RigidBodySet
+    )
+    {
+        let mut to_wake_up = vec![];
+        for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
+            let joint = &edge.weight;
+
+            let rb1 = &bodies[joint.body1];
+            let rb2 = &bodies[joint.body2];
+
+            if (rb1.is_sleeping() || rb2.is_sleeping()) && joint.data.motor_changed {
+                to_wake_up.push((edge.source(), i));
+                to_wake_up.push((edge.target(), i));
+            }
+        }
+
+        for (node_id, joint_id) in to_wake_up {
+            if let Some(rb_handle) = self.joint_graph.graph.node_weight_mut(node_id) {
+                islands.wake_up(bodies, *rb_handle, true);
+            }
+
+            if let Some(mut joint) = self.joint_graph.graph.edges.get_mut(joint_id) {
+                joint.weight.data.motor_changed = false;
+            }
+        }
+    }
+
     /// Removes a joint from this set.
     ///
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 012f40f0e..ef68e16be 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -391,6 +391,11 @@ impl PhysicsPipeline {
             &mut modified_colliders,
         );
 
+        impulse_joints.wake_up_bodies_with_active_motor(
+            islands,
+            bodies
+        );
+
         // TODO: do this only on user-change.
         // TODO: do we want some kind of automatic inverse kinematics?
         for multibody in &mut multibody_joints.multibodies {