Merge branch 'master' into collider-builder-debug

12 files changed, 413 insertions, 192 deletions

diff --git a/src_testbed/camera2d.rs b/src_testbed/camera2d.rs
index 6ba17a1..ae5c163 100644
--- a/src_testbed/camera2d.rs
+++ b/src_testbed/camera2d.rs
@@ -48,7 +48,7 @@ impl OrbitCameraPlugin {
     fn mouse_motion_system(
         _time: Res<Time>,
         mut mouse_motion_events: EventReader<MouseMotion>,
-        mouse_button_input: Res<Input<MouseButton>>,
+        mouse_button_input: Res<ButtonInput<MouseButton>>,
         mut query: Query<(&mut OrbitCamera, &mut Transform, &mut Camera)>,
     ) {
         let mut delta = Vec2::ZERO;
diff --git a/src_testbed/camera3d.rs b/src_testbed/camera3d.rs
index 9c3764e..b2313b7 100644
--- a/src_testbed/camera3d.rs
+++ b/src_testbed/camera3d.rs
@@ -61,7 +61,7 @@ impl OrbitCameraPlugin {
     fn mouse_motion_system(
         time: Res<Time>,
         mut mouse_motion_events: EventReader<MouseMotion>,
-        mouse_button_input: Res<Input<MouseButton>>,
+        mouse_button_input: Res<ButtonInput<MouseButton>>,
         mut query: Query<(&mut OrbitCamera, &mut Transform, &mut Camera)>,
     ) {
         let mut delta = Vec2::ZERO;
diff --git a/src_testbed/debug_render.rs b/src_testbed/debug_render.rs
index 0e0b8b8..3138546 100644
--- a/src_testbed/debug_render.rs
+++ b/src_testbed/debug_render.rs
@@ -30,11 +30,11 @@ impl Plugin for RapierDebugRenderPlugin {
     }
 }
 
-struct BevyLinesRenderBackend<'a> {
-    gizmos: Gizmos<'a>,
+struct BevyLinesRenderBackend<'w, 's> {
+    gizmos: Gizmos<'w, 's>,
 }
 
-impl<'a> DebugRenderBackend for BevyLinesRenderBackend<'a> {
+impl<'w, 's> DebugRenderBackend for BevyLinesRenderBackend<'w, 's> {
     #[cfg(feature = "dim2")]
     fn draw_line(&mut self, _: DebugRenderObject, a: Point<Real>, b: Point<Real>, color: [f32; 4]) {
         self.gizmos.line(
diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs
index 552d4c7..226be4d 100644
--- a/src_testbed/graphics.rs
+++ b/src_testbed/graphics.rs
@@ -22,6 +22,9 @@ pub type BevyMaterial = ColorMaterial;
 #[cfg(feature = "dim3")]
 pub type BevyMaterial = StandardMaterial;
 
+pub type InstancedMaterials = HashMap<Point3<usize>, Handle<BevyMaterial>>;
+pub const SELECTED_OBJECT_MATERIAL_KEY: Point3<usize> = point![42, 42, 42];
+
 pub struct GraphicsManager {
     rand: Pcg32,
     b2sn: HashMap<RigidBodyHandle, Vec<EntityWithGraphics>>,
@@ -30,6 +33,7 @@ pub struct GraphicsManager {
     b2wireframe: HashMap<RigidBodyHandle, bool>,
     ground_color: Point3<f32>,
     prefab_meshes: HashMap<ShapeType, Handle<Mesh>>,
+    instanced_materials: InstancedMaterials,
     pub gfx_shift: Vector<Real>,
 }
 
@@ -43,10 +47,15 @@ impl GraphicsManager {
             ground_color: point![0.5, 0.5, 0.5],
             b2wireframe: HashMap::new(),
             prefab_meshes: HashMap::new(),
+            instanced_materials: HashMap::new(),
             gfx_shift: Vector::zeros(),
         }
     }
 
+    pub fn selection_material(&self) -> Handle<BevyMaterial> {
+        self.instanced_materials[&SELECTED_OBJECT_MATERIAL_KEY].clone_weak()
+    }
+
     pub fn clear(&mut self, commands: &mut Commands) {
         for sns in self.b2sn.values_mut() {
             for sn in sns.iter_mut() {
@@ -54,6 +63,7 @@ impl GraphicsManager {
             }
         }
 
+        self.instanced_materials.clear();
         self.b2sn.clear();
         self.c2color.clear();
         self.b2color.clear();
@@ -159,10 +169,11 @@ impl GraphicsManager {
 
     fn gen_color(rng: &mut Pcg32) -> Point3<f32> {
         let mut color: Point3<f32> = rng.gen();
-        color *= 1.5;
-        color.x = color.x.min(1.0);
-        color.y = color.y.min(1.0);
-        color.z = color.z.min(1.0);
+
+        // Quantize the colors a bit to get some amount of auto-instancing from bevy.
+        color.x = (color.x * 5.0).round() / 5.0;
+        color.y = (color.y * 5.0).round() / 5.0;
+        color.z = (color.z * 5.0).round() / 5.0;
         color
     }
 
@@ -191,7 +202,7 @@ impl GraphicsManager {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         handle: RigidBodyHandle,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
@@ -214,7 +225,7 @@ impl GraphicsManager {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         handle: RigidBodyHandle,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
@@ -330,6 +341,7 @@ impl GraphicsManager {
                 meshes,
                 materials,
                 &self.prefab_meshes,
+                &mut self.instanced_materials,
                 shape,
                 handle,
                 *pos,
@@ -345,7 +357,7 @@ impl GraphicsManager {
         &mut self,
         _bodies: &RigidBodySet,
         colliders: &ColliderSet,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         _materials: &mut Assets<BevyMaterial>,
     ) {
         for (_, ns) in self.b2sn.iter_mut() {
diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs
index e27a03a..9c41472 100644
--- a/src_testbed/harness/mod.rs
+++ b/src_testbed/harness/mod.rs
@@ -9,7 +9,7 @@ use rapier::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
     RigidBodySet,
 };
-use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
+use rapier::geometry::{ColliderSet, DefaultBroadPhase, NarrowPhase};
 use rapier::math::{Real, Vector};
 use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
@@ -179,7 +179,7 @@ impl Harness {
         self.physics.hooks = Box::new(hooks);
 
         self.physics.islands = IslandManager::new();
-        self.physics.broad_phase = BroadPhase::new();
+        self.physics.broad_phase = DefaultBroadPhase::new();
         self.physics.narrow_phase = NarrowPhase::new();
         self.state.timestep_id = 0;
         self.state.time = 0.0;
diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs
index cb24d7e..2cb002a 100644
--- a/src_testbed/lib.rs
+++ b/src_testbed/lib.rs
@@ -1,4 +1,5 @@
 #![allow(clippy::too_many_arguments)]
+#![allow(unexpected_cfgs)] // This happens due to the dim2/dim3/f32/f64 cfg.
 
 extern crate nalgebra as na;
 
@@ -25,6 +26,7 @@ mod camera3d;
 mod debug_render;
 mod graphics;
 pub mod harness;
+mod mouse;
 pub mod objects;
 pub mod physics;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
diff --git a/src_testbed/mouse.rs b/src_testbed/mouse.rs
new file mode 100644
index 0000000..05b69e2
--- /dev/null
+++ b/src_testbed/mouse.rs
@@ -0,0 +1,46 @@
+use crate::math::Point;
+use bevy::prelude::*;
+use bevy::window::PrimaryWindow;
+
+#[derive(Component)]
+pub struct MainCamera;
+
+#[derive(Default, Copy, Clone, Debug, Resource)]
+pub struct SceneMouse {
+    #[cfg(feature = "dim2")]
+    pub point: Option<Point<f32>>,
+    #[cfg(feature = "dim3")]
+    pub ray: Option<(Point<f32>, crate::math::Vector<f32>)>,
+}
+
+pub fn track_mouse_state(
+    mut scene_mouse: ResMut<SceneMouse>,
+    windows: Query<&Window, With<PrimaryWindow>>,
+    camera: Query<(&GlobalTransform, &Camera), With<MainCamera>>,
+) {
+    if let Ok(window) = windows.get_single() {
+        for (camera_transform, camera) in camera.iter() {
+            if let Some(cursor) = window.cursor_position() {
+                let ndc_cursor = ((cursor / Vec2::new(window.width(), window.height()) * 2.0)
+                    - Vec2::ONE)
+                    * Vec2::new(1.0, -1.0);
+                let ndc_to_world =
+                    camera_transform.compute_matrix() * camera.projection_matrix().inverse();
+                let ray_pt1 =
+                    ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
+
+                #[cfg(feature = "dim2")]
+                {
+                    scene_mouse.point = Some(Point::new(ray_pt1.x, ray_pt1.y));
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    let ray_pt2 =
+                        ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
+                    let ray_dir = ray_pt2 - ray_pt1;
+                    scene_mouse.ray = Some((na::Vector3::from(ray_pt1).into(), ray_dir.into()));
+                }
+            }
+        }
+    }
+}
diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs
index cdc042b..58e3a00 100644
--- a/src_testbed/objects/node.rs
+++ b/src_testbed/objects/node.rs
@@ -14,7 +14,7 @@ use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
 use rapier::geometry::{Cone, Cylinder};
 use rapier::math::{Isometry, Real, Vector};
 
-use crate::graphics::BevyMaterial;
+use crate::graphics::{BevyMaterial, InstancedMaterials, SELECTED_OBJECT_MATERIAL_KEY};
 #[cfg(feature = "dim2")]
 use {
     bevy_sprite::MaterialMesh2dBundle,
@@ -30,15 +30,43 @@ pub struct EntityWithGraphics {
     pub collider: Option<ColliderHandle>,
     pub delta: Isometry<Real>,
     pub opacity: f32,
-    material: Handle<BevyMaterial>,
+    pub material: Handle<BevyMaterial>,
 }
 
 impl EntityWithGraphics {
+    pub fn register_selected_object_material(
+        materials: &mut Assets<BevyMaterial>,
+        instanced_materials: &mut InstancedMaterials,
+    ) {
+        if instanced_materials.contains_key(&SELECTED_OBJECT_MATERIAL_KEY) {
+            return; // Already added.
+        }
+
+        #[cfg(feature = "dim2")]
+        let selection_material = ColorMaterial {
+            color: Color::rgb(1.0, 0.0, 0.0),
+            texture: None,
+        };
+        #[cfg(feature = "dim3")]
+        let selection_material = StandardMaterial {
+            metallic: 0.5,
+            perceptual_roughness: 0.5,
+            double_sided: true, // TODO: this doesn't do anything?
+            ..StandardMaterial::from(Color::rgb(1.0, 0.0, 0.0))
+        };
+
+        instanced_materials.insert(
+            SELECTED_OBJECT_MATERIAL_KEY,
+            materials.add(selection_material),
+        );
+    }
+
     pub fn spawn(
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
         prefab_meshs: &HashMap<ShapeType, Handle<Mesh>>,
+        instanced_materials: &mut InstancedMaterials,
         shape: &dyn Shape,
         collider: Option<ColliderHandle>,
         collider_pos: Isometry<Real>,
@@ -46,6 +74,8 @@ impl EntityWithGraphics {
         color: Point3<f32>,
         sensor: bool,
     ) -> Self {
+        Self::register_selected_object_material(materials, instanced_materials);
+
         let entity = commands.spawn_empty().id();
 
         let scale = collider_mesh_scale(shape);
@@ -90,21 +120,23 @@ impl EntityWithGraphics {
             double_sided: true, // TODO: this doesn't do anything?
             ..StandardMaterial::from(bevy_color)
         };
-        let material_handle = materials.add(material);
+        let material_handle = instanced_materials
+            .entry(color.coords.map(|c| (c * 255.0) as usize).into())
+            .or_insert_with(|| materials.add(material));
         let material_weak_handle = material_handle.clone_weak();
 
         if let Some(mesh) = mesh {
             #[cfg(feature = "dim2")]
             let bundle = MaterialMesh2dBundle {
                 mesh: mesh.into(),
-                material: material_handle,
+                material: material_handle.clone_weak(),
                 transform,
                 ..Default::default()
             };
             #[cfg(feature = "dim3")]
             let bundle = PbrBundle {
                 mesh,
-                material: material_handle,
+                material: material_handle.clone_weak(),
                 transform,
                 ..Default::default()
             };
@@ -133,28 +165,6 @@ impl EntityWithGraphics {
         commands.entity(self.entity).despawn();
     }
 
-    pub fn select(&mut self, materials: &mut Assets<BevyMaterial>) {
-        // NOTE: we don't just call `self.set_color` because that would
-        //       overwrite self.base_color too.
-        self.color = point![1.0, 0.0, 0.0];
-        if let Some(material) = materials.get_mut(&self.material) {
-            #[cfg(feature = "dim2")]
-            {
-                material.color =
-                    Color::rgba(self.color.x, self.color.y, self.color.z, self.opacity);
-            }
-            #[cfg(feature = "dim3")]
-            {
-                material.base_color =
-                    Color::rgba(self.color.x, self.color.y, self.color.z, self.opacity);
-            }
-        }
-    }
-
-    pub fn unselect(&mut self, materials: &mut Assets<BevyMaterial>) {
-        self.set_color(materials, self.base_color);
-    }
-
     pub fn set_color(&mut self, materials: &mut Assets<BevyMaterial>, color: Point3<f32>) {
         if let Some(material) = materials.get_mut(&self.material) {
             #[cfg(feature = "dim2")]
@@ -173,11 +183,11 @@ impl EntityWithGraphics {
     pub fn update(
         &mut self,
         colliders: &ColliderSet,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         gfx_shift: &Vector<Real>,
     ) {
         if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
-            if let Ok(mut pos) = components.get_component_mut::<Transform>(self.entity) {
+            if let Ok(mut pos) = components.get_mut(self.entity) {
                 let co_pos = co.position() * self.delta;
                 pos.translation.x = (co_pos.translation.vector.x + gfx_shift.x) as f32;
                 pos.translation.y = (co_pos.translation.vector.y + gfx_shift.y) as f32;
@@ -230,18 +240,14 @@ impl EntityWithGraphics {
         //
         // Cuboid mesh
         //
-        let cuboid = Mesh::from(shape::Cube { size: 2.0 });
+        let cuboid = Mesh::from(bevy::math::primitives::Cuboid::new(2.0, 2.0, 2.0));
         out.insert(ShapeType::Cuboid, meshes.add(cuboid.clone()));
         out.insert(ShapeType::RoundCuboid, meshes.add(cuboid));
 
         //
         // Ball mesh
         //
-        let ball = Mesh::try_from(shape::Icosphere {
-            subdivisions: 2,
-            radius: 1.0,
-        })
-        .unwrap();
+        let ball = Mesh::from(bevy::math::primitives::Sphere::new(1.0));
         out.insert(ShapeType::Ball, meshes.add(ball));
 
         //
@@ -309,14 +315,14 @@ fn bevy_polyline(buffers: (Vec<Point2<Real>>, Option<Vec<[u32; 2]>>)) -> Mesh {
     let normals: Vec<_> = (0..vertices.len()).map(|_| [0.0, 0.0, 1.0]).collect();
 
     // Generate the mesh
-    let mut mesh = Mesh::new(PrimitiveTopology::LineStrip);
+    let mut mesh = Mesh::new(PrimitiveTopology::LineStrip, Default::default());
     mesh.insert_attribute(
         Mesh::ATTRIBUTE_POSITION,
         VertexAttributeValues::from(vertices),
     );
     mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, VertexAttributeValues::from(normals));
     mesh.insert_attribute(Mesh::ATTRIBUTE_UV_0, VertexAttributeValues::from(uvs));
-    mesh.set_indices(Some(Indices::U32(indices)));
+    mesh.insert_indices(Indices::U32(indices));
     mesh
 }
 
@@ -352,14 +358,14 @@ fn bevy_mesh(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
     let uvs: Vec<_> = (0..vertices.len()).map(|_| [0.0, 0.0]).collect();
 
     // Generate the mesh
-    let mut mesh = Mesh::new(PrimitiveTopology::TriangleList);
+    let mut mesh = Mesh::new(PrimitiveTopology::TriangleList, Default::default());
     mesh.insert_attribute(
         Mesh::ATTRIBUTE_POSITION,
         VertexAttributeValues::from(vertices),
     );
     mesh.insert_attribute(Mesh::ATTRIBUTE_NORMAL, VertexAttributeValues::from(normals));
     mesh.insert_attribute(Mesh::ATTRIBUTE_UV_0, VertexAttributeValues::from(uvs));
-    mesh.set_indices(Some(Indices::U32(indices)));
+    mesh.insert_indices(Indices::U32(indices));
     mesh
 }
 
diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs
index 26a55c7..3c69f6e 100644
--- a/src_testbed/physics/mod.rs
+++ b/src_testbed/physics/mod.rs
@@ -3,7 +3,9 @@ use rapier::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
     RigidBodySet,
 };
-use rapier::geometry::{BroadPhase, ColliderSet, CollisionEvent, ContactForceEvent, NarrowPhase};
+use rapier::geometry::{
+    ColliderSet, CollisionEvent, ContactForceEvent, DefaultBroadPhase, NarrowPhase,
+};
 use rapier::math::{Real, Vector};
 use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
@@ -14,65 +16,79 @@ pub struct PhysicsSnapshot {
     bodies: Vec<u8>,
     colliders: Vec<u8>,
     impulse_joints: Vec<u8>,
+    multibody_joints: Vec<u8>,
+    island_manager: Vec<u8>,
+}
+
+pub struct DeserializedPhysicsSnapshot {
+    pub timestep_id: usize,
+    pub broad_phase: DefaultBroadPhase,
+    pub narrow_phase: NarrowPhase,
+    pub island_manager: IslandManager,
+    pub bodies: RigidBodySet,
+    pub colliders: ColliderSet,
+    pub impulse_joints: ImpulseJointSet,
+    pub multibody_joints: MultibodyJointSet,
 }
 
 impl PhysicsSnapshot {
     pub fn new(
         timestep_id: usize,
-        broad_phase: &BroadPhase,
+        broad_phase: &DefaultBroadPhase,
         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)?,
         })
     }
 
-    pub fn restore(
-        &self,
-    ) -> bincode::Result<(
-        usize,
-        BroadPhase,
-        NarrowPhase,
-        RigidBodySet,
-        ColliderSet,
-        ImpulseJointSet,
-    )> {
-        Ok((
-            self.timestep_id,
-            bincode::deserialize(&self.broad_phase)?,
-            bincode::deserialize(&self.narrow_phase)?,
-            bincode::deserialize(&self.bodies)?,
-            bincode::deserialize(&self.colliders)?,
-            bincode::deserialize(&self.impulse_joints)?,
-        ))
+    pub fn restore(&self) -> bincode::Result<DeserializedPhysicsSnapshot> {
+        Ok(DeserializedPhysicsSnapshot {
+            timestep_id: self.timestep_id,
+            broad_phase: bincode::deserialize(&self.broad_phase)?,
+            narrow_phase: bincode::deserialize(&self.narrow_phase)?,
+            island_manager: bincode::deserialize(&self.island_manager)?,
+            bodies: bincode::deserialize(&self.bodies)?,
+            colliders: bincode::deserialize(&self.colliders)?,
+            impulse_joints: bincode::deserialize(&self.impulse_joints)?,
+            multibody_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());
     }
 }
 
 pub struct PhysicsState {
     pub islands: IslandManager,
-    pub broad_phase: BroadPhase,
+    pub broad_phase: DefaultBroadPhase,
     pub narrow_phase: NarrowPhase,
     pub bodies: RigidBodySet,
     pub colliders: ColliderSet,
@@ -96,7 +112,7 @@ impl PhysicsState {
     pub fn new() -> Self {
         Self {
             islands: IslandManager::new(),
-            broad_phase: BroadPhase::new(),
+            broad_phase: DefaultBroadPhase::new(),
             narrow_phase: NarrowPhase::new(),
             bodies: RigidBodySet::new(),
             colliders: ColliderSet::new(),
diff --git a/src_testbed/plugin.rs b/src_testbed/plugin.rs
index 7f19aa9..42fdd17 100644
--- a/src_testbed/plugin.rs
+++ b/src_testbed/plugin.rs
@@ -14,7 +14,7 @@ pub trait TestbedPlugin {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         harness: &mut Harness,
     );
     fn clear_graphics(&mut self, graphics: &mut GraphicsManager, commands: &mut Commands);
@@ -26,7 +26,7 @@ pub trait TestbedPlugin {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
         harness: &mut Harness,
     );
     fn update_ui(
@@ -37,7 +37,7 @@ pub trait TestbedPlugin {
         commands: &mut Commands,
         meshes: &mut Assets<Mesh>,
         materials: &mut Assets<BevyMaterial>,
-        components: &mut Query<(&mut Transform,)>,
+        components: &mut Query<&mut Transform>,
     );
     fn profiling_string(&self) -> String;
 }
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index e5a5f13..836fc1f 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -1,4 +1,4 @@
-#![allow(clippy::bad_bit_mask)] // otherwsie clippy complains because of TestbedStateFlags::NONE which is 0.
+#![allow(clippy::bad_bit_mask)] // otherwise clippy complains because of TestbedStateFlags::NONE which is 0.
 
 use std::env;
 use std::mem;
@@ -7,10 +7,10 @@ use std::num::NonZeroUsize;
 use bevy::prelude::*;
 
 use crate::debug_render::{DebugRenderPipelineResource, RapierDebugRenderPlugin};
-use crate::physics::{PhysicsEvents, PhysicsSnapshot, PhysicsState};
+use crate::physics::{DeserializedPhysicsSnapshot, PhysicsEvents, PhysicsSnapshot, PhysicsState};
 use crate::plugin::TestbedPlugin;
-use crate::ui;
 use crate::{graphics::GraphicsManager, harness::RunState};
+use crate::{mouse, ui};
 
 use na::{self, Point2, Point3, Vector3};
 #[cfg(feature = "dim3")]
@@ -24,15 +24,14 @@ 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;
 use crate::harness::Harness;
 #[cfg(all(feature = "dim3", feature = "other-backends"))]
 use crate::physx_backend::PhysxWorld;
-use bevy::render::camera::Camera;
-use bevy_core_pipeline::prelude::ClearColor;
+use bevy::render::camera::{Camera, ClearColor};
 use bevy_egui::EguiContexts;
 use bevy_pbr::wireframe::WireframePlugin;
 use bevy_pbr::AmbientLight;
@@ -103,10 +102,12 @@ bitflags! {
     }
 }
 
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)]
 pub enum RapierSolverType {
-    SmallStepsPgs,
-    StandardPgs,
+    #[default]
+    TgsSoft,
+    TgsSoftNoWarmstart,
+    PgsLegacy,
 }
 
 pub type SimulationBuilders = Vec<(&'static str, fn(&mut Testbed))>;
@@ -134,7 +135,7 @@ pub struct TestbedState {
     pub solver_type: RapierSolverType,
     pub physx_use_two_friction_directions: bool,
     pub snapshot: Option<PhysicsSnapshot>,
-    nsteps: usize,
+    pub nsteps: usize,
     camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button.
 }
 
@@ -155,11 +156,12 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
     commands: &'a mut Commands<'d, 'e>,
     meshes: &'a mut Assets<Mesh>,
     materials: &'a mut Assets<BevyMaterial>,
-    components: &'a mut Query<'b, 'f, (&'c mut Transform,)>,
+    components: &'a mut Query<'b, 'f, &'c mut Transform>,
     #[allow(dead_code)] // Dead in 2D but not in 3D.
     camera_transform: GlobalTransform,
     camera: &'a mut OrbitCamera,
-    keys: &'a Input<KeyCode>,
+    keys: &'a ButtonInput<KeyCode>,
+    mouse: &'a SceneMouse,
 }
 
 pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -215,7 +217,7 @@ impl TestbedApp {
             example_names: Vec::new(),
             selected_example: 0,
             selected_backend: RAPIER_BACKEND,
-            solver_type: RapierSolverType::SmallStepsPgs,
+            solver_type: RapierSolverType::default(),
             physx_use_two_friction_directions: true,
             nsteps: 1,
             camera_locked: false,
@@ -399,6 +401,7 @@ impl TestbedApp {
                     brightness: 0.3,
                     ..Default::default()
                 })
+                .init_resource::<mouse::SceneMouse>()
                 .add_plugins(DefaultPlugins.set(window_plugin))
                 .add_plugins(OrbitCameraPlugin)
                 .add_plugins(WireframePlugin)
@@ -418,7 +421,9 @@ impl TestbedApp {
                 .insert_resource(self.builders)
                 .insert_non_send_resource(self.plugins)
                 .add_systems(Update, update_testbed)
-                .add_systems(Update, egui_focus);
+                .add_systems(Update, egui_focus)
+                .add_systems(Update, track_mouse_state);
+
             init(&mut app);
             app.run();
         }
@@ -468,9 +473,18 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
         )
     }
 
-    pub fn keys(&self) -> &Input<KeyCode> {
+    pub fn keys(&self) -> &ButtonInput<KeyCode> {
         self.keys
     }
+
+    pub fn mouse(&self) -> &SceneMouse {
+        self.mouse
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn camera_fwd_dir(&self) -> Vector<f32> {
+        (self.camera_transform * -Vec3::Z).normalize().into()
+    }
 }
 
 impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -660,7 +674,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
     }
 
     #[cfg(feature = "dim3")]
-    fn update_vehicle_controller(&mut self, events: &Input<KeyCode>) {
+    fn update_vehicle_controller(&mut self, events: &ButtonInput<KeyCode>) {
         if self.state.running == RunMode::Stop {
             return;
         }
@@ -671,16 +685,16 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
 
             for key in events.get_pressed() {
                 match *key {
-                    KeyCode::Right => {
+                    KeyCode::ArrowRight => {
                         steering_angle += -0.7;
                     }
-                    KeyCode::Left => {
+                    KeyCode::ArrowLeft => {
                         steering_angle += 0.7;
                     }
-                    KeyCode::Up => {
+                    KeyCode::ArrowUp => {
                         engine_force += 30.0;
                     }
-                    KeyCode::Down => {
+                    KeyCode::ArrowDown => {
                         engine_force += -30.0;
                     }
                     _ => {}
@@ -703,7 +717,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         }
     }
 
-    fn update_character_controller(&mut self, events: &Input<KeyCode>) {
+    fn update_character_controller(&mut self, events: &ButtonInput<KeyCode>) {
         if self.state.running == RunMode::Stop {
             return;
         }
@@ -715,10 +729,10 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
             #[cfg(feature = "dim2")]
             for key in events.get_pressed() {
                 match *key {
-                    KeyCode::Right => {
+                    KeyCode::ArrowRight => {
                         desired_movement += Vector::x();
                     }
-                    KeyCode::Left => {
+                    KeyCode::ArrowLeft => {
                         desired_movement -= Vector::x();
                     }
                     KeyCode::Space => {
@@ -750,16 +764,16 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
 
                 for key in events.get_pressed() {
                     match *key {
-                        KeyCode::Right => {
+                        KeyCode::ArrowRight => {
                             desired_movement += rot_x;
                         }
-                        KeyCode::Left => {
+                        KeyCode::ArrowLeft => {
                             desired_movement -= rot_x;
                         }
-                        KeyCode::Up => {
+                        KeyCode::ArrowUp => {
                             desired_movement -= rot_z;
                         }
-                        KeyCode::Down => {
+                        KeyCode::ArrowDown => {
                             desired_movement += rot_z;
                         }
                         KeyCode::Space => {
@@ -818,22 +832,22 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         }
     }
 
-    fn handle_common_events(&mut self, events: &Input<KeyCode>) {
+    fn handle_common_events(&mut self, events: &ButtonInput<KeyCode>) {
         for key in events.get_just_released() {
             match *key {
-                KeyCode::T => {
+                KeyCode::KeyT => {
                     if self.state.running == RunMode::Stop {
                         self.state.running = RunMode::Running;
                     } else {
                         self.state.running = RunMode::Stop;
                     }
                 }
-                KeyCode::S => self.state.running = RunMode::Step,
-                KeyCode::R => self
+                KeyCode::KeyS => self.state.running = RunMode::Step,
+                KeyCode::KeyR => self
                     .state
                     .action_flags
                     .set(TestbedActionFlags::EXAMPLE_CHANGED, true),
-                KeyCode::C => {
+                KeyCode::KeyC => {
                     // Delete 1 collider of 10% of the remaining dynamic bodies.
                     let mut colliders: Vec<_> = self
                         .harness
@@ -859,7 +873,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
                         );
                     }
                 }
-                KeyCode::D => {
+                KeyCode::KeyD => {
                     // Delete 10% of the remaining dynamic bodies.
                     let dynamic_bodies: Vec<_> = self
                         .harness
@@ -884,7 +898,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
                         );
                     }
                 }
-                KeyCode::J => {
+                KeyCode::KeyJ => {
                     // Delete 10% of the remaining impulse_joints.
                     let impulse_joi