Add a method to propagate collider transforms with the RigidBodySet

1 files changed, 13 insertions, 0 deletions

diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index d033839..8f5d8ef 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -259,6 +259,19 @@ impl RigidBodySet {
             (RigidBodyHandle(h), b)
         })
     }
+
+    pub fn propagate_modified_body_positions_to_colliders(&self, colliders: &mut ColliderSet) {
+        for body in self.modified_bodies.iter().filter_map(|h| self.get(*h)) {
+            if body.changes.contains(RigidBodyChanges::POSITION) {
+                for handle in body.colliders() {
+                    if let Some(collider) = colliders.get_mut(*handle) {
+                        let new_pos = body.position() * collider.position_wrt_parent().unwrap();
+                        collider.set_position(new_pos);
+                    }
+                }
+            }
+        }
+    }
 }
 
 impl Index<RigidBodyHandle> for RigidBodySet {