Fix shape modification not updating graphics in testbed (#708)

* Fix shape modification not updating graphics in testbed

* Add update collider to Testbed

* chore: lint shape_modifications3

* chore: simplify GraphicsManager::remove_collider_nodes

---------

Co-authored-by: Sébastien Crozet <sebcrozet@dimforge.com>

3 files changed, 52 insertions, 11 deletions

diff --git a/examples3d/debug_shape_modification3.rs b/examples3d/debug_shape_modification3.rs
index 0fc3cc5..8974218 100644
--- a/examples3d/debug_shape_modification3.rs
+++ b/examples3d/debug_shape_modification3.rs
@@ -35,13 +35,32 @@ pub fn init_world(testbed: &mut Testbed) {
     let collider = ColliderBuilder::ball(ball_rad).density(100.0);
     let ball_coll_handle = colliders.insert_with_parent(collider, ball_handle, &mut bodies);
 
+    /*
+     * Colliders without bodies
+     */
+    let shape_size = 3.0;
+    let static_collider =
+        ColliderBuilder::ball(shape_size).translation(vector![-15.0, shape_size, 18.0]);
+    colliders.insert(static_collider);
+
+    let shapes = [
+        SharedShape::ball(shape_size),
+        SharedShape::cuboid(shape_size, shape_size, shape_size),
+        SharedShape::cone(shape_size, shape_size),
+        SharedShape::cylinder(shape_size, shape_size),
+    ];
+    let mut shape_idx = 0;
+    let shapeshifting_collider = ColliderBuilder::new(shapes[shape_idx].clone())
+        .translation(vector![-15.0, shape_size, 9.0]);
+    let shapeshifting_coll_handle = colliders.insert(shapeshifting_collider);
+
     let mut linvel = Vector::zeros();
     let mut angvel = Vector::zeros();
     let mut pos = Isometry::identity();
     let mut step = 0;
     let snapped_frame = 51;
 
-    testbed.add_callback(move |_, physics, _, _| {
+    testbed.add_callback(move |mut gfx, physics, _, _| {
         step += 1;
 
         // Snap the ball velocity or restore it.
@@ -53,6 +72,15 @@ pub fn init_world(testbed: &mut Testbed) {
             pos = *ball.position();
         }
 
+        let shapeshifting_coll = physics
+            .colliders
+            .get_mut(shapeshifting_coll_handle)
+            .unwrap();
+        if step % 50 == 0 {
+            shape_idx = (shape_idx + 1) % 4;
+            shapeshifting_coll.set_shape(shapes[shape_idx].clone())
+        }
+
         if step == 100 {
             ball.set_linvel(linvel, true);
             ball.set_angvel(angvel, true);
@@ -62,6 +90,11 @@ pub fn init_world(testbed: &mut Testbed) {
 
         let ball_coll = physics.colliders.get_mut(ball_coll_handle).unwrap();
         ball_coll.set_shape(SharedShape::ball(ball_rad * step as f32 * 2.0));
+
+        if let Some(gfx) = &mut gfx {
+            gfx.update_collider(ball_coll_handle, &physics.colliders);
+            gfx.update_collider(shapeshifting_coll_handle, &physics.colliders);
+        }
     });
 
     /*
@@ -111,5 +144,5 @@ pub fn init_world(testbed: &mut Testbed) {
      * Set up the testbed.
      */
     testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
-    testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
+    testbed.look_at(point![40.0, 40.0, 40.0], Point::origin());
 }
diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs
index 076ef29..355f80f 100644
--- a/src_testbed/graphics.rs
+++ b/src_testbed/graphics.rs
@@ -83,13 +83,16 @@ impl GraphicsManager {
     ) {
         let body = body.unwrap_or(RigidBodyHandle::invalid());
         if let Some(sns) = self.b2sn.get_mut(&body) {
-            for sn in sns.iter_mut() {
+            sns.retain(|sn| {
                 if let Some(sn_c) = sn.collider {
                     if sn_c == collider {
                         commands.entity(sn.entity).despawn();
+                        return false;
                     }
                 }
-            }
+
+                true
+            });
         }
     }
 
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 2020ed9..37b5710 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -492,13 +492,6 @@ impl TestbedGraphics<'_, '_, '_, '_, '_, '_> {
         )
     }
 
-    pub fn remove_collider(&mut self, handle: ColliderHandle, colliders: &ColliderSet) {
-        if let Some(parent_handle) = colliders.get(handle).map(|c| c.parent()) {
-            self.graphics
-                .remove_collider_nodes(&mut *self.commands, parent_handle, handle)
-        }
-    }
-
     pub fn remove_body(&mut self, handle: RigidBodyHandle) {
         self.graphics.remove_body_nodes(&mut *self.commands, handle)
     }
@@ -513,6 +506,18 @@ impl TestbedGraphics<'_, '_, '_, '_, '_, '_> {
         )
     }
 
+    pub fn remove_collider(&mut self, handle: ColliderHandle, colliders: &ColliderSet) {
+        if let Some(parent_handle) = colliders.get(handle).map(|c| c.parent()) {
+            self.graphics
+                .remove_collider_nodes(&mut *self.commands, parent_handle, handle)
+        }
+    }
+
+    pub fn update_collider(&mut self, handle: ColliderHandle, colliders: &ColliderSet) {
+        self.remove_collider(handle, colliders);
+        self.add_collider(handle, colliders);
+    }
+
     pub fn keys(&self) -> &ButtonInput<KeyCode> {
         self.keys
     }