From 31e7d95ff9b4a9accad3035f1c529456e93827fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Jan 2022 16:48:24 +0100 Subject: [PATCH 1/7] Add f64 testbed --- Cargo.toml | 4 +- crates/rapier_testbed2d-f64/Cargo.toml | 57 ++++++++++++++++++++ crates/rapier_testbed3d-f64/Cargo.toml | 55 ++++++++++++++++++++ examples3d-f64/Cargo.toml | 36 +++++++++++++ examples3d-f64/all_examples3-f64.rs | 67 ++++++++++++++++++++++++ examples3d-f64/debug_serialized3.rs | 72 ++++++++++++++++++++++++++ src_testbed/graphics.rs | 8 +-- src_testbed/objects/node.rs | 15 ++++-- src_testbed/testbed.rs | 10 +++- 9 files changed, 313 insertions(+), 11 deletions(-) create mode 100644 crates/rapier_testbed2d-f64/Cargo.toml create mode 100644 crates/rapier_testbed3d-f64/Cargo.toml create mode 100644 examples3d-f64/Cargo.toml create mode 100644 examples3d-f64/all_examples3-f64.rs create mode 100644 examples3d-f64/debug_serialized3.rs 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 " ] +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 " ] +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 " ] +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 { + 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 { + 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 { + 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, ground_color: Point3, prefab_meshes: HashMap>, + pub gfx_shift: Vector, } 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, + ) { if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) { if let Ok(mut pos) = components.get_component_mut::(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) { + 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, zoom: f32) { if !self.state.camera_locked { From e6af3384e1bd32045c217135192a37a60fabf71a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Jan 2022 16:48:57 +0100 Subject: [PATCH 2/7] Move convex decomposition example models to the assets folder. --- .../models => assets/3d}/camel_decimated.obj | 0 .../media/models => assets/3d}/chair.obj | 0 .../models => assets/3d}/cup_decimated.obj | 0 .../models => assets/3d}/dilo_decimated.obj | 0 .../models => assets/3d}/feline_decimated.obj | 0 .../models => assets/3d}/genus3_decimated.obj | 0 .../models => assets/3d}/hand2_decimated.obj | 0 .../models => assets/3d}/hand_decimated.obj | 0 .../media/models => assets/3d}/hornbug.obj | 0 .../3d}/octopus_decimated.obj | 0 .../models => assets/3d}/rabbit_decimated.obj | 0 .../3d}/rust_logo_simplified.obj | 0 .../3d}/screwdriver_decimated.obj | 0 .../media/models => assets/3d}/table.obj | 0 .../models => assets/3d}/tstTorusModel.obj | 0 .../models => assets/3d}/tstTorusModel2.obj | 0 .../models => assets/3d}/tstTorusModel3.obj | 0 examples3d/convex_decomposition3.rs | 36 +++++++++---------- 18 files changed, 18 insertions(+), 18 deletions(-) rename {examples3d/media/models => assets/3d}/camel_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/chair.obj (100%) rename {examples3d/media/models => assets/3d}/cup_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/dilo_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/feline_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/genus3_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/hand2_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/hand_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/hornbug.obj (100%) rename {examples3d/media/models => assets/3d}/octopus_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/rabbit_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/rust_logo_simplified.obj (100%) rename {examples3d/media/models => assets/3d}/screwdriver_decimated.obj (100%) rename {examples3d/media/models => assets/3d}/table.obj (100%) rename {examples3d/media/models => assets/3d}/tstTorusModel.obj (100%) rename {examples3d/media/models => assets/3d}/tstTorusModel2.obj (100%) rename {examples3d/media/models => assets/3d}/tstTorusModel3.obj (100%) diff --git a/examples3d/media/models/camel_decimated.obj b/assets/3d/camel_decimated.obj similarity index 100% rename from examples3d/media/models/camel_decimated.obj rename to assets/3d/camel_decimated.obj diff --git a/examples3d/media/models/chair.obj b/assets/3d/chair.obj similarity index 100% rename from examples3d/media/models/chair.obj rename to assets/3d/chair.obj diff --git a/examples3d/media/models/cup_decimated.obj b/assets/3d/cup_decimated.obj similarity index 100% rename from examples3d/media/models/cup_decimated.obj rename to assets/3d/cup_decimated.obj diff --git a/examples3d/media/models/dilo_decimated.obj b/assets/3d/dilo_decimated.obj similarity index 100% rename from examples3d/media/models/dilo_decimated.obj rename to assets/3d/dilo_decimated.obj diff --git a/examples3d/media/models/feline_decimated.obj b/assets/3d/feline_decimated.obj similarity index 100% rename from examples3d/media/models/feline_decimated.obj rename to assets/3d/feline_decimated.obj diff --git a/examples3d/media/models/genus3_decimated.obj b/assets/3d/genus3_decimated.obj similarity index 100% rename from examples3d/media/models/genus3_decimated.obj rename to assets/3d/genus3_decimated.obj diff --git a/examples3d/media/models/hand2_decimated.obj b/assets/3d/hand2_decimated.obj similarity index 100% rename from examples3d/media/models/hand2_decimated.obj rename to assets/3d/hand2_decimated.obj diff --git a/examples3d/media/models/hand_decimated.obj b/assets/3d/hand_decimated.obj similarity index 100% rename from examples3d/media/models/hand_decimated.obj rename to assets/3d/hand_decimated.obj diff --git a/examples3d/media/models/hornbug.obj b/assets/3d/hornbug.obj similarity index 100% rename from examples3d/media/models/hornbug.obj rename to assets/3d/hornbug.obj diff --git a/examples3d/media/models/octopus_decimated.obj b/assets/3d/octopus_decimated.obj similarity index 100% rename from examples3d/media/models/octopus_decimated.obj rename to assets/3d/octopus_decimated.obj diff --git a/examples3d/media/models/rabbit_decimated.obj b/assets/3d/rabbit_decimated.obj similarity index 100% rename from examples3d/media/models/rabbit_decimated.obj rename to assets/3d/rabbit_decimated.obj diff --git a/examples3d/media/models/rust_logo_simplified.obj b/assets/3d/rust_logo_simplified.obj similarity index 100% rename from examples3d/media/models/rust_logo_simplified.obj rename to assets/3d/rust_logo_simplified.obj diff --git a/examples3d/media/models/screwdriver_decimated.obj b/assets/3d/screwdriver_decimated.obj similarity index 100% rename from examples3d/media/models/screwdriver_decimated.obj rename to assets/3d/screwdriver_decimated.obj diff --git a/examples3d/media/models/table.obj b/assets/3d/table.obj similarity index 100% rename from examples3d/media/models/table.obj rename to assets/3d/table.obj diff --git a/examples3d/media/models/tstTorusModel.obj b/assets/3d/tstTorusModel.obj similarity index 100% rename from examples3d/media/models/tstTorusModel.obj rename to assets/3d/tstTorusModel.obj diff --git a/examples3d/media/models/tstTorusModel2.obj b/assets/3d/tstTorusModel2.obj similarity index 100% rename from examples3d/media/models/tstTorusModel2.obj rename to assets/3d/tstTorusModel2.obj diff --git a/examples3d/media/models/tstTorusModel3.obj b/assets/3d/tstTorusModel3.obj similarity index 100% rename from examples3d/media/models/tstTorusModel3.obj rename to assets/3d/tstTorusModel3.obj diff --git a/examples3d/convex_decomposition3.rs b/examples3d/convex_decomposition3.rs index 98cf4a9..6daf4d7 100644 --- a/examples3d/convex_decomposition3.rs +++ b/examples3d/convex_decomposition3.rs @@ -110,23 +110,23 @@ pub fn init_world(testbed: &mut Testbed) { fn models() -> Vec { vec![ - "media/models/camel_decimated.obj".to_string(), - "media/models/chair.obj".to_string(), - "media/models/cup_decimated.obj".to_string(), - "media/models/dilo_decimated.obj".to_string(), - "media/models/feline_decimated.obj".to_string(), - "media/models/genus3_decimated.obj".to_string(), - "media/models/hand2_decimated.obj".to_string(), - "media/models/hand_decimated.obj".to_string(), - "media/models/hornbug.obj".to_string(), - "media/models/octopus_decimated.obj".to_string(), - "media/models/rabbit_decimated.obj".to_string(), - // "media/models/rust_logo.obj".to_string(), - "media/models/rust_logo_simplified.obj".to_string(), - "media/models/screwdriver_decimated.obj".to_string(), - "media/models/table.obj".to_string(), - "media/models/tstTorusModel.obj".to_string(), - // "media/models/tstTorusModel2.obj".to_string(), - // "media/models/tstTorusModel3.obj".to_string(), + "assets/3d/camel_decimated.obj".to_string(), + "assets/3d/chair.obj".to_string(), + "assets/3d/cup_decimated.obj".to_string(), + "assets/3d/dilo_decimated.obj".to_string(), + "assets/3d/feline_decimated.obj".to_string(), + "assets/3d/genus3_decimated.obj".to_string(), + "assets/3d/hand2_decimated.obj".to_string(), + "assets/3d/hand_decimated.obj".to_string(), + "assets/3d/hornbug.obj".to_string(), + "assets/3d/octopus_decimated.obj".to_string(), + "assets/3d/rabbit_decimated.obj".to_string(), + // "assets/3d/rust_logo.obj".to_string(), + "assets/3d/rust_logo_simplified.obj".to_string(), + "assets/3d/screwdriver_decimated.obj".to_string(), + "assets/3d/table.obj".to_string(), + "assets/3d/tstTorusModel.obj".to_string(), + // "assets/3d/tstTorusModel2.obj".to_string(), + // "assets/3d/tstTorusModel3.obj".to_string(), ] } From b7bf80550d8cc61637a251aa2ba0e6cdb8d26b74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Jan 2022 16:49:20 +0100 Subject: [PATCH 3/7] Add vscode project files --- .vscode/launch.json | 326 ++++++++++++++++++++++++++++++++++++++++++++ .vscode/tasks.json | 75 ++++++++++ 2 files changed, 401 insertions(+) create mode 100644 .vscode/launch.json create mode 100644 .vscode/tasks.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..3f328c5 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,326 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in library 'rapier2d'", + "cargo": { + "args": [ + "test", + "--no-run", + "--lib", + "--package=rapier2d" + ], + "filter": { + "name": "rapier2d", + "kind": "lib" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in library 'rapier2d_f64'", + "cargo": { + "args": [ + "test", + "--no-run", + "--lib", + "--package=rapier2d-f64" + ], + "filter": { + "name": "rapier2d_f64", + "kind": "lib" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in library 'rapier_testbed2d'", + "cargo": { + "args": [ + "test", + "--no-run", + "--lib", + "--package=rapier_testbed2d" + ], + "filter": { + "name": "rapier_testbed2d", + "kind": "lib" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug executable 'all_examples2'", + "cargo": { + "args": [ + "build", + "--bin=all_examples2", + "--package=rapier-examples-2d" + ], + "filter": { + "name": "all_examples2", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in executable 'all_examples2'", + "cargo": { + "args": [ + "test", + "--no-run", + "--bin=all_examples2", + "--package=rapier-examples-2d" + ], + "filter": { + "name": "all_examples2", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug executable 'all_benchmarks2'", + "cargo": { + "args": [ + "build", + "--bin=all_benchmarks2", + "--package=rapier-benchmarks-2d" + ], + "filter": { + "name": "all_benchmarks2", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in executable 'all_benchmarks2'", + "cargo": { + "args": [ + "test", + "--no-run", + "--bin=all_benchmarks2", + "--package=rapier-benchmarks-2d" + ], + "filter": { + "name": "all_benchmarks2", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in library 'rapier3d'", + "cargo": { + "args": [ + "test", + "--no-run", + "--lib", + "--package=rapier3d" + ], + "filter": { + "name": "rapier3d", + "kind": "lib" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in library 'rapier3d_f64'", + "cargo": { + "args": [ + "test", + "--no-run", + "--lib", + "--package=rapier3d-f64" + ], + "filter": { + "name": "rapier3d_f64", + "kind": "lib" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in library 'rapier_testbed3d'", + "cargo": { + "args": [ + "test", + "--no-run", + "--lib", + "--package=rapier_testbed3d" + ], + "filter": { + "name": "rapier_testbed3d", + "kind": "lib" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug executable 'all_examples3'", + "cargo": { + "args": [ + "build", + "--bin=all_examples3", + "--package=rapier-examples-3d" + ], + "filter": { + "name": "all_examples3", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in executable 'all_examples3'", + "cargo": { + "args": [ + "test", + "--no-run", + "--bin=all_examples3", + "--package=rapier-examples-3d" + ], + "filter": { + "name": "all_examples3", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Run 'all_examples3'", + "cargo": { + "args": [ + "run", + "--release", + "--bin=all_examples3", + "--package=rapier-examples-3d" + ], + "filter": { + "name": "all_examples3", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug executable 'harness_capsules3'", + "cargo": { + "args": [ + "build", + "--bin=harness_capsules3", + "--package=rapier-examples-3d" + ], + "filter": { + "name": "harness_capsules3", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in executable 'harness_capsules3'", + "cargo": { + "args": [ + "test", + "--no-run", + "--bin=harness_capsules3", + "--package=rapier-examples-3d" + ], + "filter": { + "name": "harness_capsules3", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug executable 'all_benchmarks3'", + "cargo": { + "args": [ + "build", + "--bin=all_benchmarks3", + "--package=rapier-benchmarks-3d" + ], + "filter": { + "name": "all_benchmarks3", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + }, + { + "type": "lldb", + "request": "launch", + "name": "Debug unit tests in executable 'all_benchmarks3'", + "cargo": { + "args": [ + "test", + "--no-run", + "--bin=all_benchmarks3", + "--package=rapier-benchmarks-3d" + ], + "filter": { + "name": "all_benchmarks3", + "kind": "bin" + } + }, + "args": [], + "cwd": "${workspaceFolder}" + } + ] +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..de72ffb --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,75 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "run 3d (no-simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_examples3", + "--release", + "--features", + "other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "run 3d (simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_examples3", + "--release", + "--features", + "simd-stable", + "--features", + "other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "run 2d (no-simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_examples2", + "--release", + "--features", + "other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "run 2d (simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_examples2", + "--release", + "--features", + "simd-stable", + "--features", + "other-backends", + "--", + "--pause" + ], + "group": "build" + } + ] +} \ No newline at end of file From 78c8bc6cdef26d14c57d0eeb23188cba592961bf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Jan 2022 16:50:02 +0100 Subject: [PATCH 4/7] Improve cfm configuration using the critical damping factor --- .gitignore | 2 +- src/dynamics/integration_parameters.rs | 56 ++++-- .../solver/generic_velocity_constraint.rs | 5 + .../generic_velocity_constraint_element.rs | 8 +- .../generic_velocity_ground_constraint.rs | 2 + ...eric_velocity_ground_constraint_element.rs | 6 +- .../joint_generic_velocity_constraint.rs | 2 +- src/dynamics/solver/velocity_constraint.rs | 20 ++- .../solver/velocity_constraint_element.rs | 6 +- .../solver/velocity_constraint_wide.rs | 13 +- .../solver/velocity_ground_constraint.rs | 8 +- .../velocity_ground_constraint_element.rs | 16 +- .../solver/velocity_ground_constraint_wide.rs | 10 +- src/dynamics/solver/velocity_solver.rs | 164 ++++++++++-------- 14 files changed, 196 insertions(+), 122 deletions(-) diff --git a/.gitignore b/.gitignore index 055e3a4..db22dde 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,4 @@ target .DS_Store package-lock.json **/*.csv -.vscode/ +.history \ No newline at end of file diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 844db41..c1d3e26 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -29,13 +29,11 @@ pub struct IntegrationParameters { /// A good non-zero value is around `0.2`. /// (default `0.0`). pub erp: Real, - - /// 0-1: multiplier applied to each accumulated impulse during constraints resolution. - /// This is similar to the concept of CFN (Constraint Force Mixing) except that it is - /// a multiplicative factor instead of an additive factor. - /// Larger values lead to stiffer constraints (1.0 being completely stiff). - /// Smaller values lead to more compliant constraints. - pub delassus_inv_factor: Real, + /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization. + /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations + /// before stabilization). + /// (default `0.25`). + pub damping_ratio: Real, /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, @@ -89,10 +87,42 @@ impl IntegrationParameters { } } - /// Convenience: `erp / dt` - #[inline] - pub(crate) fn erp_inv_dt(&self) -> Real { - self.erp * self.inv_dt() + /// The ERP coefficient, multiplied by the inverse timestep length. + pub fn erp_inv_dt(&self) -> Real { + 0.8 / self.dt + } + + /// The CFM factor to be used in the constraints resolution. + pub fn cfm_factor(&self) -> Real { + // Compute CFM assuming a critically damped spring multiplied by the dampingratio. + let inv_erp_minus_one = 1.0 / self.erp - 1.0; + + // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass + // / (dt * dt * inv_erp_minus_one * inv_erp_minus_one); + // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass + // / (dt * inv_erp_minus_one); + // let cfm = 1.0 / (dt * dt * stiffness + dt * damping); + // NOTE: This simplies to cfm = cfm_coefff / projected_mass: + let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one + / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio); + + // Furthermore, we use this coefficient inside of the impulse resolution. + // Surprisingly, several simplifications happen there. + // Let `m` the projected mass of the constraint. + // Let `m’` the projected mass that includes CFM: `m’ = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)` + // We have: + // new_impulse = old_impulse - m’ (delta_vel - cfm * old_impulse) + // = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse) + // = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel + // = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff) + // = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel) + // So, setting cfm_factor = 1 / (1 + cfm_coeff). + // We obtain: + // new_impulse = cfm_factor * (old_impulse - m * delta_vel) + // + // The value returned by this function is this cfm_factor that can be used directly + // in the constraints solver. + 1.0 / (1.0 + cfm_coeff) } } @@ -103,14 +133,14 @@ impl Default for IntegrationParameters { min_ccd_dt: 1.0 / 60.0 / 100.0, velocity_solve_fraction: 1.0, erp: 0.8, - delassus_inv_factor: 0.75, + damping_ratio: 0.25, allowed_linear_error: 0.001, // 0.005 prediction_distance: 0.002, max_velocity_iterations: 4, max_velocity_friction_iterations: 8, max_stabilization_iterations: 1, interleave_restitution_and_friction_resolution: true, // Enabling this makes a big difference for 2D stability. - // FIXME: what is the optimal value for min_island_size? + // TODO: what is the optimal value for min_island_size? // It should not be too big so that we don't end up with // huge islands that don't fit in cache. // However we don't want it to be too small and end up with diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index f1ab0ea..c1d4134 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -23,6 +23,7 @@ pub(crate) enum AnyGenericVelocityConstraint { impl AnyGenericVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, jacobians: &DVector, mj_lambdas: &mut [DeltaVel], generic_mj_lambdas: &mut DVector, @@ -31,6 +32,7 @@ impl AnyGenericVelocityConstraint { ) { match self { AnyGenericVelocityConstraint::Nongrouped(c) => c.solve( + cfm_factor, jacobians, mj_lambdas, generic_mj_lambdas, @@ -38,6 +40,7 @@ impl AnyGenericVelocityConstraint { solve_friction, ), AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve( + cfm_factor, jacobians, generic_mj_lambdas, solve_restitution, @@ -379,6 +382,7 @@ impl GenericVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, jacobians: &DVector, mj_lambdas: &mut [DeltaVel], generic_mj_lambdas: &mut DVector, @@ -400,6 +404,7 @@ impl GenericVelocityConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityConstraintElement::generic_solve_group( + cfm_factor, elements, jacobians, &self.velocity_constraint.dir1, diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs index e75dd01..19fba43 100644 --- a/src/dynamics/solver/generic_velocity_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_constraint_element.rs @@ -243,6 +243,7 @@ impl VelocityConstraintNormalPart { #[inline] pub fn generic_solve( &mut self, + cfm_factor: Real, j_id: usize, jacobians: &DVector, dir1: &Vector, @@ -261,7 +262,7 @@ impl VelocityConstraintNormalPart { + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas) + self.rhs; - let new_impulse = (self.impulse - self.r * dvel).max(0.0); + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -291,6 +292,7 @@ impl VelocityConstraintNormalPart { impl VelocityConstraintElement { #[inline] pub fn generic_solve_group( + cfm_factor: Real, elements: &mut [Self], jacobians: &DVector, dir1: &Vector, @@ -318,8 +320,8 @@ impl VelocityConstraintElement { for element in elements.iter_mut() { element.normal_part.generic_solve( - nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, mj_lambda2, - mj_lambdas, + cfm_factor, nrm_j_id, jacobians, &dir1, im1, im2, ndofs1, ndofs2, mj_lambda1, + mj_lambda2, mj_lambdas, ); nrm_j_id += j_step; } diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index c9b2c3f..9ce824e 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -210,6 +210,7 @@ impl GenericVelocityGroundConstraint { pub fn solve( &mut self, + cfm_factor: Real, jacobians: &DVector, generic_mj_lambdas: &mut DVector, solve_restitution: bool, @@ -220,6 +221,7 @@ impl GenericVelocityGroundConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityGroundConstraintElement::generic_solve_group( + cfm_factor, elements, jacobians, self.velocity_constraint.limit, diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs index 80c97ab..f645f04 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs @@ -75,6 +75,7 @@ impl VelocityGroundConstraintNormalPart { #[inline] pub fn generic_solve( &mut self, + cfm_factor: Real, j_id2: usize, jacobians: &DVector, ndofs2: usize, @@ -86,7 +87,7 @@ impl VelocityGroundConstraintNormalPart { .dot(&mj_lambdas.rows(mj_lambda2, ndofs2)) + self.rhs; - let new_impulse = (self.impulse - self.r * dvel).max(0.0); + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -101,6 +102,7 @@ impl VelocityGroundConstraintNormalPart { impl VelocityGroundConstraintElement { #[inline] pub fn generic_solve_group( + cfm_factor: Real, elements: &mut [Self], jacobians: &DVector, limit: Real, @@ -121,7 +123,7 @@ impl VelocityGroundConstraintElement { for element in elements.iter_mut() { element .normal_part - .generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas); + .generic_solve(cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas); nrm_j_id += j_step; } } diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs index 2646fe8..0a75967 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs @@ -524,6 +524,6 @@ impl JointGenericVelocityGroundConstraint { } pub fn remove_bias_from_rhs(&mut self) { - self.rhs = self.rhs_wo_bias; + self.rhs = &mut self.rhs_wo_bias; } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 6a95492..bb00b66 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -55,23 +55,26 @@ impl AnyVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, ) { match self { AnyVelocityConstraint::NongroupedGround(c) => { - c.solve(mj_lambdas, solve_normal, solve_friction) + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) } AnyVelocityConstraint::Nongrouped(c) => { - c.solve(mj_lambdas, solve_normal, solve_friction) + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) } #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::GroupedGround(c) => { - c.solve(mj_lambdas, solve_normal, solve_friction) + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) } #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas, solve_normal, solve_friction), + AnyVelocityConstraint::Grouped(c) => { + c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction) + } AnyVelocityConstraint::Empty => unreachable!(), } } @@ -236,7 +239,7 @@ impl VelocityConstraint { .transform_vector(dp2.gcross(-force_dir1)); let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let r = params.delassus_inv_factor + let projected_mass = 1.0 / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); @@ -251,14 +254,13 @@ impl VelocityConstraint { let rhs_bias = /* is_resting * */ erp_inv_dt * (manifold_point.dist + params.allowed_linear_error).min(0.0); - constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, - impulse: 0.0, - r, + impulse: na::zero(), + r: projected_mass, }; } @@ -310,6 +312,7 @@ impl VelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -318,6 +321,7 @@ impl VelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityConstraintElement::solve_group( + cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index b0f8087..2d2221d 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -131,6 +131,7 @@ impl VelocityConstraintNormalPart { #[inline] pub fn solve( &mut self, + cfm_factor: N, dir1: &Vector, im1: &Vector, im2: &Vector, @@ -143,7 +144,7 @@ impl VelocityConstraintNormalPart { - dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; - let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero()); + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -171,6 +172,7 @@ impl VelocityConstraintElement { #[inline] pub fn solve_group( + cfm_factor: N, elements: &mut [Self], dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, @@ -191,7 +193,7 @@ impl VelocityConstraintElement { for element in elements.iter_mut() { element .normal_part - .solve(&dir1, im1, im2, mj_lambda1, mj_lambda2); + .solve(cfm_factor, &dir1, im1, im2, mj_lambda1, mj_lambda2); } } diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 7fcb7f4..44b91c6 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -48,9 +48,8 @@ impl WVelocityConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); - let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); - let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); + let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; @@ -121,7 +120,6 @@ impl WVelocityConstraint { let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); let tangent_velocity = Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); - let dp1 = point - world_com1; let dp2 = point - world_com2; @@ -137,10 +135,11 @@ impl WVelocityConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let imsum = im1 + im2; - let r = delassus_inv_factor + let projected_mass = SimdReal::splat(1.0) / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); + let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; @@ -154,8 +153,8 @@ impl WVelocityConstraint { gcross2, rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, - impulse: na::zero(), - r, + impulse: SimdReal::splat(0.0), + r: projected_mass, }; } @@ -202,6 +201,7 @@ impl WVelocityConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -221,6 +221,7 @@ impl WVelocityConstraint { }; VelocityConstraintElement::solve_group( + SimdReal::splat(cfm_factor), &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 76de3f9..cf7d9eb 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -153,7 +153,7 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let r = params.delassus_inv_factor + let projected_mass = 1.0 / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); @@ -172,8 +172,8 @@ impl VelocityGroundConstraint { gcross2, rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, - impulse: 0.0, - r, + impulse: na::zero(), + r: projected_mass, }; } @@ -219,6 +219,7 @@ impl VelocityGroundConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -226,6 +227,7 @@ impl VelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityGroundConstraintElement::solve_group( + cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs index a843905..8057030 100644 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -109,12 +109,17 @@ impl VelocityGroundConstraintNormalPart { } #[inline] - pub fn solve(&mut self, dir1: &Vector, im2: &Vector, mj_lambda2: &mut DeltaVel) - where + pub fn solve( + &mut self, + cfm_factor: N, + dir1: &Vector, + im2: &Vector, + mj_lambda2: &mut DeltaVel, + ) where AngVector: WDot, Result = N>, { let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; - let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero()); + let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; @@ -139,6 +144,7 @@ impl VelocityGroundConstraintElement { #[inline] pub fn solve_group( + cfm_factor: N, elements: &mut [Self], dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, @@ -155,7 +161,9 @@ impl VelocityGroundConstraintElement { // Solve penetration. if solve_normal { for element in elements.iter_mut() { - element.normal_part.solve(&dir1, im2, mj_lambda2); + element + .normal_part + .solve(cfm_factor, &dir1, im2, mj_lambda2); } } diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 4771469..65ac46e 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -43,9 +43,8 @@ impl WVelocityGroundConstraint { { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); - let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); - let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); + let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2]; @@ -143,8 +142,9 @@ impl WVelocityGroundConstraint { { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let r = delassus_inv_factor + let projected_mass = SimdReal::splat(1.0) / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); + let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; @@ -158,7 +158,7 @@ impl WVelocityGroundConstraint { rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, impulse: na::zero(), - r, + r: projected_mass, }; } @@ -199,6 +199,7 @@ impl WVelocityGroundConstraint { pub fn solve( &mut self, + cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -211,6 +212,7 @@ impl WVelocityGroundConstraint { }; VelocityGroundConstraintElement::solve_group( + SimdReal::splat(cfm_factor), &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 1cc43ac..47275ed 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -50,6 +50,7 @@ impl VelocitySolver { + ComponentSetMut + ComponentSet, { + let cfm_factor = params.cfm_factor(); self.mj_lambdas.clear(); self.mj_lambdas .resize(islands.active_island(island_id).len(), DeltaVel::zero()); @@ -93,18 +94,36 @@ impl VelocitySolver { } for constraint in &mut *contact_constraints { - constraint.solve(&mut self.mj_lambdas[..], true, solve_friction); + constraint.solve(cfm_factor, &mut self.mj_lambdas[..], true, false); } for constraint in &mut *generic_contact_constraints { constraint.solve( + cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, true, - solve_friction, + false, ); } + + if solve_friction { + for constraint in &mut *contact_constraints { + constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true); + } + + for constraint in &mut *generic_contact_constraints { + constraint.solve( + cfm_factor, + generic_contact_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + false, + true, + ); + } + } } let remaining_friction_iterations = @@ -118,11 +137,12 @@ impl VelocitySolver { for _ in 0..remaining_friction_iterations { for constraint in &mut *contact_constraints { - constraint.solve(&mut self.mj_lambdas[..], false, true); + constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true); } for constraint in &mut *generic_contact_constraints { constraint.solve( + cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -147,10 +167,7 @@ impl VelocitySolver { multibody.velocities += mj_lambdas; multibody.integrate(params.dt); multibody.forward_kinematics(bodies, false); - - if params.max_stabilization_iterations > 0 { - multibody.velocities = prev_vels; - } + multibody.velocities = prev_vels; } } else { let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = @@ -177,88 +194,85 @@ impl VelocitySolver { new_poss.next_position = new_vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com); bodies.set_internal(handle.0, new_poss); - - if params.max_stabilization_iterations == 0 { - bodies.set_internal(handle.0, new_vels); - } } } - if params.max_stabilization_iterations > 0 { - for joint in &mut *joint_constraints { - joint.remove_bias_from_rhs(); + for joint in &mut *joint_constraints { + joint.remove_bias_from_rhs(); + } + for constraint in &mut *contact_constraints { + constraint.remove_bias_from_rhs(); + } + for constraint in &mut *generic_contact_constraints { + constraint.remove_bias_from_rhs(); + } + + for _ in 0..params.max_stabilization_iterations { + for constraint in &mut *joint_constraints { + constraint.solve( + generic_joint_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + ); } + for constraint in &mut *contact_constraints { - constraint.remove_bias_from_rhs(); + constraint.solve(1.0, &mut self.mj_lambdas[..], true, false); } + for constraint in &mut *generic_contact_constraints { - constraint.remove_bias_from_rhs(); + constraint.solve( + 1.0, + generic_contact_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + true, + false, + ); } - for _ in 0..params.max_stabilization_iterations { - for constraint in &mut *joint_constraints { - constraint.solve( - generic_joint_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - ); - } - - for constraint in &mut *contact_constraints { - constraint.solve(&mut self.mj_lambdas[..], true, true); - } - - for constraint in &mut *generic_contact_constraints { - constraint.solve( - generic_contact_jacobians, - &mut self.mj_lambdas[..], - &mut self.generic_mj_lambdas, - true, - true, - ); - } + for constraint in &mut *contact_constraints { + constraint.solve(1.0, &mut self.mj_lambdas[..], false, true); } - // Update velocities. - for handle in islands.active_island(island_id) { - if let Some(link) = multibodies.rigid_body_link(*handle).copied() { - let multibody = multibodies - .get_multibody_mut_internal(link.multibody) - .unwrap(); + for constraint in &mut *generic_contact_constraints { + constraint.solve( + 1.0, + generic_contact_jacobians, + &mut self.mj_lambdas[..], + &mut self.generic_mj_lambdas, + false, + true, + ); + } + } - if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { - let mj_lambdas = self - .generic_mj_lambdas - .rows(multibody.solver_id, multibody.ndofs()); - multibody.velocities += mj_lambdas; - } - } else { - let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = - bodies.index_bundle(handle.0); + // Update velocities. + for handle in islands.active_island(island_id) { + if let Some(link) = multibodies.rigid_body_link(*handle).copied() { + let multibody = multibodies + .get_multibody_mut_internal(link.multibody) + .unwrap(); - let dvel = self.mj_lambdas[ids.active_set_offset]; - let dangvel = mprops - .effective_world_inv_inertia_sqrt - .transform_vector(dvel.angular); - - // let mut curr_vel_pseudo_energy = 0.0; - bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { - // curr_vel_pseudo_energy = vels.pseudo_kinetic_energy(); - vels.linvel += dvel.linear; - vels.angvel += dangvel; - }); - - // let impulse_vel_pseudo_energy = RigidBodyVelocity { - // linvel: dvel.linear, - // angvel: dangvel, - // } - // .pseudo_kinetic_energy(); - // - // bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| { - // activation.energy = - // impulse_vel_pseudo_energy.max(curr_vel_pseudo_energy / 5.0); - // }); + if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { + let mj_lambdas = self + .generic_mj_lambdas + .rows(multibody.solver_id, multibody.ndofs()); + multibody.velocities += mj_lambdas; } + } else { + let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = + bodies.index_bundle(handle.0); + + let dvel = self.mj_lambdas[ids.active_set_offset]; + let dangvel = mprops + .effective_world_inv_inertia_sqrt + .transform_vector(dvel.angular); + + bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| { + vels.linvel += dvel.linear; + vels.angvel += dangvel; + }); } } From 9bfcde25e0091bd9ad07949d497f90f0486c18e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Jan 2022 16:59:03 +0100 Subject: [PATCH 5/7] Run cargo fmt --- .../solver/generic_velocity_ground_constraint_element.rs | 6 +++--- .../joint_constraint/joint_generic_velocity_constraint.rs | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs index f645f04..750811c 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs @@ -121,9 +121,9 @@ impl VelocityGroundConstraintElement { let mut nrm_j_id = j_id; for element in elements.iter_mut() { - element - .normal_part - .generic_solve(cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas); + element.normal_part.generic_solve( + cfm_factor, nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas, + ); nrm_j_id += j_step; } } diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs index 0a75967..2646fe8 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs @@ -524,6 +524,6 @@ impl JointGenericVelocityGroundConstraint { } pub fn remove_bias_from_rhs(&mut self) { - self.rhs = &mut self.rhs_wo_bias; + self.rhs = self.rhs_wo_bias; } } From 5e2111bdb1226d818f594d1bad3db6b8f688ecb6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Jan 2022 17:06:42 +0100 Subject: [PATCH 6/7] Fix compilation of parallel version --- .vscode/tasks.json | 36 ++++++++++++++++--- .../solver/parallel_velocity_solver.rs | 9 ++++- 2 files changed, 40 insertions(+), 5 deletions(-) diff --git a/.vscode/tasks.json b/.vscode/tasks.json index de72ffb..0bc892b 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -29,9 +29,23 @@ "all_examples3", "--release", "--features", - "simd-stable", + "simd-stable,other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "run 3d (simd - parallel - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_examples3", + "--release", "--features", - "other-backends", + "simd-stable,other-backends,parallel", "--", "--pause" ], @@ -63,9 +77,23 @@ "all_examples2", "--release", "--features", - "simd-stable", + "simd-stable,other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "run 2d (simd - parallel - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_examples2", + "--release", "--features", - "other-backends", + "simd-stable,other-backends,parallel", "--", "--pause" ], diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 69ceb03..00668b1 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -54,6 +54,7 @@ impl ParallelVelocitySolver { let joint_descs = &joint_constraints.constraint_descs[..]; let mut target_num_desc = 0; let mut shift = 0; + let cfm_factor = params.cfm_factor(); for _ in 0..params.max_velocity_iterations { macro_rules! solve { @@ -116,7 +117,13 @@ impl ParallelVelocitySolver { ); shift += joint_descs.len(); start_index -= joint_descs.len(); - solve!(contact_constraints, &mut self.mj_lambdas, true, true); + solve!( + contact_constraints, + cfm_factor, + &mut self.mj_lambdas, + true, + true + ); shift += contact_descs.len(); start_index -= contact_descs.len(); } From b3b675d2de64d4437748ad46e41cca90c691de1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 23 Jan 2022 17:17:30 +0100 Subject: [PATCH 7/7] Fix tests build --- examples3d-f64/debug_serialized3.rs | 36 ----------------------------- 1 file changed, 36 deletions(-) diff --git a/examples3d-f64/debug_serialized3.rs b/examples3d-f64/debug_serialized3.rs index 5d4ae19..e94d7ef 100644 --- a/examples3d-f64/debug_serialized3.rs +++ b/examples3d-f64/debug_serialized3.rs @@ -20,36 +20,6 @@ pub fn init_world(testbed: &mut 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, @@ -60,12 +30,6 @@ pub fn init_world(testbed: &mut Testbed) { 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]);