Add f64 testbed

9 files changed, 313 insertions, 11 deletions

diff --git a/Cargo.toml b/Cargo.toml
index 1bd8c67..543850e 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,6 @@
 [workspace]
-members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "examples2d", "benchmarks2d",
-            "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "examples3d", "benchmarks3d" ]
+members = [ "crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d",
+            "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d" ]
 resolver = "2"
 
 [patch.crates-io]
diff --git a/crates/rapier_testbed2d-f64/Cargo.toml b/crates/rapier_testbed2d-f64/Cargo.toml
new file mode 100644
index 0000000..cfa0f42
--- /dev/null
+++ b/crates/rapier_testbed2d-f64/Cargo.toml
@@ -0,0 +1,57 @@
+[package]
+name    = "rapier_testbed2d-f64"
+version = "0.12.0-alpha.1"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "Testbed for the Rapier 2-dimensional physics engine in Rust."
+homepage = "http://rapier.org"
+repository = "https://github.com/dimforge/rapier"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+license = "Apache-2.0"
+edition = "2021"
+
+[badges]
+maintenance = { status = "actively-developed" }
+
+[lib]
+name = "rapier_testbed2d"
+path = "../../src_testbed/lib.rs"
+required-features = [ "dim2" ]
+
+[features]
+default = [ "dim2" ]
+dim2 = [ ]
+parallel = [ "rapier/parallel", "num_cpus" ]
+other-backends = [ "wrapped2d" ]
+
+
+[dependencies]
+nalgebra   = { version = "0.30", features = [ "rand" ] }
+rand       = "0.8"
+rand_pcg   = "0.3"
+instant    = { version = "0.1", features = [ "web-sys", "now" ]}
+bitflags   = "1"
+num_cpus   = { version = "1", optional = true }
+wrapped2d  = { version = "0.4", optional = true }
+crossbeam = "0.8"
+bincode = "1"
+Inflector  = "0.11"
+md5 = "0.7"
+
+bevy_egui = "0.10"
+bevy_ecs = "0.6"
+
+# Dependencies for native only.
+[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
+bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render", "x11"]}
+
+# Dependencies for WASM only.
+[target.'cfg(target_arch = "wasm32")'.dependencies]
+bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
+#bevy_webgl2 = "0.5"
+
+[dependencies.rapier]
+package = "rapier2d-f64"
+path = "../rapier2d-f64"
+version = "0.12.0-alpha.1"
+features = [ "serde-serialize" ]
diff --git a/crates/rapier_testbed3d-f64/Cargo.toml b/crates/rapier_testbed3d-f64/Cargo.toml
new file mode 100644
index 0000000..dfd0fbd
--- /dev/null
+++ b/crates/rapier_testbed3d-f64/Cargo.toml
@@ -0,0 +1,55 @@
+[package]
+name    = "rapier_testbed3d-f64"
+version = "0.12.0-alpha.1"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+description = "Testbed for the Rapier 3-dimensional physics engine in Rust."
+homepage = "http://rapier.org"
+repository = "https://github.com/dimforge/rapier"
+categories = [ "science", "game-development", "mathematics", "simulation", "wasm"]
+keywords = [ "physics", "dynamics", "rigid", "real-time", "impulse_joints" ]
+license = "Apache-2.0"
+edition = "2021"
+
+[badges]
+maintenance = { status = "actively-developed" }
+
+[lib]
+name = "rapier_testbed3d"
+path = "../../src_testbed/lib.rs"
+required-features = [ "dim3" ]
+
+[features]
+default = [ "dim3" ]
+dim3 = [ ]
+parallel = [ "rapier/parallel", "num_cpus" ]
+
+[dependencies]
+nalgebra   = { version = "0.30", features = [ "rand" ] }
+rand       = "0.8"
+rand_pcg   = "0.3"
+instant    = { version = "0.1", features = [ "web-sys", "now" ]}
+bitflags   = "1"
+num_cpus   = { version = "1", optional = true }
+crossbeam = "0.8"
+bincode = "1"
+md5 = "0.7"
+Inflector  = "0.11"
+serde = { version = "1", features = [ "derive" ] }
+
+bevy_egui = "0.10"
+bevy_ecs = "0.6"
+
+# Dependencies for native only.
+[target.'cfg(not(target_arch = "wasm32"))'.dependencies]
+bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render", "x11"]}
+
+# Dependencies for WASM only.
+[target.'cfg(target_arch = "wasm32")'.dependencies]
+bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
+#bevy_webgl2 = "0.5"
+
+[dependencies.rapier]
+package = "rapier3d-f64"
+path = "../rapier3d-f64"
+version = "0.12.0-alpha.1"
+features = [ "serde-serialize" ]
\ No newline at end of file
diff --git a/examples3d-f64/Cargo.toml b/examples3d-f64/Cargo.toml
new file mode 100644
index 0000000..27a4491
--- /dev/null
+++ b/examples3d-f64/Cargo.toml
@@ -0,0 +1,36 @@
+[package]
+name    = "rapier-examples-3d-f64"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+edition = "2021"
+default-run = "all_examples3-f64"
+
+[features]
+parallel = [ "rapier3d-f64/parallel", "rapier_testbed3d-f64/parallel" ]
+simd-stable = [ "rapier3d-f64/simd-stable" ]
+simd-nightly = [ "rapier3d-f64/simd-nightly" ]
+enhanced-determinism = [ "rapier3d-f64/enhanced-determinism" ]
+
+[dependencies]
+rand = "0.8"
+getrandom = { version = "0.2", features = [ "js" ] } 
+Inflector  = "0.11"
+wasm-bindgen = "0.2"
+obj-rs = { version =  "0.6", default-features = false }
+bincode = "1"
+serde = "1"
+
+[dependencies.rapier_testbed3d-f64]
+path = "../crates/rapier_testbed3d-f64"
+
+[dependencies.rapier3d-f64]
+path = "../crates/rapier3d-f64"
+
+[[bin]]
+name = "all_examples3-f64"
+path = "./all_examples3-f64.rs"
+
+#[lib]
+#crate-type = ["cdylib", "rlib"]
+#path = "./all_examples3_wasm.rs"
+
diff --git a/examples3d-f64/all_examples3-f64.rs b/examples3d-f64/all_examples3-f64.rs
new file mode 100644
index 0000000..4464d60
--- /dev/null
+++ b/examples3d-f64/all_examples3-f64.rs
@@ -0,0 +1,67 @@
+#![allow(dead_code)]
+
+#[cfg(target_arch = "wasm32")]
+use wasm_bindgen::prelude::*;
+extern crate rapier3d_f64 as rapier3d;
+
+use inflector::Inflector;
+
+use rapier_testbed3d::{Testbed, TestbedApp};
+use std::cmp::Ordering;
+
+mod debug_serialized3;
+
+fn demo_name_from_command_line() -> Option<String> {
+    let mut args = std::env::args();
+
+    while let Some(arg) = args.next() {
+        if &arg[..] == "--example" {
+            return args.next();
+        }
+    }
+
+    None
+}
+
+#[cfg(any(target_arch = "wasm32", target_arch = "asmjs"))]
+fn demo_name_from_url() -> Option<String> {
+    None
+    //    let window = stdweb::web::window();
+    //    let hash = window.location()?.search().ok()?;
+    //    if hash.len() > 0 {
+    //        Some(hash[1..].to_string())
+    //    } else {
+    //        None
+    //    }
+}
+
+#[cfg(not(any(target_arch = "wasm32", target_arch = "asmjs")))]
+fn demo_name_from_url() -> Option<String> {
+    None
+}
+
+#[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))]
+pub fn main() {
+    let demo = demo_name_from_command_line()
+        .or_else(|| demo_name_from_url())
+        .unwrap_or(String::new())
+        .to_camel_case();
+
+    let mut builders: Vec<(_, fn(&mut Testbed))> =
+        vec![("(Debug) serialized", debug_serialized3::init_world)];
+
+    // Lexicographic sort, with stress tests moved at the end of the list.
+    builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) {
+        (true, true) | (false, false) => a.0.cmp(b.0),
+        (true, false) => Ordering::Greater,
+        (false, true) => Ordering::Less,
+    });
+
+    let i = builders
+        .iter()
+        .position(|builder| builder.0.to_camel_case().as_str() == demo.as_str())
+        .unwrap_or(0);
+
+    let testbed = TestbedApp::from_builders(i, builders);
+    testbed.run()
+}
diff --git a/examples3d-f64/debug_serialized3.rs b/examples3d-f64/debug_serialized3.rs
new file mode 100644
index 0000000..5d4ae19
--- /dev/null
+++ b/examples3d-f64/debug_serialized3.rs
@@ -0,0 +1,72 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+#[derive(serde::Deserialize)]
+struct State {
+    pub islands: IslandManager,
+    pub broad_phase: BroadPhase,
+    pub narrow_phase: NarrowPhase,
+    pub bodies: RigidBodySet,
+    pub colliders: ColliderSet,
+    pub impulse_joints: ImpulseJointSet,
+    pub multibody_joints: MultibodyJointSet,
+    pub ccd_solver: CCDSolver,
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * Set up the testbed.
+     */
+    let bytes = std::fs::read("state.bin").unwrap();
+    let mut state: State = bincode::deserialize(&bytes).unwrap();
+
+    for body in state.bodies.iter_mut() {
+        dbg!(body.1.position());
+        dbg!(body.1.is_ccd_enabled());
+        dbg!(body.1.is_sleeping());
+        // dbg!(body.1);
+        body.1.clear_forces(false);
+    }
+
+    let mut to_remove = vec![];
+    for (_, co) in state.colliders.iter() {
+        if co.shape().as_ball().is_none() {
+            if let Some(parent) = co.parent() {
+                let body = &state.bodies[parent];
+                if body.is_dynamic() {
+                    to_remove.push(parent);
+                }
+            }
+        }
+    }
+
+    // for h in to_remove {
+    //     state.bodies.remove(
+    //         h,
+    //         &mut state.islands,
+    //         &mut state.colliders,
+    //         &mut state.impulse_joints,
+    //         &mut state.multibody_joints,
+    //     );
+    // }
+
+    testbed.set_world(
+        state.bodies,
+        state.colliders,
+        state.impulse_joints,
+        state.multibody_joints,
+    );
+    testbed.harness_mut().physics.islands = state.islands;
+    testbed.harness_mut().physics.broad_phase = state.broad_phase;
+    testbed.harness_mut().physics.narrow_phase = state.narrow_phase;
+    testbed.harness_mut().physics.ccd_solver = state.ccd_solver;
+    // testbed.harness_mut().physics.integration_parameters.erp = 0.0;
+    // testbed
+    //     .harness_mut()
+    //     .physics
+    //     .integration_parameters
+    //     .delassus_inv_factor = 1.0;
+
+    testbed.set_graphics_shift(vector![-541.0, -6377257.0, -61.0]);
+    testbed.look_at(point![10.0, 10.0, 10.0], point![0.0, 0.0, 0.0]);
+}
diff --git a/src_testbed/graphics.rs b/src_testbed/graphics.rs
index d0dcf2d..8bf787d 100644
--- a/src_testbed/graphics.rs
+++ b/src_testbed/graphics.rs
@@ -5,7 +5,7 @@ use na::{point, Point3};
 use crate::objects::node::EntityWithGraphics;
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
 use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
