Fix testbed snapshot restore system

2 files changed, 42 insertions, 7 deletions

diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs
index 26a55c7..bc00327 100644
--- a/src_testbed/physics/mod.rs
+++ b/src_testbed/physics/mod.rs
@@ -14,6 +14,8 @@ pub struct PhysicsSnapshot {
     bodies: Vec<u8>,
     colliders: Vec<u8>,
     impulse_joints: Vec<u8>,
+    multibody_joints: Vec<u8>,
+    island_manager: Vec<u8>,
 }
 
 impl PhysicsSnapshot {
@@ -21,17 +23,21 @@ impl PhysicsSnapshot {
         timestep_id: usize,
         broad_phase: &BroadPhase,
         narrow_phase: &NarrowPhase,
+        island_manager: &IslandManager,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
         impulse_joints: &ImpulseJointSet,
+        multibody_joints: &MultibodyJointSet,
     ) -> bincode::Result<Self> {
         Ok(Self {
             timestep_id,
             broad_phase: bincode::serialize(broad_phase)?,
             narrow_phase: bincode::serialize(narrow_phase)?,
+            island_manager: bincode::serialize(island_manager)?,
             bodies: bincode::serialize(bodies)?,
             colliders: bincode::serialize(colliders)?,
             impulse_joints: bincode::serialize(impulse_joints)?,
+            multibody_joints: bincode::serialize(multibody_joints)?,
         })
     }
 
@@ -41,32 +47,40 @@ impl PhysicsSnapshot {
         usize,
         BroadPhase,
         NarrowPhase,
+        IslandManager,
         RigidBodySet,
         ColliderSet,
         ImpulseJointSet,
+        MultibodyJointSet,
     )> {
         Ok((
             self.timestep_id,
             bincode::deserialize(&self.broad_phase)?,
             bincode::deserialize(&self.narrow_phase)?,
+            bincode::deserialize(&self.island_manager)?,
             bincode::deserialize(&self.bodies)?,
             bincode::deserialize(&self.colliders)?,
             bincode::deserialize(&self.impulse_joints)?,
+            bincode::deserialize(&self.multibody_joints)?,
         ))
     }
 
     pub fn print_snapshot_len(&self) {
         let total = self.broad_phase.len()
             + self.narrow_phase.len()
+            + self.island_manager.len()
             + self.bodies.len()
             + self.colliders.len()
-            + self.impulse_joints.len();
+            + self.impulse_joints.len()
+            + self.multibody_joints.len();
         println!("Snapshot length: {}B", total);
         println!("|_ broad_phase: {}B", self.broad_phase.len());
         println!("|_ narrow_phase: {}B", self.narrow_phase.len());
+        println!("|_ island_manager: {}B", self.island_manager.len());
         println!("|_ bodies: {}B", self.bodies.len());
         println!("|_ colliders: {}B", self.colliders.len());
         println!("|_ impulse_joints: {}B", self.impulse_joints.len());
+        println!("|_ multibody_joints: {}B", self.multibody_joints.len());
     }
 }
 
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index e5a5f13..5430384 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -24,7 +24,7 @@ use rapier::dynamics::{
 use rapier::geometry::Ray;
 use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 use rapier::math::{Real, Vector};
-use rapier::pipeline::{PhysicsHooks, QueryFilter};
+use rapier::pipeline::{PhysicsHooks, QueryFilter, QueryPipeline};
 
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
 use crate::box2d_backend::Box2dWorld;
@@ -1250,9 +1250,11 @@ fn update_testbed(
                 harness.state.timestep_id,
                 &harness.physics.broad_phase,
                 &harness.physics.narrow_phase,
+                &harness.physics.islands,
                 &harness.physics.bodies,
                 &harness.physics.colliders,
                 &harness.physics.impulse_joints,
+                &harness.physics.multibody_joints,
             )
             .ok();
 
@@ -1269,17 +1271,36 @@ fn update_testbed(
                 .action_flags
                 .set(TestbedActionFlags::RESTORE_SNAPSHOT, false);
             if let Some(snapshot) = &state.snapshot {
-                if let Ok(w) = snapshot.restore() {
+                if let Ok((
+                    timestep_id,
+                    broad_phase,
+                    narrow_phase,
+                    island_manager,
+                    bodies,
+                    colliders,
+                    impulse_joints,
+                    multibody_joints,
+                )) = snapshot.restore()
+                {
                     clear(&mut commands, &mut state, &mut graphics, &mut plugins);
 
                     for plugin in &mut plugins.0 {
                         plugin.clear_graphics(&mut graphics, &mut commands);
                     }
 
-                    // set_world(w.3, w.4, w.5);
-                    harness.physics.broad_phase = w.1;
-                    harness.physics.narrow_phase = w.2;
-                    harness.state.timestep_id = w.0;
+                    harness.state.timestep_id = timestep_id;
+                    harness.physics.broad_phase = broad_phase;
+                    harness.physics.narrow_phase = narrow_phase;
+                    harness.physics.islands = island_manager;
+                    harness.physics.bodies = bodies;
+                    harness.physics.colliders = colliders;
+                    harness.physics.impulse_joints = impulse_joints;
+                    harness.physics.multibody_joints = multibody_joints;
+                    harness.physics.query_pipeline = QueryPipeline::new();
+
+                    state
+                        .action_flags
+                        .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true);
                 }
             }
         }