Fix BroadPhase proxy handle recycling causing a crash.

10 files changed, 308 insertions, 148 deletions

diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs
new file mode 100644
index 0000000..49da436
--- /dev/null
+++ b/examples2d/add_remove2.rs
@@ -0,0 +1,53 @@
+use na::Point2;
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    let bodies = RigidBodySet::new();
+    let colliders = ColliderSet::new();
+    let joints = JointSet::new();
+    let rad = 0.5;
+
+    // Callback that will be executed on the main loop to handle proximities.
+    testbed.add_callback(move |window, physics, _, graphics, _| {
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(0.0, 10.0)
+            .build();
+        let handle = physics.bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build();
+        physics
+            .colliders
+            .insert(collider, handle, &mut physics.bodies);
+        graphics.add(window, handle, &physics.bodies, &physics.colliders);
+
+        let to_remove: Vec<_> = physics
+            .bodies
+            .iter()
+            .filter(|(_, b)| b.position.translation.vector.y < -10.0)
+            .map(|e| e.0)
+            .collect();
+        for handle in to_remove {
+            physics.pipeline.remove_rigid_body(
+                handle,
+                &mut physics.broad_phase,
+                &mut physics.narrow_phase,
+                &mut physics.bodies,
+                &mut physics.colliders,
+                &mut physics.joints,
+            );
+            graphics.remove_body_nodes(window, handle);
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 0.0), 20.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Add-remove", init_world)]);
+    testbed.run()
+}
diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index 7b8bf09..836dcbc 100644
--- a/examples2d/all_examples2.rs
+++ b/examples2d/all_examples2.rs
@@ -10,6 +10,7 @@ use inflector::Inflector;
 use rapier_testbed2d::Testbed;
 use std::cmp::Ordering;
 
+mod add_remove2;
 mod balls2;
 mod boxes2;
 mod capsules2;
@@ -56,6 +57,7 @@ pub fn main() {
         .to_camel_case();
 
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
+        ("Add remove", add_remove2::init_world),
         ("Balls", balls2::init_world),
         ("Boxes", boxes2::init_world),
         ("Capsules", capsules2::init_world),
diff --git a/examples2d/kinematic2.rs b/examples2d/kinematic2.rs
index d979cd0..c40134b 100644
--- a/examples2d/kinematic2.rs
+++ b/examples2d/kinematic2.rs
@@ -62,8 +62,8 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Setup a callback to control the platform.
      */
-    testbed.add_callback(move |bodies, _, _, _, time| {
-        let mut platform = bodies.get_mut(platform_handle).unwrap();
+    testbed.add_callback(move |_, physics, _, _, time| {
+        let mut platform = physics.bodies.get_mut(platform_handle).unwrap();
         let mut next_pos = platform.position;
 
         let dt = 0.016;
diff --git a/examples2d/sensor2.rs b/examples2d/sensor2.rs
index 75634a8..c7e0205 100644
--- a/examples2d/sensor2.rs
+++ b/examples2d/sensor2.rs
@@ -69,15 +69,15 @@ pub fn init_world(testbed: &mut Testbed) {
     testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0));
 
     // Callback that will be executed on the main loop to handle proximities.
-    testbed.add_callback(move |_, colliders, events, graphics, _| {
+    testbed.add_callback(move |_, physics, events, graphics, _| {
         while let Ok(prox) = events.proximity_events.try_recv() {
             let color = match prox.new_status {
                 Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0),
                 Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0),
             };
 
-            let parent_handle1 = colliders.get(prox.collider1).unwrap().parent();
-            let parent_handle2 = colliders.get(prox.collider2).unwrap().parent();
+            let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent();
+            let parent_handle2 = physics.colliders.get(prox.collider2).unwrap().parent();
 
             if parent_handle1 != ground_handle && parent_handle1 != sensor_handle {
                 graphics.set_body_color(parent_handle1, color);
diff --git a/examples3d/add_remove3.rs b/examples3d/add_remove3.rs
new file mode 100644
index 0000000..cf565e0
--- /dev/null
+++ b/examples3d/add_remove3.rs
@@ -0,0 +1,53 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    let bodies = RigidBodySet::new();
+    let colliders = ColliderSet::new();
+    let joints = JointSet::new();
+    let rad = 0.5;
+
+    // Callback that will be executed on the main loop to handle proximities.
+    testbed.add_callback(move |window, physics, _, graphics, _| {
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(0.0, 10.0, 0.0)
+            .build();
+        let handle = physics.bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+        physics
+            .colliders
+            .insert(collider, handle, &mut physics.bodies);
+        graphics.add(window, handle, &physics.bodies, &physics.colliders);
+
+        let to_remove: Vec<_> = physics
+            .bodies
+            .iter()
+            .filter(|(_, b)| b.position.translation.vector.y < -10.0)
+            .map(|e| e.0)
+            .collect();
+        for handle in to_remove {
+            physics.pipeline.remove_rigid_body(
+                handle,
+                &mut physics.broad_phase,
+                &mut physics.narrow_phase,
+                &mut physics.bodies,
+                &mut physics.colliders,
+                &mut physics.joints,
+            );
+            graphics.remove_body_nodes(window, handle);
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(-30.0, -4.0, -30.0), Point3::new(0.0, 1.0, 0.0));
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Add-remove", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 10b8f33..04b9b36 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -10,6 +10,7 @@ use inflector::Inflector;
 use rapier_testbed3d::Testbed;
 use std::cmp::Ordering;
 
+mod add_remove3;
 mod balls3;
 mod boxes3;
 mod capsules3;
@@ -66,6 +67,7 @@ pub fn main() {
         .to_camel_case();
 
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
+        ("Add remove", add_remove3::init_world),
         ("Balls", balls3::init_world),
         ("Boxes", boxes3::init_world),
         ("Capsules", capsules3::init_world),
diff --git a/examples3d/kinematic3.rs b/examples3d/kinematic3.rs
index d6f2014..a9adc8c 100644
--- a/examples3d/kinematic3.rs
+++ b/examples3d/kinematic3.rs
@@ -66,8 +66,8 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Setup a callback to control the platform.
      */
-    testbed.add_callback(move |bodies, _, _, _, time| {
-        let mut platform = bodies.get_mut(platform_handle).unwrap();
+    testbed.add_callback(move |_, physics, _, _, time| {
+        let mut platform = physics.bodies.get_mut(platform_handle).unwrap();
         let mut next_pos = platform.position;
 
         let dt = 0.016;
diff --git a/examples3d/sensor3.rs b/examples3d/sensor3.rs
index 50c051f..598b0ad 100644
--- a/examples3d/sensor3.rs
+++ b/examples3d/sensor3.rs
@@ -73,15 +73,15 @@ pub fn init_world(testbed: &mut Testbed) {
     testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0));
 
     // Callback that will be executed on the main loop to handle proximities.
-    testbed.add_callback(move |_, colliders, events, graphics, _| {
+    testbed.add_callback(move |_, physics, events, graphics, _| {
         while let Ok(prox) = events.proximity_events.try_recv() {
             let color = match prox.new_status {
                 Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0),
                 Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0),
             };
 
-            let parent_handle1 = colliders.get(prox.collider1).unwrap().parent();
-            let parent_handle2 = colliders.get(prox.collider2).unwrap().parent();
+            let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent();
+            let parent_handle2 = physics.colliders.get(prox.collider2).unwrap().parent();
 
             if parent_handle1 != ground_handle && parent_handle1 != sensor_handle {
                 graphics.set_body_color(parent_handle1, color);
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index 8356339..0505e21 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -426,10 +426,10 @@ impl Proxies {
 
     pub fn insert(&mut self, proxy: BroadPhaseProxy) -> usize {
         if self.first_free != NEXT_FREE_SENTINEL {
-            let proxy_id = self.first_free;
-            self.first_free = self.elements[self.first_free as usize].next_free;
-            self.elements[self.first_free as usize] = proxy;
-            proxy_id as usize
+            let proxy_id = self.first_free as usize;
+            self.first_free = self.elements[proxy_id].next_free;
+            self.elements[proxy_id] = proxy;
+            proxy_id
         } else {
             self.elements.push(proxy);
             self.elements.len() - 1
@@ -643,3 +643,47 @@ impl BroadPhase {
         // );
     }
 }
+
+#[cfg(test)]
+mod test {
+    use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+    use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
+    use crate::pipeline::PhysicsPipeline;
+
+    #[test]
+    fn test_add_update_remove() {
+        let mut broad_phase = BroadPhase::new();
+        let mut narrow_phase = NarrowPhase::new();
+        let mut bodies = RigidBodySet::new();
+        let mut colliders = ColliderSet::new();
+        let mut joints = JointSet::new();
+        let mut pipeline = PhysicsPipeline::new();
+
+        let rb = RigidBodyBuilder::new_dynamic().build();
+        let co = ColliderBuilder::ball(0.5).build();
+        let hrb = bodies.insert(rb);
+        let hco = colliders.insert(co, hrb, &mut bodies);
+
+        broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
+
+        pipeline.remove_rigid_body(
+            hrb,
+            &mut broad_phase,
+            &mut narrow_phase,
+            &mut bodies,
+            &mut colliders,
+            &mut joints,
+        );
+
+        broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
+
+        // Create another body.
+        let rb = RigidBodyBuilder::new_dynamic().build();
+        let co = ColliderBuilder::ball(0.5).build();
+        let hrb = bodies.insert(rb);
+        let hco = colliders.insert(co, hrb, &mut bodies);
+
+        // Make sure the proxy handles is recycled properly.
+        broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
+    }
+}
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index ab709ed..868bd91 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -172,6 +172,28 @@ impl PhysicsEvents {
     }
 }
 
+pub struct PhysicsState {
+    pub broad_phase: BroadPhase,
+    pub narrow_phase: NarrowPhase,
+    pub bodies: RigidBodySet,
+    pub colliders: ColliderSet,
+    pub joints: JointSet,
+    pub pipeline: PhysicsPipeline,
+}
+
+impl PhysicsState {
+    fn new() -> Self {
+        Self {
+            broad_phase: BroadPhase::new(),
+            narrow_phase: NarrowPhase::new(),
+            bodies: RigidBodySet::new(),
+            colliders: ColliderSet::new(),
+            joints: JointSet::new(),
+            pipeline: PhysicsPipeline::new(),
+        }
+    }
+}
+
 pub struct TestbedState {
     pub running: RunMode,
     pub draw_colls: bool,
@@ -207,12 +229,7 @@ pub struct Testbed {
     fluids: Option<FluidsState>,
     gravity: Vector<f32>,
     integration_parameters: IntegrationParameters,
-    broad_phase: BroadPhase,
-    narrow_phase: NarrowPhase,
-    bodies: RigidBodySet,
-    colliders: ColliderSet,
-    joints: JointSet,
-    physics_pipeline: PhysicsPipeline,
+    physics: PhysicsState,
     window: Option<Box<Window>>,
     graphics: GraphicsManager,
     nsteps: usize,
@@ -237,17 +254,17 @@ pub struct Testbed {
     nphysics: Option<NPhysicsWorld>,
 }
 
-type Callbacks = Vec<
-    Box<dyn FnMut(&mut RigidBodySet, &mut ColliderSet, &PhysicsEvents, &mut GraphicsManager, f32)>,
->;
+type Callbacks =
+    Vec<Box<dyn FnMut(&mut Window, &mut PhysicsState, &PhysicsEvents, &mut GraphicsManager, f32)>>;
 
 #[cfg(feature = "fluids")]
 type CallbacksFluids = Vec<
     Box<
         dyn FnMut(
+            &mut Window,
             &mut LiquidWorld<f32>,
             &mut ColliderCouplingSet<f32, RigidBodyHandle>,
-            &mut RigidBodySet,
+            &mut PhysicsState,
             &mut GraphicsManager,
             f32,
         ),
@@ -316,11 +333,6 @@ impl Testbed {
 
         let gravity = Vector::y() * -9.81;
         let integration_parameters = IntegrationParameters::default();
-        let broad_phase = BroadPhase::new();
-        let narrow_phase = NarrowPhase::new();
-        let bodies = RigidBodySet::new();
-        let colliders = ColliderSet::new();
-        let joints = JointSet::new();
         let contact_channel = crossbeam::channel::unbounded();
         let proximity_channel = crossbeam::channel::unbounded();
         let event_handler = ChannelEventCollector::new(proximity_channel.0, contact_channel.0);
@@ -328,6 +340,7 @@ impl Testbed {
             contact_events: contact_channel.1,
             proximity_events: proximity_channel.1,
         };
+        let physics = PhysicsState::new();
 
         Testbed {
             builders: Vec::new(),
@@ -335,12 +348,7 @@ impl Testbed {
             fluids: None,
             gravity,
             integration_parameters,
-            broad_phase,
-            narrow_phase,
-            bodies,
-            colliders,
-            joints,
-            physics_pipeline: PhysicsPipeline::new(),
+            physics,
             callbacks: Vec::new(),
             #[cfg(feature = "fluids")]
             callbacks_fluids: Vec::new(),
@@ -401,26 +409,26 @@ impl Testbed {
     pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) {
         println!("Num bodies: {}", bodies.len());
         println!("Num joints: {}", joints.len());
-        self.bodies = bodies;
-        self.colliders = colliders;
-        self.joints = joints;
-        self.broad_phase = BroadPhase::new();
-        self.narrow_phase = NarrowPhase::new();
+        self.physics.bodies = bodies;
+        self.physics.colliders = colliders;
+        self.physics.joints = joints;
+        self.physics.broad_phase = BroadPhase::new();
+        self.physics.narrow_phase = NarrowPhase::new();
         self.state
             .action_flags
             .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true);
         self.time = 0.0;
         self.state.timestep_id = 0;
-        self.physics_pipeline.counters.enable();
+        self.physics.pipeline.counters.enable();
 
         #[cfg(all(feature = "dim2", feature = "other-backends"))]
         {
             if self.state.selected_backend == BOX2D_BACKEND {
                 self.box2d = Some(Box2dWorld::from_rapier(
                     self.gravity,
-                    &self.bodies,
-                    &self.colliders,
-                    &self.joints,
+                    &self.physics.bodies,
+                    &self.physics.colliders,
+                    &self.physics.joints,
                 ));
             }
         }
@@ -433,9 +441,9 @@ impl Testbed {
                 self.physx = Some(PhysxWorld::from_rapier(
                     self.gravity,
                     &self.integration_parameters,
-                    &self.bodies,
-                    &self.colliders,
-                    &self.joints,
+                    &self.physics.bodies,
+                    &self.physics.colliders,
+                    &self.physics.joints,
                     self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR,
                     self.state.num_threads,
                 ));
@@ -447,9 +455,9 @@ impl Testbed {
             if self.state.selected_backend == NPHYSICS_BACKEND {
                 self.nphysics = Some(NPhysicsWorld::from_rapier(
                     self.gravity,
-                    &self.bodies,
-                    &self.colliders,
-                    &self.joints,
+                    &self.physics.bodies,
+                    &self.physics.colliders,
+                    &self.physics.joints,
                 ));
             }
         }
@@ -562,8 +570,7 @@ impl Testbed {
     }
 
     pub fn add_callback<
-        F: FnMut(&mut RigidBodySet, &mut ColliderSet, &PhysicsEvents, &mut GraphicsManager, f32)
-            + 'static,
+        F: FnMut(&mut Window, &mut PhysicsState, &PhysicsEvents, &mut GraphicsManager, f32) + 'static,
     >(
         &mut self,
         callback: F,
@@ -574,9 +581,10 @@ impl Testbed {
     #[cfg(feature = "fluids")]
     pub fn add_callback_with_fluids<
         F: FnMut(
+                &mut Window,
                 &mut LiquidWorld<f32>,
                 &mut ColliderCouplingSet<f32, RigidBodyHandle>,
-                &mut RigidBodySet,
+                &mut PhysicsState,
                 &mut GraphicsManager,
                 f32,
             ) + 'static,
@@ -643,36 +651,31 @@ impl Testbed {
                                 {
                                     let gravity = &self.gravity;
                                     let params = &self.integration_parameters;
-                                    let pipeline = &mut self.physics_pipeline;
-                                    let narrow_phase = &mut self.narrow_phase;
-                                    let broad_phase = &mut self.broad_phase;
-                                    let bodies = &mut self.bodies;
-                                    let colliders = &mut self.colliders;
-                                    let joints = &mut self.joints;
+                                    let physics = &mut self.physics;
                                     let event_handler = &self.event_handler;
                                     self.state.thread_pool.install(|| {
-                                        pipeline.step(
+                                        physics.pipeline.step(
                                             gravity,
                                             params,
-                                            broad_phase,
-                                            narrow_phase,
-                                            bodies,
-                                            colliders,
-                                            joints,
+                                            &mut physics.broad_phase,
+                                            &mut physics.narrow_phase,
+                                            &mut physics.bodies,
+                                            &mut physics.colliders,
+                                            &mut physics.joints,
                                             event_handler,
                                         );
                                     });
                                 }
 
                                 #[cfg(not(feature = "parallel"))]
-                                self.physics_pipeline.step(
+                                self.physics.pipeline.step(
                                     &self.gravity,
                                     &self.integration_parameters,
-                                    &mut self.broad_phase,
-                                    &mut self.narrow_phase,
-                                    &mut self.bodies,
-                                    &mut self.colliders,
-                                    &mut self.joints,
+                                    &mut self.physics.broad_phase,
+                                    &mut self.physics.narrow_phase,
+                                    &mut self.physics.bodies,
+                                    &mut self.physics.colliders,
+                                    &mut self.physics.joints,
                                     &self.event_handler,
                                 );
 
@@ -685,9 +688,10 @@ impl Testbed {
                                         fluids.world.step_with_coupling(
                                             dt,
                                             gravity,
-                                            &mut fluids
-                                                .coupling
-                                                .as_manager_mut(&self.colliders, &mut self.bodies),
+                                            &mut fluids.coupling.as_manager_mut(
+                                                &self.physics.colliders,
+                                                &mut self.physics.bodies,
+                                            ),
                                         );
                                     }
 
@@ -699,13 +703,13 @@ impl Testbed {
                             {
                                 if self.state.selected_backend == BOX2D_BACKEND {
                                     self.box2d.as_mut().unwrap().step(
-                                        &mut self.physics_pipeline.counters,
+                                        &mut self.physics.pipeline.counters,
                                         &self.integration_parameters,
                                     );
-                                    self.box2d
-                                        .as_mut()
-                                        .unwrap()
-                                        .sync(&mut self.bodies, &mut self.colliders);
+                                    self.box2d.as_mut().unwrap().sync(
+                                        &mut self.physics.bodies,
+                                        &mut self.physics.colliders,
+                                    );
                                 }
                             }
 
@@ -716,13 +720,13 @@ impl Testbed {
                                 {
                                     //                        println!("Step");
                                     self.physx.as_mut().unwrap().step(
-                                        &mut self.physics_pipeline.counters,
+                                        &mut self.physics.pipeline.counters,
                                         &self.integration_parameters,
                                     );
-                                    self.physx
-                                        .as_mut()
-                                        .unwrap()
-                                        .sync(&mut self.bodies, &mut self.colliders);
+                                    self.physx.as_mut().unwrap().sync(
+                                        &mut self.physics.bodies,
+                                        &mut self.physics.colliders,
+                                    );
                                 }
                             }
 
@@ -730,20 +734,20 @@ impl Testbed {
                             {
                                 if self.state.selected_backend == NPHYSICS_BACKEND {
                                     self.nphysics.as_mut().unwrap().step(
-                                        &mut self.physics_pipeline.counters,
+                                        &mut self.physics.pipeline.counters,
                                         &self.integration_parameters,
                                     );
-                                    self.nphysics
-                                        .as_mut()
-                                        .unwrap()
-                                        .sync(&mut self.bodies, &mut self.colliders);
+                                    self.nphysics.as_mut().unwrap().sync(
+                                        &mut self.physics.bodies,
+                                        &mut self.physics.colliders,
+                                    );
                                 }
                             }
                         }
 
                         // Skip the first update.
                         if k > 0 {
-                            timings.push(self.physics_pipeline.counters.step_time.time());
+                            timings.push(self.physics.pipeline.counters.step_time.time());
                         }
                     }
                     results.push(timings);
@@ -790,6 +794,7 @@ impl Testbed {
             WindowEvent::Key(Key::D, Action::Release, _) => {
                 // Delete 10% of the remaining dynamic bodies.
                 let dynamic_bodies: Vec<_> = self
+                    .physics
                     .bodies
                     .iter()
                     .filter(|e| e.1.is_dynamic())
@@ -797,13 +802,13 @@ impl Testbed {
                     .collect();
                 let num_to_delete = (dynamic_bodies.len() / 10).max(1);
                 for to_delete in &dynamic_bodies[..num_to_delete] {
-                    self.physics_pipeline.remove_rigid_body(
+                    self.physics.pipeline.remove_rigid_body(
                         *to_delete,
-                        &mut self.broad_phase,
-                        &mut self.narrow_phase,
-                        &mut self.bodies,
-                        &mut self.colliders,
-                        &mut self.joints,
+                        &mut self.physics.broad_phase,
+                        &mut self.physics.narrow_phase,
+                        &mut self.physics.bodies,
+                        &mut self.physics.colliders,
+                        &mut self.physics.joints,
                     );
                 }
             }
@@ -1236,11 +1241,11 @@ impl State for Testbed {
                     .set(TestbedActionFlags::TAKE_SNAPSHOT, false);
                 self.state.snapshot = PhysicsSnapshot::new(
                     self.state.timestep_id,
-                    &self.broad_phase,
-                    &self.narrow_phase,
-                    &self.bodies,
-                    &self.colliders,
-                    &self.joints,
+                    &self.physics.broad_phase,
+                    &self.physics.narrow_phase,
+                    &self.physics.bodies,
+                    &self.physics.colliders,
+                    &self.physics.joints,
                 )
                 .ok();
 
@@ -1261,8 +1266,8 @@ impl State for Testbed {
                     if let Ok(w) = snapshot.restore() {
                         self.clear(window);
                         self.graphics.clear(window);
-                        self.broad_phase = w.1;
-                        self.narrow_phase = w.2;
+                        self.physics.broad_phase = w.1;
+                        self.physics.narrow_phase = w.2;
                         self.set_world(w.3, w.4, w.5);
                         self.state.timestep_id = w.0;
                     }
@@ -1277,9 +1282,13 @@ impl State for Testbed {
                 self.state
                     .action_flags
                     .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, false);
-                for (handle, _) in self.bodies.iter() {
-                    self.graphics
-                        .add(window, handle, &self.bodies, &self.colliders);
+                for (handle, _) in self.physics.bodies.iter() {
+                    self.graphics.add(
+                        window,
+                        handle,
+                        &self.physics.bodies,
+                        &self.physics.colliders,
+                    );
                 }
 
                 #[cfg(feature = "fluids")]
@@ -1303,7 +1312,7 @@ impl State for Testbed {
                     != self.state.flags.contains(TestbedStateFlags::WIREFRAME)
             {
                 self.graphics.toggle_wireframe_mode(
-                    &self.colliders,
+                    &self.physics.colliders,
                     self.state.flags.contains(TestbedStateFlags::WIREFRAME),
                 )
             }
@@ -1312,11 +1321,11 @@ impl State for Testbed {
                 != self.state.flags.contains(TestbedStateFlags::SLEEP)
             {
                 if self.state.flags.contains(TestbedStateFlags::SLEEP) {
-                    for (_, mut body) in self.bodies.iter_mut() {
+                    for (_, mut body) in self.physics.bodies.iter_mut() {
                         body.activation.threshold = ActivationStatus::default_threshold();
                     }
                 } else {
-                    for (_, mut body) in self.bodies.iter_mut() {
+                    for (_, mut body) in self.physics.bodies.iter_mut() {
                         body.wake_up();
                         body.activation.threshold = -1.0;
                     }
@@ -1388,36 +1397,31 @@ impl State for Testbed {
                     {
                         let gravity = &self.gravity;
                         let params = &self.integration_parameters;
-                        let pipeline = &mut self.physics_pipeline;
-                        let narrow_phase = &mut self.narrow_phase;
-                        let broad_phase = &mut self.broad_phase;
-                        let bodies = &mut self.bodies;
-                        let colliders = &mut self.colliders;
-                        let joints = &mut self.joints;
+                        let physics = &mut self.physics;
                         let event_handler = &self.event_handler;
                         self.state.thread_pool.install(|| {
-                            pipeline.step(
+                            physics.pipeline.step(
                                 gravity,
                                 params,
-                                broad_phase,
-                                narrow_phase,
-                                bodies,
-                                colliders,
-                                joints,
+                                &mut physics.broad_phase,
+                                &mut physics.narrow_phase,
+                                &mut physics.bodies,
+                                &mut physics.colliders,
+                                &mut physics.joints,
                                 event_handler,
                             );
                         });
                     }
 
                     #[cfg(not(feature = "parallel"))]
-                    self.physics_pipeline.step(
+                    self.physics.pipeline.step(
                         &self.gravity,
                         &self.integration_parameters,
-                        &mut self.broad_phase,
-                        &mut self.narrow_phase,
-                        &mut self.bodies,
-                        &mut self.colliders,
-                        &mut self.joints,
+                        &mut self.physics.broad_phase,
+                        &mut self.physics.narrow_phase,
+                        &mut self.physics.bodies,
+                        &mut self.physics.colliders,
+                        &mut self.physics.joints,
                         &self.event_handler,
                     );
 
@@ -1430,9 +1434,10 @@ impl State for Testbed {
                             fluids.world.step_with_coupling(
                                 dt,
                                 gravity,
-                                &mut fluids
-                                    .coupling
-                                    .as_manager_mut(&self.colliders, &mut self.bodies),
+                                &mut fluids.coupling.as_manager_mut(
+                                    &self.physics.colliders,
+                                    &mut self.physics.bodies,
+                                ),
                             );
                         }
 
@@ -1444,13 +1449,13 @@ impl State for Testbed {
                 {
                     if self.state.selected_backend == BOX2D_BACKEND {
                         self.box2d.as_mut().unwrap().step(
-                            &mut self.physics_pipeline.counters,
+                            &mut self.physics.pipeline.counters,
                             &self.integration_parameters,
                         );
                         self.box2d
                             .as_mut()
                             .unwrap()
-                            .sync(&mut self.bodies, &mut self.colliders);
+                            .sync(&mut self.physics.bodies, &mut self.physics.colliders);
                     }
                 }
 
@@ -1461,13 +1466,13 @@ impl State for Testbed {
                     {
                         //                        println!("Step");
                         self.physx.as_mut().unwrap().step(
-                            &mut self.physics_pipeline.counters,
+