-use rapier::math::{Isometry, Real};
+use rapier::math::{Isometry, Real, Vector};
 //use crate::objects::capsule::Capsule;
 //#[cfg(feature = "dim3")]
 //use crate::objects::mesh::Mesh;
@@ -30,6 +30,7 @@ pub struct GraphicsManager {
     b2wireframe: HashMap<RigidBodyHandle, bool>,
     ground_color: Point3<f32>,
     prefab_meshes: HashMap<ShapeType, Handle<Mesh>>,
+    pub gfx_shift: Vector<Real>,
 }
 
 impl GraphicsManager {
@@ -42,6 +43,7 @@ impl GraphicsManager {
             ground_color: point![0.5, 0.5, 0.5],
             b2wireframe: HashMap::new(),
             prefab_meshes: HashMap::new(),
+            gfx_shift: Vector::zeros(),
         }
     }
 
@@ -239,7 +241,7 @@ impl GraphicsManager {
 
         new_nodes
             .iter_mut()
-            .for_each(|n| n.update(colliders, components));
+            .for_each(|n| n.update(colliders, components, &self.gfx_shift));
 
         // for node in new_nodes.iter_mut().filter_map(|n| n.scene_node_mut()) {
         //     if self.b2wireframe.get(&handle).cloned() == Some(true) {
@@ -368,7 +370,7 @@ impl GraphicsManager {
                 //     }
                 // }
 
-                n.update(colliders, components);
+                n.update(colliders, components, &self.gfx_shift);
             }
         }
     }
diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs
index 45a77db..6a4807f 100644
--- a/src_testbed/objects/node.rs
+++ b/src_testbed/objects/node.rs
@@ -10,7 +10,7 @@ use bevy::render::render_resource::PrimitiveTopology;
 use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
 #[cfg(feature = "dim3")]
 use rapier::geometry::{Cone, Cylinder};
-use rapier::math::{Isometry, Real};
+use rapier::math::{Isometry, Real, Vector};
 
 use crate::graphics::BevyMaterial;
 #[cfg(feature = "dim2")]
@@ -168,15 +168,20 @@ impl EntityWithGraphics {
         self.base_color = color;
     }
 
-    pub fn update(&mut self, colliders: &ColliderSet, components: &mut Query<(&mut Transform,)>) {
+    pub fn update(
+        &mut self,
+        colliders: &ColliderSet,
+        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) {
                 let co_pos = co.position() * self.delta;
-                pos.translation.x = co_pos.translation.vector.x as f32;
-                pos.translation.y = co_pos.translation.vector.y as f32;
+                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;
                 #[cfg(feature = "dim3")]
                 {
-                    pos.translation.z = co_pos.translation.vector.z as f32;
+                    pos.translation.z = (co_pos.translation.vector.z + gfx_shift.z) as f32;
                     pos.rotation = Quat::from_xyzw(
                         co_pos.rotation.i as f32,
                         co_pos.rotation.j as f32,
diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs
index 75eb50b..dba40fd 100644
--- a/src_testbed/testbed.rs
+++ b/src_testbed/testbed.rs
@@ -362,7 +362,7 @@ impl TestbedApp {
                 vsync: true,
                 ..Default::default()
             })
-            .insert_resource(ClearColor(Color::rgb(0.85, 0.85, 0.85)))
+            .insert_resource(ClearColor(Color::rgb(0.15, 0.15, 0.15)))
             .insert_resource(Msaa { samples: 4 })
             .insert_resource(WgpuOptions {
                 // Required for wireframes.
@@ -536,6 +536,14 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         }
     }
 
+    pub fn set_graphics_shift(&mut self, shift: Vector<Real>) {
+        if !self.state.camera_locked {
+            if let Some(graphics) = &mut self.graphics {
+                graphics.graphics.gfx_shift = shift;
+            }
+        }
+    }
+
     #[cfg(feature = "dim2")]
     pub fn look_at(&mut self, at: Point2<f32>, zoom: f32) {
         if !self.state.camera_locked {