From 134f4339037cbf83ea9c6abd6c352dc3a2fcf06e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 5 Sep 2025 19:31:58 +0200 Subject: [PATCH] =?UTF-8?q?feat:=E2=80=AFsolver=20improvements=20+=20relea?= =?UTF-8?q?se=20v0.29.0=20(#876)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat: solver improvements * feat: add function to get/set whether gyroscopic forces are enabled on a rigid-body * chore: switch to released versions of parry and wide instead of local patches * fix cargo doc * chore: typo fixes * chore: clippy fix * Release v0.29.0 * chore: more clippy fixes --- CHANGELOG.md | 16 + Cargo.toml | 1 + benchmarks3d/joint_revolute3.rs | 2 +- benchmarks3d/stacks3.rs | 2 +- crates/rapier2d-f64/Cargo.toml | 7 +- crates/rapier2d/Cargo.toml | 14 +- crates/rapier3d-f64/Cargo.toml | 7 +- crates/rapier3d-meshloader/Cargo.toml | 4 +- crates/rapier3d-urdf/Cargo.toml | 6 +- crates/rapier3d/Cargo.toml | 10 +- crates/rapier_testbed2d-f64/Cargo.toml | 4 +- crates/rapier_testbed2d/Cargo.toml | 4 +- crates/rapier_testbed3d-f64/Cargo.toml | 4 +- crates/rapier_testbed3d/Cargo.toml | 4 +- examples2d/one_way_platforms2.rs | 2 +- examples3d-f64/all_examples3-f64.rs | 7 +- examples3d-f64/debug_serialized3.rs | 5 +- examples3d-f64/trimesh3_f64.rs | 82 ++ examples3d/all_examples3.rs | 2 + examples3d/debug_multibody_ang_motor_pos3.rs | 2 +- examples3d/domino3.rs | 2 +- examples3d/gyroscopic3.rs | 30 + examples3d/joints3.rs | 2 +- examples3d/one_way_platforms3.rs | 2 +- examples3d/platform3.rs | 10 +- examples3d/vehicle_joints3.rs | 6 +- src/control/pid_controller.rs | 20 +- src/control/ray_cast_vehicle_controller.rs | 10 +- src/data/arena.rs | 6 +- src/data/graph.rs | 6 +- src/dynamics/ccd/ccd_solver.rs | 30 +- src/dynamics/ccd/toi_entry.rs | 20 +- src/dynamics/integration_parameters.rs | 82 +- src/dynamics/island_manager.rs | 119 +- src/dynamics/joint/generic_joint.rs | 24 +- .../joint/impulse_joint/impulse_joint_set.rs | 8 +- .../joint/multibody_joint/multibody.rs | 13 +- .../joint/multibody_joint/multibody_joint.rs | 6 +- .../joint/multibody_joint/multibody_link.rs | 2 +- .../multibody_joint/multibody_workspace.rs | 2 +- .../multibody_joint/unit_multibody_joint.rs | 26 +- src/dynamics/mod.rs | 4 + src/dynamics/rigid_body.rs | 161 ++- src/dynamics/rigid_body_components.rs | 278 ++-- src/dynamics/rigid_body_set.rs | 7 +- src/dynamics/solver/categorization.rs | 23 +- .../any_contact_constraint.rs | 63 + ...ement.rs => contact_constraint_element.rs} | 262 ++-- .../contact_constraints_set.rs | 580 ++++---- .../contact_with_coulomb_friction.rs | 505 +++++++ .../contact_with_twist_friction.rs | 515 +++++++ ...raint.rs => generic_contact_constraint.rs} | 371 +++-- ... => generic_contact_constraint_element.rs} | 126 +- .../generic_one_body_constraint.rs | 307 ---- .../generic_one_body_constraint_element.rs | 233 --- src/dynamics/solver/contact_constraint/mod.rs | 86 +- .../contact_constraint/one_body_constraint.rs | 424 ------ .../one_body_constraint_element.rs | 311 ---- .../one_body_constraint_simd.rs | 426 ------ .../contact_constraint/two_body_constraint.rs | 533 ------- .../two_body_constraint_simd.rs | 433 ------ src/dynamics/solver/interaction_groups.rs | 70 +- src/dynamics/solver/island_solver.rs | 24 +- .../joint_constraint/any_joint_constraint.rs | 89 +- .../generic_joint_constraint.rs | 320 +++++ .../generic_joint_constraint_builder.rs | 749 ++++++++++ .../joint_constraint_builder.rs | 961 ++----------- .../joint_constraint/joint_constraints_set.rs | 355 ++--- .../joint_generic_constraint.rs | 552 ------- .../joint_generic_constraint_builder.rs | 1279 ----------------- .../joint_velocity_constraint.rs | 446 +----- src/dynamics/solver/joint_constraint/mod.rs | 18 +- src/dynamics/solver/mod.rs | 14 +- src/dynamics/solver/parallel_island_solver.rs | 6 +- .../solver/parallel_solver_constraints.rs | 12 +- .../solver/parallel_velocity_solver.rs | 12 +- src/dynamics/solver/solver_body.rs | 582 +++++++- src/dynamics/solver/solver_constraints_set.rs | 242 ---- src/dynamics/solver/solver_vel.rs | 66 - src/dynamics/solver/velocity_solver.rs | 201 +-- src/geometry/contact_pair.rs | 239 ++- src/geometry/mod.rs | 3 +- src/geometry/narrow_phase.rs | 16 +- src/lib.rs | 28 +- src/pipeline/physics_pipeline.rs | 32 +- src/pipeline/query_pipeline.rs | 2 +- src/pipeline/user_changes.rs | 73 +- src/utils.rs | 111 +- src_testbed/box2d_backend.rs | 250 ---- src_testbed/lib.rs | 2 - src_testbed/physx_backend.rs | 8 +- src_testbed/save.rs | 18 +- src_testbed/testbed.rs | 67 +- src_testbed/ui.rs | 96 +- 94 files changed, 5066 insertions(+), 8136 deletions(-) create mode 100644 examples3d-f64/trimesh3_f64.rs create mode 100644 examples3d/gyroscopic3.rs create mode 100644 src/dynamics/solver/contact_constraint/any_contact_constraint.rs rename src/dynamics/solver/contact_constraint/{two_body_constraint_element.rs => contact_constraint_element.rs} (57%) create mode 100644 src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs create mode 100644 src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs rename src/dynamics/solver/contact_constraint/{generic_two_body_constraint.rs => generic_contact_constraint.rs} (50%) rename src/dynamics/solver/contact_constraint/{generic_two_body_constraint_element.rs => generic_contact_constraint_element.rs} (83%) delete mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs delete mode 100644 src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs delete mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint.rs delete mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_element.rs delete mode 100644 src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs delete mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint.rs delete mode 100644 src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs create mode 100644 src/dynamics/solver/joint_constraint/generic_joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint.rs delete mode 100644 src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs delete mode 100644 src/dynamics/solver/solver_constraints_set.rs delete mode 100644 src/dynamics/solver/solver_vel.rs delete mode 100644 src_testbed/box2d_backend.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index b71410a..a66ba95 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,19 @@ +## v0.29.0 (05 Sept. 2025) + +This version contains a significant rework of the internal velocity constraints solver. +This makes it both simpler and more optimized (making the whole simulation up to 25% faster). For details on all the +changes, see [#876](https://github.com/dimforge/rapier/pull/876). + +Notable changes include: + +- Update to parry 0.24 (includes a breaking change where all the `*angular_inertia_sqrt` fields and functions have been + replaced by their non-square-root equivalent. +- Fixed bug where friction on kinematic bodies would affect dynamic bodies much more weakly than it should. +- In 3D, added a new friction model that is more efficient than the traditional Coulomb friction. This new simplified + model is enabled by default and can be changed with `IntegrationParameters::friction_model`. +- Removed support for the legacy PGS solver. Removed `IntegrationParameters::pgs_legacy` and + `::tgs_soft_without_warmstart`. + ## v0.28.0 (08 August 2025) ### Modified diff --git a/Cargo.toml b/Cargo.toml index d8c77bc..574fb41 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -33,6 +33,7 @@ needless_lifetimes = "allow" #parry3d-f64 = { path = "../parry/crates/parry3d-f64" } #nalgebra = { path = "../nalgebra" } #simba = { path = "../simba" } +#wide = { path = "../wide" } #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } diff --git a/benchmarks3d/joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs index b43522d..a946992 100644 --- a/benchmarks3d/joint_revolute3.rs +++ b/benchmarks3d/joint_revolute3.rs @@ -38,7 +38,7 @@ pub fn init_world(testbed: &mut Testbed) { let mut handles = [curr_parent; 4]; for k in 0..4 { let density = 1.0; - let rigid_body = RigidBodyBuilder::dynamic().position(positions[k]); + let rigid_body = RigidBodyBuilder::dynamic().pose(positions[k]); handles[k] = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(rad, rad, rad).density(density); colliders.insert_with_parent(collider, handles[k], &mut bodies); diff --git a/benchmarks3d/stacks3.rs b/benchmarks3d/stacks3.rs index f4cdaed..b5fa34f 100644 --- a/benchmarks3d/stacks3.rs +++ b/benchmarks3d/stacks3.rs @@ -23,7 +23,7 @@ fn create_tower_circle( * Translation::new(0.0, y, radius); // Build the rigid body. - let rigid_body = RigidBodyBuilder::dynamic().position(pos); + let rigid_body = RigidBodyBuilder::dynamic().pose(pos); let handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z); colliders.insert_with_parent(collider, handle, bodies); diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index 357897f..76217b7 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier2d-f64" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "2-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier2d" @@ -69,8 +69,8 @@ vec_map = { version = "0.8", optional = true } web-time = { version = "1.1", optional = true } num-traits = "0.2" nalgebra = "0.34" -parry2d-f64 = "0.23.0" -simba = "0.9" +parry2d-f64 = "0.24.0" +simba = "0.9.1" approx = "0.5" rayon = { version = "1", optional = true } arrayvec = "0.7" @@ -84,6 +84,7 @@ log = "0.4" ordered-float = "5" thiserror = "2" profiling = "1.0" +static_assertions = "1" [dev-dependencies] bincode = "1" diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 348a5b0..7fa4c7a 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier2d" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "2-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier2d" @@ -33,8 +33,8 @@ default = ["dim2", "f32"] dim2 = [] f32 = [] parallel = ["dep:rayon"] -simd-stable = ["simba/wide", "simd-is-enabled"] -simd-nightly = ["simba/portable_simd", "simd-is-enabled"] +simd-stable = ["simba/wide", "parry2d/simd-stable", "simd-is-enabled"] +simd-nightly = ["simba/portable_simd", "parry2d/simd-nightly", "simd-is-enabled"] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = ["dep:vec_map"] @@ -70,8 +70,8 @@ vec_map = { version = "0.8", optional = true } web-time = { version = "1.1", optional = true } num-traits = "0.2" nalgebra = "0.34" -parry2d = "0.23.0" -simba = "0.9" +parry2d = "0.24.0" +simba = "0.9.1" approx = "0.5" rayon = { version = "1", optional = true } arrayvec = "0.7" @@ -85,6 +85,10 @@ log = "0.4" ordered-float = "5" thiserror = "2" profiling = "1.0" +static_assertions = "1" + +# TODO: should be re-exported from simba +wide = "0.7.1" [dev-dependencies] bincode = "1" diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index c02377f..ed04eed 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier3d-f64" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "3-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier3d" @@ -72,8 +72,8 @@ vec_map = { version = "0.8", optional = true } web-time = { version = "1.1", optional = true } num-traits = "0.2" nalgebra = "0.34" -parry3d-f64 = "0.23.0" -simba = "0.9" +parry3d-f64 = "0.24.0" +simba = "0.9.1" approx = "0.5" rayon = { version = "1", optional = true } arrayvec = "0.7" @@ -87,6 +87,7 @@ log = "0.4" ordered-float = "5" thiserror = "2" profiling = "1.0" +static_assertions = "1" [dev-dependencies] bincode = "1" diff --git a/crates/rapier3d-meshloader/Cargo.toml b/crates/rapier3d-meshloader/Cargo.toml index e0b9a12..5bdbe3d 100644 --- a/crates/rapier3d-meshloader/Cargo.toml +++ b/crates/rapier3d-meshloader/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier3d-meshloader" -version = "0.9.0" +version = "0.10.0" authors = ["Sébastien Crozet "] description = "STL file loader for the 3D rapier physics engine." documentation = "https://docs.rs/rapier3d-meshloader" @@ -29,4 +29,4 @@ thiserror = "2" profiling = "1.0" mesh-loader = "0.1.12" -rapier3d = { version = "0.28.0", path = "../rapier3d" } +rapier3d = { version = "0.29.0", path = "../rapier3d" } diff --git a/crates/rapier3d-urdf/Cargo.toml b/crates/rapier3d-urdf/Cargo.toml index 086f471..69727c2 100644 --- a/crates/rapier3d-urdf/Cargo.toml +++ b/crates/rapier3d-urdf/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier3d-urdf" -version = "0.9.0" +version = "0.10.0" authors = ["Sébastien Crozet "] description = "URDF file loader for the 3D rapier physics engine." documentation = "https://docs.rs/rapier3d-urdf" @@ -31,5 +31,5 @@ anyhow = "1" bitflags = "2" urdf-rs = "0.9" -rapier3d = { version = "0.28.0", path = "../rapier3d" } -rapier3d-meshloader = { version = "0.9.0", path = "../rapier3d-meshloader", default-features = false, optional = true } +rapier3d = { version = "0.29.0", path = "../rapier3d" } +rapier3d-meshloader = { version = "0.10.0", path = "../rapier3d-meshloader", default-features = false, optional = true } diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index ac2372c..fd0c8ab 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier3d" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "3-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier3d" @@ -74,8 +74,8 @@ vec_map = { version = "0.8", optional = true } web-time = { version = "1.1", optional = true } num-traits = "0.2" nalgebra = "0.34" -parry3d = "0.23.0" -simba = "0.9" +parry3d = "0.24.0" +simba = "0.9.1" approx = "0.5" rayon = { version = "1", optional = true } arrayvec = "0.7" @@ -89,6 +89,10 @@ log = "0.4" ordered-float = "5" thiserror = "2" profiling = "1.0" +static_assertions = "1" + +# TODO: should be re-exported from simba +wide = "0.7.1" [dev-dependencies] bincode = "1" diff --git a/crates/rapier_testbed2d-f64/Cargo.toml b/crates/rapier_testbed2d-f64/Cargo.toml index 37f7e95..595f013 100644 --- a/crates/rapier_testbed2d-f64/Cargo.toml +++ b/crates/rapier_testbed2d-f64/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed2d-f64" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "Testbed for the Rapier 2-dimensional physics engine in Rust." homepage = "http://rapier.rs" @@ -98,5 +98,5 @@ bevy = { version = "0.15", default-features = false, features = [ [dependencies.rapier] package = "rapier2d-f64" path = "../rapier2d-f64" -version = "0.28.0" +version = "0.29.0" features = ["serde-serialize", "debug-render", "profiler"] diff --git a/crates/rapier_testbed2d/Cargo.toml b/crates/rapier_testbed2d/Cargo.toml index af4d34f..88ac2a9 100644 --- a/crates/rapier_testbed2d/Cargo.toml +++ b/crates/rapier_testbed2d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed2d" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "Testbed for the Rapier 2-dimensional physics engine in Rust." homepage = "http://rapier.rs" @@ -98,5 +98,5 @@ bevy = { version = "0.15", default-features = false, features = [ [dependencies.rapier] package = "rapier2d" path = "../rapier2d" -version = "0.28.0" +version = "0.29.0" features = ["serde-serialize", "debug-render", "profiler"] diff --git a/crates/rapier_testbed3d-f64/Cargo.toml b/crates/rapier_testbed3d-f64/Cargo.toml index 9088a4a..04e01ec 100644 --- a/crates/rapier_testbed3d-f64/Cargo.toml +++ b/crates/rapier_testbed3d-f64/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed3d-f64" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "Testbed for the Rapier 3-dimensional physics engine in Rust." homepage = "http://rapier.rs" @@ -99,5 +99,5 @@ bevy = { version = "0.15", default-features = false, features = [ [dependencies.rapier] package = "rapier3d-f64" path = "../rapier3d-f64" -version = "0.28.0" +version = "0.29.0" features = ["serde-serialize", "debug-render", "profiler"] diff --git a/crates/rapier_testbed3d/Cargo.toml b/crates/rapier_testbed3d/Cargo.toml index ba2ed0d..1ad02a7 100644 --- a/crates/rapier_testbed3d/Cargo.toml +++ b/crates/rapier_testbed3d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed3d" -version = "0.28.0" +version = "0.29.0" authors = ["Sébastien Crozet "] description = "Testbed for the Rapier 3-dimensional physics engine in Rust." homepage = "http://rapier.rs" @@ -97,5 +97,5 @@ bevy = { version = "0.15", default-features = false, features = [ [dependencies.rapier] package = "rapier3d" path = "../rapier3d" -version = "0.28.0" +version = "0.29.0" features = ["serde-serialize", "debug-render", "profiler"] diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index 62ed87c..0d005c4 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -104,7 +104,7 @@ pub fn init_world(testbed: &mut Testbed) { } } - for handle in physics.islands.active_dynamic_bodies() { + for handle in physics.islands.active_bodies() { let body = &mut physics.bodies[*handle]; if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); diff --git a/examples3d-f64/all_examples3-f64.rs b/examples3d-f64/all_examples3-f64.rs index b7de076..1459fb2 100644 --- a/examples3d-f64/all_examples3-f64.rs +++ b/examples3d-f64/all_examples3-f64.rs @@ -9,11 +9,14 @@ use rapier_testbed3d::{Testbed, TestbedApp}; use std::cmp::Ordering; mod debug_serialized3; +mod trimesh3_f64; #[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] pub fn main() { - let mut builders: Vec<(_, fn(&mut Testbed))> = - vec![("(Debug) serialized", debug_serialized3::init_world)]; + let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ + ("Trimesh", trimesh3_f64::init_world), + ("(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('(')) { diff --git a/examples3d-f64/debug_serialized3.rs b/examples3d-f64/debug_serialized3.rs index 0ab1487..54f8041 100644 --- a/examples3d-f64/debug_serialized3.rs +++ b/examples3d-f64/debug_serialized3.rs @@ -17,7 +17,10 @@ pub fn init_world(testbed: &mut Testbed) { /* * Set up the testbed. */ - let bytes = std::fs::read("state.bin").unwrap(); + let Ok(bytes) = std::fs::read("state.bin") else { + println!("Failed to load serialized world state."); + return; + }; let state: State = bincode::deserialize(&bytes).unwrap(); testbed.set_world( diff --git a/examples3d-f64/trimesh3_f64.rs b/examples3d-f64/trimesh3_f64.rs new file mode 100644 index 0000000..a95443a --- /dev/null +++ b/examples3d-f64/trimesh3_f64.rs @@ -0,0 +1,82 @@ +use rapier_testbed3d::Testbed; +use rapier3d::na::ComplexField; +use rapier3d::prelude::*; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = vector![200.0, 1.0, 200.0]; + let nsubdivs = 20; + + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs { + 10.0 + } else { + let x = i as f64 * ground_size.x / (nsubdivs as f64); + let z = j as f64 * ground_size.z / (nsubdivs as f64); + + // NOTE: make sure we use the sin/cos from simba to ensure + // cross-platform determinism of the example when the + // enhanced_determinism feature is enabled. + ::sin(x) + ::cos(z) + } + }); + + // Here we will build our trimesh from the mesh representation of an + // heightfield. + let heightfield = HeightField::new(heights, ground_size); + let (vertices, indices) = heightfield.to_trimesh(); + + let rigid_body = RigidBodyBuilder::fixed(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::trimesh(vertices, indices).unwrap(); + colliders.insert_with_parent(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 1.0; + + let shift = rad * 2.0 + rad; + let centerx = shift * (num / 2) as f64; + let centery = shift / 2.0; + let centerz = shift * (num / 2) as f64; + + for j in 0usize..47 { + for i in 0..num { + for k in 0usize..num { + let x = i as f64 * shift - centerx; + let y = j as f64 * shift + centery + 3.0; + let z = k as f64 * shift - centerz; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]); + let handle = bodies.insert(rigid_body); + + if j % 2 == 0 { + let collider = ColliderBuilder::cuboid(rad, rad, rad); + colliders.insert_with_parent(collider, handle, &mut bodies); + } else { + let collider = ColliderBuilder::ball(rad); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); +} diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 5aa3a68..ae272dd 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -44,6 +44,7 @@ mod debug_cube_high_mass_ratio3; mod debug_internal_edges3; mod debug_long_chain3; mod debug_multibody_ang_motor_pos3; +mod gyroscopic3; mod inverse_kinematics3; mod joint_motor_position3; mod keva3; @@ -75,6 +76,7 @@ pub fn main() { ("Convex decomposition", convex_decomposition3::init_world), ("Convex polyhedron", convex_polyhedron3::init_world), ("Damping", damping3::init_world), + ("Gyroscopic", gyroscopic3::init_world), ("Domino", domino3::init_world), ("Dynamic trimeshes", dynamic_trimesh3::init_world), ("Heightfield", heightfield3::init_world), diff --git a/examples3d/debug_multibody_ang_motor_pos3.rs b/examples3d/debug_multibody_ang_motor_pos3.rs index dcf55a3..4c769e0 100644 --- a/examples3d/debug_multibody_ang_motor_pos3.rs +++ b/examples3d/debug_multibody_ang_motor_pos3.rs @@ -12,7 +12,7 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0); colliders.insert_with_parent(collider, body, &mut bodies); - let rigid_body = RigidBodyBuilder::dynamic().position(Isometry::translation(0.0, 1.0, 0.0)); + let rigid_body = RigidBodyBuilder::dynamic().pose(Isometry::translation(0.0, 1.0, 0.0)); let body_part = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0).density(1.0); colliders.insert_with_parent(collider, body_part, &mut bodies); diff --git a/examples3d/domino3.rs b/examples3d/domino3.rs index a6ac28a..fea8418 100644 --- a/examples3d/domino3.rs +++ b/examples3d/domino3.rs @@ -53,7 +53,7 @@ pub fn init_world(testbed: &mut Testbed) { Translation::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad) * tilt * rot; - let rigid_body = RigidBodyBuilder::dynamic().position(position); + let rigid_body = RigidBodyBuilder::dynamic().pose(position); let handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width); colliders.insert_with_parent(collider, handle, &mut bodies); diff --git a/examples3d/gyroscopic3.rs b/examples3d/gyroscopic3.rs new file mode 100644 index 0000000..0f35004 --- /dev/null +++ b/examples3d/gyroscopic3.rs @@ -0,0 +1,30 @@ +use rapier_testbed3d::Testbed; +use rapier3d::prelude::*; + +// Simulate the the Dzhanibekov effect: +// https://en.wikipedia.org/wiki/Tennis_racket_theorem +pub fn init_world(testbed: &mut Testbed) { + let mut colliders = ColliderSet::new(); + let mut bodies = RigidBodySet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + let shapes = vec![ + (Isometry::identity(), SharedShape::cuboid(2.0, 0.2, 0.2)), + ( + Isometry::translation(0.0, 0.8, 0.0), + SharedShape::cuboid(0.2, 0.4, 0.2), + ), + ]; + + let body = RigidBodyBuilder::dynamic() + .gravity_scale(0.0) + .angvel(vector![0.0, 20.0, 0.1]) + .gyroscopic_forces_enabled(true); + let body_handle = bodies.insert(body); + let collider = ColliderBuilder::compound(shapes); + colliders.insert_with_parent(collider, body_handle, &mut bodies); + + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![8.0, 0.0, 8.0], point![0.0, 0.0, 0.0]); +} diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index aad2f03..9d65275 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -167,7 +167,7 @@ fn create_revolute_joints( let mut handles = [curr_parent; 4]; for k in 0..4 { - let rigid_body = RigidBodyBuilder::dynamic().position(positions[k]); + let rigid_body = RigidBodyBuilder::dynamic().pose(positions[k]); handles[k] = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handles[k], bodies); diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index dc27eee..f9f7cb0 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -104,7 +104,7 @@ pub fn init_world(testbed: &mut Testbed) { } } - for handle in physics.islands.active_dynamic_bodies() { + for handle in physics.islands.active_bodies() { let body = physics.bodies.get_mut(*handle).unwrap(); if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index cb5618a..2da2206 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -75,21 +75,25 @@ pub fn init_world(testbed: &mut Testbed) { testbed.add_callback(move |_, physics, _, run_state| { let velocity = vector![ 0.0, - (run_state.time * 2.0).sin(), + (run_state.time * 2.0).cos(), run_state.time.sin() * 2.0 ]; // Update the velocity-based kinematic body by setting its velocity. if let Some(platform) = physics.bodies.get_mut(velocity_based_platform_handle) { platform.set_linvel(velocity, true); - platform.set_angvel(vector![0.0, 0.2, 0.0], true); + platform.set_angvel(vector![0.0, 1.0, 0.0], true); } // Update the position-based kinematic body by setting its next position. if let Some(platform) = physics.bodies.get_mut(position_based_platform_handle) { let mut next_tra = *platform.translation(); - next_tra += velocity * physics.integration_parameters.dt; + let mut next_rot = *platform.rotation(); + next_tra += -velocity * physics.integration_parameters.dt; + next_rot = Rotation::new(vector![0.0, -0.5 * physics.integration_parameters.dt, 0.0]) + * next_rot; platform.set_next_kinematic_translation(next_tra); + platform.set_next_kinematic_rotation(next_rot); } }); diff --git a/examples3d/vehicle_joints3.rs b/examples3d/vehicle_joints3.rs index c0784dc..7c4b1c4 100644 --- a/examples3d/vehicle_joints3.rs +++ b/examples3d/vehicle_joints3.rs @@ -55,7 +55,7 @@ pub fn init_world(testbed: &mut Testbed) { .density(100.0) .collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP)); let body_rb = RigidBodyBuilder::dynamic() - .position(body_position.into()) + .pose(body_position.into()) .build(); let body_handle = bodies.insert(body_rb); colliders.insert_with_parent(body_co, body_handle, &mut bodies); @@ -69,7 +69,7 @@ pub fn init_world(testbed: &mut Testbed) { let axle_mass_props = MassProperties::from_ball(100.0, wheel_radius); let axle_rb = RigidBodyBuilder::dynamic() - .position(wheel_center.into()) + .pose(wheel_center.into()) .additional_mass_properties(axle_mass_props); let axle_handle = bodies.insert(axle_rb); @@ -87,7 +87,7 @@ pub fn init_world(testbed: &mut Testbed) { .density(100.0) .collision_groups(InteractionGroups::new(CAR_GROUP, !CAR_GROUP)) .friction(1.0); - let wheel_rb = RigidBodyBuilder::dynamic().position(wheel_center.into()); + let wheel_rb = RigidBodyBuilder::dynamic().pose(wheel_center.into()); let wheel_handle = bodies.insert(wheel_rb); colliders.insert_with_parent(wheel_co, wheel_handle, &mut bodies); colliders.insert_with_parent(wheel_fake_co, wheel_handle, &mut bodies); diff --git a/src/control/pid_controller.rs b/src/control/pid_controller.rs index 483489e..d24cd37 100644 --- a/src/control/pid_controller.rs +++ b/src/control/pid_controller.rs @@ -79,8 +79,8 @@ pub struct PdErrors { pub angular: AngVector, } -impl From for PdErrors { - fn from(vels: RigidBodyVelocity) -> Self { +impl From> for PdErrors { + fn from(vels: RigidBodyVelocity) -> Self { Self { linear: vels.linvel, angular: vels.angvel, @@ -173,8 +173,8 @@ impl PdController { &self, rb: &RigidBody, target_pose: Isometry, - target_vels: RigidBodyVelocity, - ) -> RigidBodyVelocity { + target_vels: RigidBodyVelocity, + ) -> RigidBodyVelocity { let pose_errors = RigidBodyPosition { position: rb.pos.position, next_position: target_pose, @@ -218,7 +218,11 @@ impl PdController { /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to /// the inverse of the simulation step so the returned value is a rigid-body velocity /// change. - pub fn correction(&self, pose_errors: &PdErrors, vel_errors: &PdErrors) -> RigidBodyVelocity { + pub fn correction( + &self, + pose_errors: &PdErrors, + vel_errors: &PdErrors, + ) -> RigidBodyVelocity { let lin_mask = self.lin_mask(); let ang_mask = self.ang_mask(); @@ -359,8 +363,8 @@ impl PidController { dt: Real, rb: &RigidBody, target_pose: Isometry, - target_vels: RigidBodyVelocity, - ) -> RigidBodyVelocity { + target_vels: RigidBodyVelocity, + ) -> RigidBodyVelocity { let pose_errors = RigidBodyPosition { position: rb.pos.position, next_position: target_pose, @@ -384,7 +388,7 @@ impl PidController { dt: Real, pose_errors: &PdErrors, vel_errors: &PdErrors, - ) -> RigidBodyVelocity { + ) -> RigidBodyVelocity { self.lin_integral += pose_errors.linear * dt; self.ang_integral += pose_errors.angular * dt; diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 576c6e1..2f17442 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -723,9 +723,7 @@ impl<'a> WheelContactPoint<'a> { fn impulse_denominator(body: &RigidBody, pos: &Point, n: &Vector) -> Real { let dpt = pos - body.center_of_mass(); let gcross = dpt.gcross(*n); - let v = (body.mprops.effective_world_inv_inertia_sqrt - * (body.mprops.effective_world_inv_inertia_sqrt * gcross)) - .gcross(dpt); + let v = (body.mprops.effective_world_inv_inertia * gcross).gcross(dpt); // TODO: take the effective inv mass into account instead of the inv_mass? body.mprops.local_mprops.inv_mass + n.dot(&v) } @@ -782,8 +780,8 @@ fn resolve_single_bilateral( let dpt2 = pt2 - body2.center_of_mass(); let aj = dpt1.gcross(*normal); let bj = dpt2.gcross(-*normal); - let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj; - let ibj = body2.mprops.effective_world_inv_inertia_sqrt * bj; + let iaj = body1.mprops.effective_world_inv_inertia * aj; + let ibj = body2.mprops.effective_world_inv_inertia * bj; // TODO: take the effective_inv_mass into account? let im1 = body1.mprops.local_mprops.inv_mass; @@ -803,7 +801,7 @@ fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point, normal: &Vect let dvel = vel1; let dpt1 = pt1 - body1.center_of_mass(); let aj = dpt1.gcross(*normal); - let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj; + let iaj = body1.mprops.effective_world_inv_inertia * aj; // TODO: take the effective_inv_mass into account? let im1 = body1.mprops.local_mprops.inv_mass; diff --git a/src/data/arena.rs b/src/data/arena.rs index 51789a8..9a20c3d 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -687,7 +687,7 @@ impl Arena { /// println!("{} is at index {:?}", value, idx); /// } /// ``` - pub fn iter(&self) -> Iter { + pub fn iter(&self) -> Iter<'_, T> { Iter { len: self.len, inner: self.items.iter().enumerate(), @@ -714,7 +714,7 @@ impl Arena { /// *value += 5; /// } /// ``` - pub fn iter_mut(&mut self) -> IterMut { + pub fn iter_mut(&mut self) -> IterMut<'_, T> { IterMut { len: self.len, inner: self.items.iter_mut().enumerate(), @@ -746,7 +746,7 @@ impl Arena { /// assert!(arena.get(idx_1).is_none()); /// assert!(arena.get(idx_2).is_none()); /// ``` - pub fn drain(&mut self) -> Drain { + pub fn drain(&mut self) -> Drain<'_, T> { Drain { inner: self.items.drain(..).enumerate(), } diff --git a/src/data/graph.rs b/src/data/graph.rs index 4620b88..12ead1d 100644 --- a/src/data/graph.rs +++ b/src/data/graph.rs @@ -389,7 +389,7 @@ impl Graph { /// /// Produces an empty iterator if the node doesn't exist.
/// Iterator element type is `EdgeReference`. - pub fn edges(&self, a: NodeIndex) -> Edges { + pub fn edges(&self, a: NodeIndex) -> Edges<'_, E> { self.edges_directed(a, Direction::Outgoing) } @@ -404,7 +404,7 @@ impl Graph { /// /// Produces an empty iterator if the node `a` doesn't exist.
/// Iterator element type is `EdgeReference`. - pub fn edges_directed(&self, a: NodeIndex, dir: Direction) -> Edges { + pub fn edges_directed(&self, a: NodeIndex, dir: Direction) -> Edges<'_, E> { Edges { skip_start: a, edges: &self.edges, @@ -527,7 +527,7 @@ fn edges_walker_mut( edges: &mut [Edge], next: EdgeIndex, dir: Direction, -) -> EdgesWalkerMut { +) -> EdgesWalkerMut<'_, E> { EdgesWalkerMut { edges, next, dir } } diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 9bfaa6e..555db29 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -36,7 +36,7 @@ impl CCDSolver { let min_toi = (rb.ccd.ccd_thickness * 0.15 - * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels))) + * crate::utils::inv(rb.ccd.max_point_velocity(&rb.ccd_vels))) .min(dt); // println!( // "Min toi: {}, Toi: {}, thick: {}, max_vel: {}", @@ -45,9 +45,9 @@ impl CCDSolver { // rb.ccd.ccd_thickness, // rb.ccd.max_point_velocity(&rb.integrated_vels) // ); - let new_pos = - rb.integrated_vels - .integrate(toi.max(min_toi), &rb.pos.position, local_com); + let new_pos = rb + .ccd_vels + .integrate(toi.max(min_toi), &rb.pos.position, local_com); rb.pos.next_position = new_pos; } } @@ -66,7 +66,7 @@ impl CCDSolver { let mut ccd_active = false; // println!("Checking CCD activation"); - for handle in islands.active_dynamic_bodies() { + for handle in islands.active_bodies() { let rb = bodies.index_mut_internal(*handle); if rb.ccd.ccd_enabled { @@ -75,7 +75,7 @@ impl CCDSolver { } else { None }; - let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces); + let moving_fast = rb.ccd.is_moving_fast(dt, &rb.ccd_vels, forces); rb.ccd.ccd_active = moving_fast; ccd_active = ccd_active || moving_fast; } @@ -121,14 +121,14 @@ impl CCDSolver { let mut pairs_seen = HashMap::default(); let mut min_toi = dt; - for handle in islands.active_dynamic_bodies() { + for handle in islands.active_bodies() { let rb1 = &bodies[*handle]; if rb1.ccd.ccd_active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, - &rb1.integrated_vels, + &rb1.ccd_vels, &rb1.mprops, ); @@ -254,14 +254,14 @@ impl CCDSolver { * */ // TODO: don't iterate through all the colliders. - for handle in islands.active_dynamic_bodies() { + for handle in islands.active_bodies() { let rb1 = &bodies[*handle]; if rb1.ccd.ccd_active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, - &rb1.integrated_vels, + &rb1.ccd_vels, &rb1.mprops, ); @@ -487,10 +487,7 @@ impl CCDSolver { let local_com1 = &rb1.mprops.local_mprops.local_com; let frozen1 = frozen.get(&b1); let pos1 = frozen1 - .map(|t| { - rb1.integrated_vels - .integrate(*t, &rb1.pos.position, local_com1) - }) + .map(|t| rb1.ccd_vels.integrate(*t, &rb1.pos.position, local_com1)) .unwrap_or(rb1.pos.next_position); pos1 * co_parent1.pos_wrt_parent } else { @@ -503,10 +500,7 @@ impl CCDSolver { let local_com2 = &rb2.mprops.local_mprops.local_com; let frozen2 = frozen.get(&b2); let pos2 = frozen2 - .map(|t| { - rb2.integrated_vels - .integrate(*t, &rb2.pos.position, local_com2) - }) + .map(|t| rb2.ccd_vels.integrate(*t, &rb2.pos.position, local_com2)) .unwrap_or(rb2.pos.next_position); pos2 * co_parent2.pos_wrt_parent } else { diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 33e1fc6..162aded 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -54,14 +54,14 @@ impl TOIEntry { return None; } - let linvel1 = frozen1.is_none() as u32 as Real - * rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero()); - let linvel2 = frozen2.is_none() as u32 as Real - * rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero()); - let angvel1 = frozen1.is_none() as u32 as Real - * rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero()); - let angvel2 = frozen2.is_none() as u32 as Real - * rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero()); + let linvel1 = + frozen1.is_none() as u32 as Real * rb1.map(|b| b.ccd_vels.linvel).unwrap_or(na::zero()); + let linvel2 = + frozen2.is_none() as u32 as Real * rb2.map(|b| b.ccd_vels.linvel).unwrap_or(na::zero()); + let angvel1 = + frozen1.is_none() as u32 as Real * rb1.map(|b| b.ccd_vels.angvel).unwrap_or(na::zero()); + let angvel2 = + frozen2.is_none() as u32 as Real * rb2.map(|b| b.ccd_vels.angvel).unwrap_or(na::zero()); #[cfg(feature = "dim2")] let vel12 = (linvel2 - linvel1).norm() @@ -160,8 +160,8 @@ impl TOIEntry { NonlinearRigidMotion::new( rb.pos.position, rb.mprops.local_mprops.local_com, - rb.integrated_vels.linvel, - rb.integrated_vels.angvel, + rb.ccd_vels.linvel, + rb.ccd_vels.angvel, ) } else { NonlinearRigidMotion::constant_position(rb.pos.next_position) diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 2425ea6..a23c0d0 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -1,6 +1,5 @@ use crate::math::Real; use na::RealField; -use std::num::NonZeroUsize; #[cfg(doc)] use super::RigidBodyActivation; @@ -9,6 +8,28 @@ use super::RigidBodyActivation; // the 3D domino demo. So for now we dont enable it in 3D. pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2"); +/// Friction models used for all contact constraints between two rigid-bodies. +/// +/// This selection does not apply to multibodies that always rely on the [`FrictionModel::Coulomb`]. +#[cfg(feature = "dim3")] +#[derive(Default, Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum FrictionModel { + /// A simplified friction model significantly faster to solve than [`Self::Coulomb`] + /// but less accurate. + /// + /// Instead of solving one Coulomb friction constraint per contact in a contact manifold, + /// this approximation only solves one Coulomb friction constraint per group of 4 contacts + /// in a contact manifold, plus one "twist" constraint. The "twist" constraint is purely + /// rotational and aims to eliminate angular movement in the manifold’s tangent plane. + #[default] + Simplified, + /// The coulomb friction model. + /// + /// This results in one Coulomb friction constraint per contact point. + Coulomb, +} + /// Parameters for a time-step of the physics engine. #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -92,24 +113,25 @@ pub struct IntegrationParameters { /// This value is implicitly scaled by [`IntegrationParameters::length_unit`]. pub normalized_prediction_distance: Real, /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`). - pub num_solver_iterations: NonZeroUsize, - /// Number of addition friction resolution iteration run during the last solver sub-step (default: `0`). - pub num_additional_friction_iterations: usize, + pub num_solver_iterations: usize, /// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`). pub num_internal_pgs_iterations: usize, - /// The number of stabilization iterations run at each solver iterations (default: `2`). + /// The number of stabilization iterations run at each solver iterations (default: `1`). pub num_internal_stabilization_iterations: usize, - /// Minimum number of dynamic bodies in each active island (default: `128`). + /// Minimum number of dynamic bodies on each active island (default: `128`). pub min_island_size: usize, /// Maximum number of substeps performed by the solver (default: `1`). pub max_ccd_substeps: usize, + /// The type of friction constraints used in the simulation. + #[cfg(feature = "dim3")] + pub friction_model: FrictionModel, } impl IntegrationParameters { /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz). /// /// This is zero if `self.dt` is zero. - #[inline(always)] + #[inline] pub fn inv_dt(&self) -> Real { if self.dt == 0.0 { 0.0 } else { 1.0 / self.dt } } @@ -260,12 +282,10 @@ impl IntegrationParameters { pub fn prediction_distance(&self) -> Real { self.normalized_prediction_distance * self.length_unit } +} - /// Initialize the simulation parameters with settings matching the TGS-soft solver - /// with warmstarting. - /// - /// This is the default configuration, equivalent to [`IntegrationParameters::default()`]. - pub fn tgs_soft() -> Self { +impl Default for IntegrationParameters { + fn default() -> Self { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, @@ -275,9 +295,8 @@ impl IntegrationParameters { joint_damping_ratio: 1.0, warmstart_coefficient: 1.0, num_internal_pgs_iterations: 1, - num_internal_stabilization_iterations: 2, - num_additional_friction_iterations: 0, - num_solver_iterations: NonZeroUsize::new(4).unwrap(), + num_internal_stabilization_iterations: 1, + num_solver_iterations: 4, // 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. @@ -289,37 +308,8 @@ impl IntegrationParameters { normalized_prediction_distance: 0.002, max_ccd_substeps: 1, length_unit: 1.0, - } - } - - /// Initialize the simulation parameters with settings matching the TGS-soft solver - /// **without** warmstarting. - /// - /// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless - /// warmstarting proves to be undesirable for your use-case. - pub fn tgs_soft_without_warmstart() -> Self { - Self { - contact_damping_ratio: 0.25, - warmstart_coefficient: 0.0, - num_additional_friction_iterations: 4, - ..Self::tgs_soft() - } - } - - /// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17. - /// - /// This exists mainly for testing and comparison purpose. - pub fn pgs_legacy() -> Self { - Self { - num_solver_iterations: NonZeroUsize::new(1).unwrap(), - num_internal_pgs_iterations: 4, - ..Self::tgs_soft_without_warmstart() + #[cfg(feature = "dim3")] + friction_model: FrictionModel::default(), } } } - -impl Default for IntegrationParameters { - fn default() -> Self { - Self::tgs_soft() - } -} diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 92c9dab..9f89d9b 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -11,8 +11,7 @@ use crate::utils::SimdDot; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Default)] pub struct IslandManager { - pub(crate) active_dynamic_set: Vec, - pub(crate) active_kinematic_set: Vec, + pub(crate) active_set: Vec, pub(crate) active_islands: Vec, pub(crate) active_islands_additional_solver_iterations: Vec, active_set_timestamp: u32, @@ -26,8 +25,7 @@ impl IslandManager { /// Creates a new empty island manager. pub fn new() -> Self { Self { - active_dynamic_set: vec![], - active_kinematic_set: vec![], + active_set: vec![], active_islands: vec![], active_islands_additional_solver_iterations: vec![], active_set_timestamp: 0, @@ -42,26 +40,22 @@ impl IslandManager { /// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`. pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) { - let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + let mut i = 0; - for active_set in &mut active_sets { - let mut i = 0; + while i < self.active_set.len() { + let handle = self.active_set[i]; + if bodies.get(handle).is_none() { + // This rigid-body no longer exists, so we need to remove it from the active set. + self.active_set.swap_remove(i); - while i < active_set.len() { - let handle = active_set[i]; - if bodies.get(handle).is_none() { - // This rigid-body no longer exists, so we need to remove it from the active set. - active_set.swap_remove(i); - - if i < active_set.len() { - // Update the active_set_id for the body that has been swapped. - if let Some(swapped_rb) = bodies.get_mut_internal(active_set[i]) { - swapped_rb.ids.active_set_id = i; - } + if i < self.active_set.len() { + // Update the self.active_set_id for the body that has been swapped. + if let Some(swapped_rb) = bodies.get_mut_internal(self.active_set[i]) { + swapped_rb.ids.active_set_id = i; } - } else { - i += 1; } + } else { + i += 1; } } } @@ -72,18 +66,15 @@ impl IslandManager { removed_ids: &RigidBodyIds, bodies: &mut RigidBodySet, ) { - let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set]; + if self.active_set.get(removed_ids.active_set_id) == Some(&removed_handle) { + self.active_set.swap_remove(removed_ids.active_set_id); - for active_set in &mut active_sets { - if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) { - active_set.swap_remove(removed_ids.active_set_id); - - if let Some(replacement) = active_set - .get(removed_ids.active_set_id) - .and_then(|h| bodies.get_mut_internal(*h)) - { - replacement.ids.active_set_id = removed_ids.active_set_id; - } + if let Some(replacement) = self + .active_set + .get(removed_ids.active_set_id) + .and_then(|h| bodies.get_mut_internal(*h)) + { + replacement.ids.active_set_id = removed_ids.active_set_id; } } } @@ -104,41 +95,27 @@ impl IslandManager { if !rb.changes.contains(RigidBodyChanges::SLEEP) { rb.activation.wake_up(strong); - if rb.is_enabled() - && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle) - { - rb.ids.active_set_id = self.active_dynamic_set.len(); - self.active_dynamic_set.push(handle); + if rb.is_enabled() && self.active_set.get(rb.ids.active_set_id) != Some(&handle) { + rb.ids.active_set_id = self.active_set.len(); + self.active_set.push(handle); } } } } - /// Iter through all the active kinematic rigid-bodies on this set. - pub fn active_kinematic_bodies(&self) -> &[RigidBodyHandle] { - &self.active_kinematic_set[..] - } - - /// Iter through all the active dynamic rigid-bodies on this set. - pub fn active_dynamic_bodies(&self) -> &[RigidBodyHandle] { - &self.active_dynamic_set[..] - } - pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - &self.active_dynamic_set[island_range] + &self.active_set[island_range] } pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { self.active_islands_additional_solver_iterations[island_id] } - #[inline(always)] - pub(crate) fn iter_active_bodies(&self) -> impl Iterator + '_ { - self.active_dynamic_set - .iter() - .copied() - .chain(self.active_kinematic_set.iter().copied()) + /// Handls of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). + #[inline] + pub fn active_bodies(&self) -> &[RigidBodyHandle] { + &self.active_set } #[cfg(feature = "parallel")] @@ -171,10 +148,10 @@ impl IslandManager { self.can_sleep.clear(); // NOTE: the `.rev()` is here so that two successive timesteps preserve - // the order of the bodies in the `active_dynamic_set` vec. This reversal + // the order of the bodies in the `active_set` vec. This reversal // does not seem to affect performances nor stability. However it makes // debugging slightly nicer. - for h in self.active_dynamic_set.drain(..).rev() { + for h in self.active_set.drain(..).rev() { let can_sleep = &mut self.can_sleep; let stack = &mut self.stack; @@ -196,7 +173,7 @@ impl IslandManager { } // Read all the contacts and push objects touching touching this rigid-body. - #[inline(always)] + #[inline] fn push_contacting_bodies( rb_colliders: &RigidBodyColliders, colliders: &ColliderSet, @@ -221,20 +198,6 @@ impl IslandManager { } } - // Now iterate on all active kinematic bodies and push all the bodies - // touching them to the stack so they can be woken up. - for h in self.active_kinematic_set.iter() { - let rb = &bodies[*h]; - - if rb.vels.is_zero() { - // If the kinematic body does not move, it does not have - // to wake up any dynamic body. - continue; - } - - push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); - } - // println!("Selection: {}", Instant::now() - t); // let t = Instant::now(); @@ -258,7 +221,9 @@ impl IslandManager { while let Some(handle) = self.stack.pop() { let rb = bodies.index_mut_internal(handle); - if rb.ids.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() { + if rb.ids.active_set_timestamp == self.active_set_timestamp + || !rb.is_dynamic_or_kinematic() + { // We already visited this body and its neighbors. // Also, we don't propagate awake state through fixed bodies. continue; @@ -266,13 +231,13 @@ impl IslandManager { if self.stack.len() < island_marker { if additional_solver_iterations != rb.additional_solver_iterations - || self.active_dynamic_set.len() - *self.active_islands.last().unwrap() + || self.active_set.len() - *self.active_islands.last().unwrap() >= min_island_size { // We are starting a new island. self.active_islands_additional_solver_iterations .push(additional_solver_iterations); - self.active_islands.push(self.active_dynamic_set.len()); + self.active_islands.push(self.active_set.len()); additional_solver_iterations = 0; } @@ -297,17 +262,17 @@ impl IslandManager { rb.activation.wake_up(false); rb.ids.active_island_id = self.active_islands.len() - 1; - rb.ids.active_set_id = self.active_dynamic_set.len(); + rb.ids.active_set_id = self.active_set.len(); rb.ids.active_set_offset = - rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]; + (rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]) as u32; rb.ids.active_set_timestamp = self.active_set_timestamp; - self.active_dynamic_set.push(handle); + self.active_set.push(handle); } self.active_islands_additional_solver_iterations .push(additional_solver_iterations); - self.active_islands.push(self.active_dynamic_set.len()); + self.active_islands.push(self.active_set.len()); // println!( // "Extraction: {}, num islands: {}", // Instant::now() - t, diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs index 1732820..19a02cb 100644 --- a/src/dynamics/joint/generic_joint.rs +++ b/src/dynamics/joint/generic_joint.rs @@ -1,7 +1,9 @@ #![allow(clippy::bad_bit_mask)] // Clippy will complain about the bitmasks due to JointAxesMask::FREE_FIXED_AXES being 0. use crate::dynamics::solver::MotorParameters; -use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint}; +use crate::dynamics::{ + FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RigidBody, RopeJoint, +}; use crate::math::{Isometry, Point, Real, Rotation, SPATIAL_DIM, UnitVector, Vector}; use crate::utils::{SimdBasis, SimdRealCopy}; @@ -230,13 +232,13 @@ pub struct GenericJoint { /// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized /// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF. pub coupled_axes: JointAxesMask, - /// The limits, along each degrees of freedoms of this joint. + /// The limits, along each degree of freedoms of this joint. /// /// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask. /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis` /// bitmask is applied to the coupled linear (resp. angular) axes. pub limits: [JointLimits; SPATIAL_DIM], - /// The motors, along each degrees of freedoms of this joint. + /// The motors, along each degree of freedoms of this joint. /// /// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask. /// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes` @@ -244,7 +246,7 @@ pub struct GenericJoint { pub motors: [JointMotor; SPATIAL_DIM], /// Are contacts between the attached rigid-bodies enabled? pub contacts_enabled: bool, - /// Whether or not the joint is enabled. + /// Whether the joint is enabled. pub enabled: JointEnabled, /// User-defined data associated to this joint. pub user_data: u128, @@ -516,6 +518,20 @@ impl GenericJoint { self.motors[dim].target_pos = -self.motors[dim].target_pos; } } + + pub(crate) fn transform_to_solver_body_space(&mut self, rb1: &RigidBody, rb2: &RigidBody) { + if rb1.is_fixed() { + self.local_frame1 = rb1.pos.position * self.local_frame1; + } else { + self.local_frame1.translation.vector -= rb1.mprops.local_mprops.local_com.coords; + } + + if rb2.is_fixed() { + self.local_frame2 = rb2.pos.position * self.local_frame2; + } else { + self.local_frame2.translation.vector -= rb2.mprops.local_mprops.local_com.coords; + } + } } macro_rules! joint_conversion_methods( diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 013fa32..c3039b2 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -311,11 +311,11 @@ impl ImpulseJointSet { let rb2 = &bodies[joint.body2]; if joint.data.is_enabled() - && (rb1.is_dynamic() || rb2.is_dynamic()) - && (!rb1.is_dynamic() || !rb1.is_sleeping()) - && (!rb2.is_dynamic() || !rb2.is_sleeping()) + && (rb1.is_dynamic_or_kinematic() || rb2.is_dynamic_or_kinematic()) + && (!rb1.is_dynamic_or_kinematic() || !rb1.is_sleeping()) + && (!rb2.is_dynamic_or_kinematic() || !rb2.is_sleeping()) { - let island_index = if !rb1.is_dynamic() { + let island_index = if !rb1.is_dynamic_or_kinematic() { rb2.ids.active_island_id } else { rb1.ids.active_island_id diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 473e662..0396bef 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -86,7 +86,7 @@ pub struct Multibody { ndofs: usize, pub(crate) root_is_dynamic: bool, - pub(crate) solver_id: usize, + pub(crate) solver_id: u32, self_contacts_enabled: bool, /* @@ -822,7 +822,7 @@ impl Multibody { /// The generalized velocity at the multibody_joint of the given link. #[inline] - pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView { + pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<'_, Real> { let ndofs = link.joint().ndofs(); DVectorView::from_slice( &self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs], @@ -832,13 +832,13 @@ impl Multibody { /// The generalized accelerations of this multibodies. #[inline] - pub fn generalized_acceleration(&self) -> DVectorView { + pub fn generalized_acceleration(&self) -> DVectorView<'_, Real> { self.accelerations.rows(0, self.ndofs) } /// The generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity(&self) -> DVectorView { + pub fn generalized_velocity(&self) -> DVectorView<'_, Real> { self.velocities.rows(0, self.ndofs) } @@ -850,7 +850,7 @@ impl Multibody { /// The mutable generalized velocities of this multibodies. #[inline] - pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut { + pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<'_, Real> { self.velocities.rows_mut(0, self.ndofs) } @@ -971,7 +971,8 @@ impl Multibody { } if update_mass_properties { - rb.mprops.update_world_mass_properties(&link.local_to_world); + rb.mprops + .update_world_mass_properties(rb.body_type, &link.local_to_world); } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 1806de0..84886ca 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::solver::JointGenericOneBodyConstraint; +use crate::dynamics::solver::GenericJointConstraint; use crate::dynamics::{ FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink, RigidBodyVelocity, joint, @@ -185,7 +185,7 @@ impl MultibodyJoint { /// Multiply the multibody_joint jacobian by generalized velocities to obtain the /// relative velocity of the multibody link containing this multibody_joint. - pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity { + pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity { let locked_bits = self.data.locked_axes.bits(); let mut result = RigidBodyVelocity::zero(); let mut curr_free_dof = 0; @@ -269,7 +269,7 @@ impl MultibodyJoint { link: &MultibodyLink, mut j_id: usize, jacobians: &mut DVector, - constraints: &mut [JointGenericOneBodyConstraint], + constraints: &mut [GenericJointConstraint], ) -> usize { let j_id = &mut j_id; let locked_bits = self.data.locked_axes.bits(); diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs index 611cb49..3edb302 100644 --- a/src/dynamics/joint/multibody_joint/multibody_link.rs +++ b/src/dynamics/joint/multibody_joint/multibody_link.rs @@ -27,7 +27,7 @@ pub struct MultibodyLink { pub(crate) shift23: Vector, /// The velocity added by the joint, in world-space. - pub(crate) joint_velocity: RigidBodyVelocity, + pub(crate) joint_velocity: RigidBodyVelocity, } impl MultibodyLink { diff --git a/src/dynamics/joint/multibody_joint/multibody_workspace.rs b/src/dynamics/joint/multibody_joint/multibody_workspace.rs index 3bad390..651d425 100644 --- a/src/dynamics/joint/multibody_joint/multibody_workspace.rs +++ b/src/dynamics/joint/multibody_joint/multibody_workspace.rs @@ -6,7 +6,7 @@ use na::DVector; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug)] pub(crate) struct MultibodyWorkspace { - pub accs: Vec, + pub accs: Vec>, pub ndofs_vec: DVector, } diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index d6efd12..0f8f71d 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -1,7 +1,7 @@ #![allow(missing_docs)] // For downcast. use crate::dynamics::joint::MultibodyLink; -use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId}; +use crate::dynamics::solver::{GenericJointConstraint, WritebackId}; use crate::dynamics::{IntegrationParameters, JointMotor, Multibody}; use crate::math::Real; use na::DVector; @@ -17,7 +17,7 @@ pub fn unit_joint_limit_constraint( dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, - constraints: &mut [JointGenericOneBodyConstraint], + constraints: &mut [GenericJointConstraint], insert_at: &mut usize, ) { let ndofs = multibody.ndofs(); @@ -42,11 +42,17 @@ pub fn unit_joint_limit_constraint( max_enabled as u32 as Real * Real::MAX, ]; - let constraint = JointGenericOneBodyConstraint { + let constraint = GenericJointConstraint { + is_rigid_body1: false, + solver_vel1: u32::MAX, + ndofs1: 0, + j_id1: 0, + + is_rigid_body2: false, solver_vel2: multibody.solver_id, ndofs2: ndofs, j_id2: *j_id, - joint_id: usize::MAX, + joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet. impulse: 0.0, impulse_bounds, inv_lhs: crate::utils::inv(lhs), @@ -75,7 +81,7 @@ pub fn unit_joint_motor_constraint( dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, - constraints: &mut [JointGenericOneBodyConstraint], + constraints: &mut [GenericJointConstraint], insert_at: &mut usize, ) { let inv_dt = params.inv_dt(); @@ -108,11 +114,17 @@ pub fn unit_joint_motor_constraint( rhs_wo_bias += -target_vel; - let constraint = JointGenericOneBodyConstraint { + let constraint = GenericJointConstraint { + is_rigid_body1: false, + solver_vel1: u32::MAX, + ndofs1: 0, + j_id1: 0, + + is_rigid_body2: false, solver_vel2: multibody.solver_id, ndofs2: ndofs, j_id2: *j_id, - joint_id: usize::MAX, + joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet. impulse: 0.0, impulse_bounds, cfm_coeff: motor_params.cfm_coeff, diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 3e68402..75a9155 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -4,6 +4,10 @@ pub use self::ccd::CCDSolver; pub use self::coefficient_combine_rule::CoefficientCombineRule; pub use self::integration_parameters::IntegrationParameters; pub use self::island_manager::IslandManager; + +#[cfg(feature = "dim3")] +pub use self::integration_parameters::FrictionModel; + pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::joint::JointIndex; pub use self::joint::*; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 3323f01..026a416 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -17,18 +17,18 @@ use num::Zero; /// /// To create a new rigid-body, use the [`RigidBodyBuilder`] structure. #[derive(Debug, Clone)] +// #[repr(C)] +// #[repr(align(64))] pub struct RigidBody { - pub(crate) pos: RigidBodyPosition, - pub(crate) mprops: RigidBodyMassProps, - // NOTE: we need this so that the CCD can use the actual velocities obtained - // by the velocity solver with bias. If we switch to interpolation, we - // should remove this field. - pub(crate) integrated_vels: RigidBodyVelocity, - pub(crate) vels: RigidBodyVelocity, - pub(crate) damping: RigidBodyDamping, - pub(crate) forces: RigidBodyForces, - pub(crate) ccd: RigidBodyCcd, pub(crate) ids: RigidBodyIds, + pub(crate) pos: RigidBodyPosition, + pub(crate) damping: RigidBodyDamping, + pub(crate) vels: RigidBodyVelocity, + pub(crate) forces: RigidBodyForces, + pub(crate) mprops: RigidBodyMassProps, + + pub(crate) ccd_vels: RigidBodyVelocity, + pub(crate) ccd: RigidBodyCcd, pub(crate) colliders: RigidBodyColliders, /// Whether or not this rigid-body is sleeping. pub(crate) activation: RigidBodyActivation, @@ -54,7 +54,7 @@ impl RigidBody { Self { pos: RigidBodyPosition::default(), mprops: RigidBodyMassProps::default(), - integrated_vels: RigidBodyVelocity::default(), + ccd_vels: RigidBodyVelocity::default(), vels: RigidBodyVelocity::default(), damping: RigidBodyDamping::default(), forces: RigidBodyForces::default(), @@ -98,7 +98,7 @@ impl RigidBody { let RigidBody { pos, mprops, - integrated_vels, + ccd_vels: integrated_vels, vels, damping, forces, @@ -116,7 +116,7 @@ impl RigidBody { self.pos = *pos; self.mprops = mprops.clone(); - self.integrated_vels = *integrated_vels; + self.ccd_vels = *integrated_vels; self.vels = *vels; self.damping = *damping; self.forces = *forces; @@ -224,7 +224,7 @@ impl RigidBody { self.vels = RigidBodyVelocity::zero(); } - if self.is_dynamic() && wake_up { + if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); } } @@ -261,7 +261,7 @@ impl RigidBody { #[inline] pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) { if locked_axes != self.mprops.flags { - if self.is_dynamic() && wake_up { + if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); } @@ -280,7 +280,7 @@ impl RigidBody { /// Locks or unlocks all the rotations of this rigid-body. pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) { if locked != self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED) { - if self.is_dynamic() && wake_up { + if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); } @@ -304,7 +304,7 @@ impl RigidBody { || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y || self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z { - if self.is_dynamic() && wake_up { + if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); } @@ -342,7 +342,7 @@ impl RigidBody { /// Locks or unlocks all the rotations of this rigid-body. pub fn lock_translations(&mut self, locked: bool, wake_up: bool) { if locked != self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED) { - if self.is_dynamic() && wake_up { + if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); } @@ -378,7 +378,7 @@ impl RigidBody { return; } - if self.is_dynamic() && wake_up { + if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); } @@ -498,6 +498,7 @@ impl RigidBody { self.mprops.recompute_mass_properties_from_colliders( colliders, &self.colliders, + self.body_type, &self.pos.position, ); } @@ -565,7 +566,7 @@ impl RigidBody { self.changes.insert(RigidBodyChanges::LOCAL_MASS_PROPERTIES); self.mprops.additional_local_mprops = new_mprops; - if self.is_dynamic() && wake_up { + if self.is_dynamic_or_kinematic() && wake_up { self.wake_up(true); } } @@ -590,6 +591,26 @@ impl RigidBody { self.body_type.is_kinematic() } + /// Is this rigid-body a dynamic rigid-body or a kinematic rigid-body? + /// + /// This method is mostly convenient internally where kinematic and dynamic rigid-body + /// are subject to the same behavior. + pub fn is_dynamic_or_kinematic(&self) -> bool { + self.body_type.is_dynamic_or_kinematic() + } + + /// The offset index in the solver’s active set, or `u32::MAX` if + /// the rigid-body isn’t dynamic or kinematic. + // TODO: is this really necessary? Could we just always assign u32::MAX + // to all the fixed bodies active set offsets? + pub fn effective_active_set_offset(&self) -> u32 { + if self.is_dynamic_or_kinematic() { + self.ids.active_set_offset + } else { + u32::MAX + } + } + /// Is this rigid body fixed? /// /// A fixed body cannot move and is not affected by forces. @@ -653,6 +674,7 @@ impl RigidBody { co_mprops: &ColliderMassProps, ) { self.colliders.attach_collider( + self.body_type, &mut self.changes, &mut self.ccd, &mut self.mprops, @@ -710,7 +732,7 @@ impl RigidBody { } /// The linear and angular velocity of this rigid-body. - pub fn vels(&self) -> &RigidBodyVelocity { + pub fn vels(&self) -> &RigidBodyVelocity { &self.vels } @@ -735,7 +757,7 @@ impl RigidBody { /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was /// put to sleep because it did not move for a while. - pub fn set_vels(&mut self, vels: RigidBodyVelocity, wake_up: bool) { + pub fn set_vels(&mut self, vels: RigidBodyVelocity, wake_up: bool) { self.set_linvel(vels.linvel, wake_up); self.set_angvel(vels.angvel, wake_up); } @@ -831,7 +853,7 @@ impl RigidBody { self.update_world_mass_properties(); // TODO: Do we really need to check that the body isn't dynamic? - if wake_up && self.is_dynamic() { + if wake_up && self.is_dynamic_or_kinematic() { self.wake_up(true) } } @@ -855,7 +877,7 @@ impl RigidBody { self.update_world_mass_properties(); // TODO: Do we really need to check that the body isn't dynamic? - if wake_up && self.is_dynamic() { + if wake_up && self.is_dynamic_or_kinematic() { self.wake_up(true) } } @@ -880,7 +902,7 @@ impl RigidBody { self.update_world_mass_properties(); // TODO: Do we really need to check that the body isn't dynamic? - if wake_up && self.is_dynamic() { + if wake_up && self.is_dynamic_or_kinematic() { self.wake_up(true) } } @@ -890,13 +912,21 @@ impl RigidBody { pub fn set_next_kinematic_rotation(&mut self, rotation: Rotation) { if self.is_kinematic() { self.pos.next_position.rotation = rotation; + + if self.pos.position.rotation != rotation { + self.wake_up(true); + } } } /// If this rigid body is kinematic, sets its future translation after the next timestep integration. pub fn set_next_kinematic_translation(&mut self, translation: Vector) { if self.is_kinematic() { - self.pos.next_position.translation = translation.into(); + self.pos.next_position.translation.vector = translation; + + if self.pos.position.translation.vector != translation { + self.wake_up(true); + } } } @@ -905,6 +935,10 @@ impl RigidBody { pub fn set_next_kinematic_position(&mut self, pos: Isometry) { if self.is_kinematic() { self.pos.next_position = pos; + + if self.pos.position != pos { + self.wake_up(true); + } } } @@ -946,7 +980,8 @@ impl RigidBody { } pub(crate) fn update_world_mass_properties(&mut self) { - self.mprops.update_world_mass_properties(&self.pos.position); + self.mprops + .update_world_mass_properties(self.body_type, &self.pos.position); } } @@ -1053,8 +1088,7 @@ impl RigidBody { #[profiling::function] pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic { - self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt - * (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse); + self.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse; if wake_up { self.wake_up(true); @@ -1069,8 +1103,7 @@ impl RigidBody { #[profiling::function] pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic { - self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt - * (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse); + self.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse; if wake_up { self.wake_up(true); @@ -1113,6 +1146,23 @@ impl RigidBody { AngVector::zero() } } + + /// Are gyroscopic forces enabled for rigid-bodies? + #[cfg(feature = "dim3")] + pub fn gyroscopic_forces_enabled(&self) -> bool { + self.forces.gyroscopic_forces_enabled + } + + /// Enables or disables gyroscopic forces for this rigid-body. + /// + /// Enabling gyroscopic forces allows more realistic behaviors like gyroscopic precession, + /// but result in a slight performance overhead. + /// + /// Disabled by default. + #[cfg(feature = "dim3")] + pub fn enable_gyroscopic_forces(&mut self, enabled: bool) { + self.forces.gyroscopic_forces_enabled = enabled; + } } impl RigidBody { @@ -1140,6 +1190,29 @@ impl RigidBody { -self.mass() * self.forces.gravity_scale * gravity.dot(&world_com) } + + /// Computes the angular velocity of this rigid-body after application of gyroscopic forces. + #[cfg(feature = "dim3")] + pub fn angvel_with_gyroscopic_forces(&self, dt: Real) -> AngVector { + // NOTE: integrating the gyroscopic forces implicitly are both slower and + // very dissipative. Instead, we only keep the explicit term and + // ensure angular momentum is preserved (similar to Jolt). + let w = self.pos.position.inverse_transform_vector(self.angvel()); + let i = self.mprops.local_mprops.principal_inertia(); + let ii = self.mprops.local_mprops.inv_principal_inertia; + let curr_momentum = i.component_mul(&w); + let explicit_gyro_momentum = -w.cross(&curr_momentum) * dt; + let total_momentum = curr_momentum + explicit_gyro_momentum; + let total_momentum_sqnorm = total_momentum.norm_squared(); + + if total_momentum_sqnorm != 0.0 { + let capped_momentum = + total_momentum * (curr_momentum.norm_squared() / total_momentum_sqnorm).sqrt(); + self.pos.position * (ii.component_mul(&capped_momentum)) + } else { + *self.angvel() + } + } } /// A builder for rigid-bodies. @@ -1193,6 +1266,8 @@ pub struct RigidBodyBuilder { /// /// See [`RigidBody::set_additional_solver_iterations`] for additional information. pub additional_solver_iterations: usize, + /// Are gyroscopic forces enabled for this rigid-body? + pub gyroscopic_forces_enabled: bool, } impl Default for RigidBodyBuilder { @@ -1222,6 +1297,7 @@ impl RigidBodyBuilder { enabled: true, user_data: 0, additional_solver_iterations: 0, + gyroscopic_forces_enabled: false, } } @@ -1295,11 +1371,18 @@ impl RigidBodyBuilder { } /// Sets the initial position (translation and orientation) of the rigid-body to be created. + #[deprecated = "renamed to `RigidBodyBuilder::pose`"] pub fn position(mut self, pos: Isometry) -> Self { self.position = pos; self } + /// Sets the initial pose (translation and orientation) of the rigid-body to be created. + pub fn pose(mut self, pos: Isometry) -> Self { + self.position = pos; + self + } + /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder. pub fn user_data(mut self, data: u128) -> Self { self.user_data = data; @@ -1491,6 +1574,18 @@ impl RigidBodyBuilder { self } + /// Are gyroscopic forces enabled for this rigid-body? + /// + /// Enabling gyroscopic forces allows more realistic behaviors like gyroscopic precession, + /// but result in a slight performance overhead. + /// + /// Disabled by default. + #[cfg(feature = "dim3")] + pub fn gyroscopic_forces_enabled(mut self, enabled: bool) -> Self { + self.gyroscopic_forces_enabled = enabled; + self + } + /// Enable or disable the rigid-body after its creation. pub fn enabled(mut self, enabled: bool) -> Self { self.enabled = enabled; @@ -1519,6 +1614,10 @@ impl RigidBodyBuilder { rb.damping.linear_damping = self.linear_damping; rb.damping.angular_damping = self.angular_damping; rb.forces.gravity_scale = self.gravity_scale; + #[cfg(feature = "dim3")] + { + rb.forces.gyroscopic_forces_enabled = self.gyroscopic_forces_enabled; + } rb.dominance = RigidBodyDominance(self.dominance_group); rb.enabled = self.enabled; rb.enable_ccd(self.ccd_enabled); diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index e214d14..579feb2 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1,6 +1,8 @@ #[cfg(doc)] use super::IntegrationParameters; use crate::control::PdErrors; +#[cfg(doc)] +use crate::control::PidController; use crate::dynamics::MassProperties; use crate::geometry::{ ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition, @@ -9,12 +11,9 @@ use crate::geometry::{ use crate::math::{ AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, }; -use crate::utils::{SimdAngularInertia, SimdCross, SimdDot}; +use crate::utils::{SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy}; use num::Zero; -#[cfg(doc)] -use crate::control::PidController; - /// The unique handle of a rigid body added to a `RigidBodySet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash, Default)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -87,6 +86,14 @@ impl RigidBodyType { self == RigidBodyType::KinematicPositionBased || self == RigidBodyType::KinematicVelocityBased } + + /// Is this rigid-body a dynamic rigid-body or a kinematic rigid-body? + /// + /// This method is mostly convenient internally where kinematic and dynamic rigid-body + /// are subject to the same behavior. + pub fn is_dynamic_or_kinematic(self) -> bool { + self != RigidBodyType::Fixed + } } bitflags::bitflags! { @@ -150,7 +157,11 @@ impl RigidBodyPosition { /// Computes the velocity need to travel from `self.position` to `self.next_position` in /// a time equal to `1.0 / inv_dt`. #[must_use] - pub fn interpolate_velocity(&self, inv_dt: Real, local_com: &Point) -> RigidBodyVelocity { + pub fn interpolate_velocity( + &self, + inv_dt: Real, + local_com: &Point, + ) -> RigidBodyVelocity { let pose_err = self.pose_errors(local_com); RigidBodyVelocity { linvel: pose_err.linear * inv_dt, @@ -166,7 +177,7 @@ impl RigidBodyPosition { &self, dt: Real, forces: &RigidBodyForces, - vels: &RigidBodyVelocity, + vels: &RigidBodyVelocity, mprops: &RigidBodyMassProps, ) -> Isometry { let new_vels = forces.integrate(dt, vels, mprops); @@ -285,21 +296,22 @@ impl Default for RigidBodyAdditionalMassProps { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug, PartialEq)] +// #[repr(C)] /// The mass properties of a rigid-body. pub struct RigidBodyMassProps { - /// Flags for locking rotation and translation. - pub flags: LockedAxes, - /// The local mass properties of the rigid-body. - pub local_mprops: MassProperties, - /// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders. - pub additional_local_mprops: Option>, /// The world-space center of mass of the rigid-body. pub world_com: Point, /// The inverse mass taking into account translation locking. pub effective_inv_mass: Vector, /// The square-root of the world-space inverse angular inertia tensor of the rigid-body, /// taking into account rotation locking. - pub effective_world_inv_inertia_sqrt: AngularInertia, + pub effective_world_inv_inertia: AngularInertia, + /// The local mass properties of the rigid-body. + pub local_mprops: MassProperties, + /// Flags for locking rotation and translation. + pub flags: LockedAxes, + /// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders. + pub additional_local_mprops: Option>, } impl Default for RigidBodyMassProps { @@ -310,7 +322,7 @@ impl Default for RigidBodyMassProps { additional_local_mprops: None, world_com: Point::origin(), effective_inv_mass: Vector::zero(), - effective_world_inv_inertia_sqrt: AngularInertia::zero(), + effective_world_inv_inertia: AngularInertia::zero(), } } } @@ -350,9 +362,9 @@ impl RigidBodyMassProps { /// The square root of the effective world-space angular inertia (that takes the potential rotation locking into account) of /// this rigid-body. #[must_use] - pub fn effective_angular_inertia_sqrt(&self) -> AngularInertia { + pub fn effective_angular_inertia(&self) -> AngularInertia { #[allow(unused_mut)] // mut needed in 3D. - let mut ang_inertia = self.effective_world_inv_inertia_sqrt; + let mut ang_inertia = self.effective_world_inv_inertia; // Make the matrix invertible. #[cfg(feature = "dim3")] @@ -388,18 +400,12 @@ impl RigidBodyMassProps { result } - /// The effective world-space angular inertia (that takes the potential rotation locking into account) of - /// this rigid-body. - #[must_use] - pub fn effective_angular_inertia(&self) -> AngularInertia { - self.effective_angular_inertia_sqrt().squared() - } - /// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders. pub fn recompute_mass_properties_from_colliders( &mut self, colliders: &ColliderSet, attached_colliders: &RigidBodyColliders, + body_type: RigidBodyType, position: &Isometry, ) { let added_mprops = self @@ -434,53 +440,56 @@ impl RigidBodyMassProps { } } - self.update_world_mass_properties(position); + self.update_world_mass_properties(body_type, position); } /// Update the world-space mass properties of `self`, taking into account the new position. - pub fn update_world_mass_properties(&mut self, position: &Isometry) { + pub fn update_world_mass_properties( + &mut self, + body_type: RigidBodyType, + position: &Isometry, + ) { self.world_com = self.local_mprops.world_com(position); self.effective_inv_mass = Vector::repeat(self.local_mprops.inv_mass); - self.effective_world_inv_inertia_sqrt = - self.local_mprops.world_inv_inertia_sqrt(&position.rotation); + self.effective_world_inv_inertia = self.local_mprops.world_inv_inertia(&position.rotation); // Take into account translation/rotation locking. - if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) { + if !body_type.is_dynamic() || self.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) { self.effective_inv_mass.x = 0.0; } - if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) { + if !body_type.is_dynamic() || self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) { self.effective_inv_mass.y = 0.0; } #[cfg(feature = "dim3")] - if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) { + if !body_type.is_dynamic() || self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) { self.effective_inv_mass.z = 0.0; } #[cfg(feature = "dim2")] { - if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { - self.effective_world_inv_inertia_sqrt = 0.0; + if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { + self.effective_world_inv_inertia = 0.0; } } #[cfg(feature = "dim3")] { - if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) { - self.effective_world_inv_inertia_sqrt.m11 = 0.0; - self.effective_world_inv_inertia_sqrt.m12 = 0.0; - self.effective_world_inv_inertia_sqrt.m13 = 0.0; + if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_X) { + self.effective_world_inv_inertia.m11 = 0.0; + self.effective_world_inv_inertia.m12 = 0.0; + self.effective_world_inv_inertia.m13 = 0.0; } - if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) { - self.effective_world_inv_inertia_sqrt.m22 = 0.0; - self.effective_world_inv_inertia_sqrt.m12 = 0.0; - self.effective_world_inv_inertia_sqrt.m23 = 0.0; + if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) { + self.effective_world_inv_inertia.m22 = 0.0; + self.effective_world_inv_inertia.m12 = 0.0; + self.effective_world_inv_inertia.m23 = 0.0; } - if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { - self.effective_world_inv_inertia_sqrt.m33 = 0.0; - self.effective_world_inv_inertia_sqrt.m13 = 0.0; - self.effective_world_inv_inertia_sqrt.m23 = 0.0; + if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) { + self.effective_world_inv_inertia.m33 = 0.0; + self.effective_world_inv_inertia.m13 = 0.0; + self.effective_world_inv_inertia.m23 = 0.0; } } } @@ -489,20 +498,20 @@ impl RigidBodyMassProps { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug, Copy, PartialEq)] /// The velocities of this rigid-body. -pub struct RigidBodyVelocity { +pub struct RigidBodyVelocity { /// The linear velocity of the rigid-body. - pub linvel: Vector, + pub linvel: Vector, /// The angular velocity of the rigid-body. - pub angvel: AngVector, + pub angvel: AngVector, } -impl Default for RigidBodyVelocity { +impl Default for RigidBodyVelocity { fn default() -> Self { Self::zero() } } -impl RigidBodyVelocity { +impl RigidBodyVelocity { /// Create a new rigid-body velocity component. #[must_use] pub fn new(linvel: Vector, angvel: AngVector) -> Self { @@ -618,15 +627,6 @@ impl RigidBodyVelocity { 0.5 * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel)) } - /// Returns the update velocities after applying the given damping. - #[must_use] - pub fn apply_damping(&self, dt: Real, damping: &RigidBodyDamping) -> Self { - RigidBodyVelocity { - linvel: self.linvel * (1.0 / (1.0 + dt * damping.linear_damping)), - angvel: self.angvel * (1.0 / (1.0 + dt * damping.angular_damping)), - } - } - /// The velocity of the given world-space point on this rigid-body. #[must_use] pub fn velocity_at_point(&self, point: &Point, world_com: &Point) -> Vector { @@ -634,23 +634,6 @@ impl RigidBodyVelocity { self.linvel + self.angvel.gcross(dpt) } - /// Integrate the velocities in `self` to compute obtain new positions when moving from the given - /// initial position `init_pos`. - #[must_use] - pub fn integrate( - &self, - dt: Real, - init_pos: &Isometry, - local_com: &Point, - ) -> Isometry { - let com = init_pos * local_com; - let shift = Translation::from(com.coords); - let mut result = - shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() * init_pos; - result.rotation.renormalize_fast(); - result - } - /// Are these velocities exactly equal to zero? #[must_use] pub fn is_zero(&self) -> bool { @@ -664,17 +647,15 @@ impl RigidBodyVelocity { let mut energy = (rb_mprops.mass() * self.linvel.norm_squared()) / 2.0; #[cfg(feature = "dim2")] - if !rb_mprops.effective_world_inv_inertia_sqrt.is_zero() { - let inertia_sqrt = 1.0 / rb_mprops.effective_world_inv_inertia_sqrt; - energy += (inertia_sqrt * self.angvel).powi(2) / 2.0; + if !rb_mprops.effective_world_inv_inertia.is_zero() { + let inertia = 1.0 / rb_mprops.effective_world_inv_inertia; + energy += inertia * self.angvel * self.angvel / 2.0; } #[cfg(feature = "dim3")] - if !rb_mprops.effective_world_inv_inertia_sqrt.is_zero() { - let inertia_sqrt = rb_mprops - .effective_world_inv_inertia_sqrt - .inverse_unchecked(); - energy += (inertia_sqrt * self.angvel).norm_squared() / 2.0; + if !rb_mprops.effective_world_inv_inertia.is_zero() { + let inertia = rb_mprops.effective_world_inv_inertia.inverse_unchecked(); + energy += self.angvel.dot(&(inertia * self.angvel)) / 2.0; } energy @@ -692,8 +673,7 @@ impl RigidBodyVelocity { /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim2")] pub fn apply_torque_impulse(&mut self, rb_mprops: &RigidBodyMassProps, torque_impulse: Real) { - self.angvel += rb_mprops.effective_world_inv_inertia_sqrt - * (rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); + self.angvel += rb_mprops.effective_world_inv_inertia * torque_impulse; } /// Applies an angular impulse at the center-of-mass of this rigid-body. @@ -705,8 +685,7 @@ impl RigidBodyVelocity { rb_mprops: &RigidBodyMassProps, torque_impulse: Vector, ) { - self.angvel += rb_mprops.effective_world_inv_inertia_sqrt - * (rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse); + self.angvel += rb_mprops.effective_world_inv_inertia * torque_impulse; } /// Applies an impulse at the given world-space point of this rigid-body. @@ -724,7 +703,74 @@ impl RigidBodyVelocity { } } -impl std::ops::Mul for RigidBodyVelocity { +impl RigidBodyVelocity { + /// Returns the update velocities after applying the given damping. + #[must_use] + pub fn apply_damping(&self, dt: T, damping: &RigidBodyDamping) -> Self { + let one = T::one(); + RigidBodyVelocity { + linvel: self.linvel * (one / (one + dt * damping.linear_damping)), + angvel: self.angvel * (one / (one + dt * damping.angular_damping)), + } + } + + /// Integrate the velocities in `self` to compute obtain new positions when moving from the given + /// initial position `init_pos`. + #[must_use] + #[inline] + pub fn integrate(&self, dt: T, init_pos: &Isometry, local_com: &Point) -> Isometry { + let com = init_pos * local_com; + let shift = Translation::from(com.coords); + let mut result = + shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() * init_pos; + result.rotation.renormalize_fast(); + result + } + + /// Same as [`Self::integrate`] but with a local center-of-mass assumed to be zero. + #[must_use] + #[inline] + pub fn integrate_centered(&self, dt: T, mut pose: Isometry) -> Isometry { + pose.translation.vector += self.linvel * dt; + pose.rotation = Rotation::new(self.angvel * dt) * pose.rotation; + pose.rotation.renormalize_fast(); + pose + } + + /// Same as [`Self::integrate`] but with the angular part linearized and the local + /// center-of-mass assumed to be zero. + #[inline] + #[cfg(feature = "dim2")] + pub fn integrate_linearized(&self, dt: T, pose: &mut Isometry) { + let dang = self.angvel * dt; + let new_cos = pose.rotation.re - dang * pose.rotation.im; + let new_sin = pose.rotation.im + dang * pose.rotation.re; + pose.rotation = Rotation::from_cos_sin_unchecked(new_cos, new_sin); + // NOTE: don’t use renormalize_fast since the linearization might cause more drift. + // TODO: check for zeros? + pose.rotation.renormalize(); + pose.translation.vector += self.linvel * dt; + } + + /// Same as [`Self::integrate`] but with the angular part linearized and the local + /// center-of-mass assumed to be zero. + #[inline] + #[cfg(feature = "dim3")] + pub fn integrate_linearized(&self, dt: T, pose: &mut Isometry) { + // Rotations linearization is inspired from + // https://ahrs.readthedocs.io/en/latest/filters/angular.html (not using the matrix form). + let hang = self.angvel * (dt * T::splat(0.5)); + // Quaternion identity + `hang` seen as a quaternion. + let id_plus_hang = na::Quaternion::new(T::one(), hang.x, hang.y, hang.z); + pose.rotation = Rotation::new_unchecked(id_plus_hang * pose.rotation.into_inner()); + // NOTE: don’t use renormalize_fast since the linearization might cause more drift. + // TODO: check for zeros? + pose.rotation.renormalize(); + pose.translation.vector += self.linvel * dt; + } +} + +impl std::ops::Mul for RigidBodyVelocity { type Output = Self; fn mul(self, rhs: Real) -> Self { @@ -735,7 +781,7 @@ impl std::ops::Mul for RigidBodyVelocity { } } -impl std::ops::Add for RigidBodyVelocity { +impl std::ops::Add> for RigidBodyVelocity { type Output = Self; fn add(self, rhs: Self) -> Self { @@ -746,14 +792,14 @@ impl std::ops::Add for RigidBodyVelocity { } } -impl std::ops::AddAssign for RigidBodyVelocity { +impl std::ops::AddAssign> for RigidBodyVelocity { fn add_assign(&mut self, rhs: Self) { self.linvel += rhs.linvel; self.angvel += rhs.angvel; } } -impl std::ops::Sub for RigidBodyVelocity { +impl std::ops::Sub> for RigidBodyVelocity { type Output = Self; fn sub(self, rhs: Self) -> Self { @@ -764,7 +810,7 @@ impl std::ops::Sub for RigidBodyVelocity { } } -impl std::ops::SubAssign for RigidBodyVelocity { +impl std::ops::SubAssign> for RigidBodyVelocity { fn sub_assign(&mut self, rhs: Self) { self.linvel -= rhs.linvel; self.angvel -= rhs.angvel; @@ -774,18 +820,18 @@ impl std::ops::SubAssign for RigidBodyVelocity { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Debug, Copy, PartialEq)] /// Damping factors to progressively slow down a rigid-body. -pub struct RigidBodyDamping { +pub struct RigidBodyDamping { /// Damping factor for gradually slowing down the translational motion of the rigid-body. - pub linear_damping: Real, + pub linear_damping: T, /// Damping factor for gradually slowing down the angular motion of the rigid-body. - pub angular_damping: Real, + pub angular_damping: T, } -impl Default for RigidBodyDamping { +impl Default for RigidBodyDamping { fn default() -> Self { Self { - linear_damping: 0.0, - angular_damping: 0.0, + linear_damping: T::zero(), + angular_damping: T::zero(), } } } @@ -805,6 +851,9 @@ pub struct RigidBodyForces { pub user_force: Vector, /// Torque applied by the user. pub user_torque: AngVector, + /// Are gyroscopic forces enabled for this rigid-body? + #[cfg(feature = "dim3")] + pub gyroscopic_forces_enabled: bool, } impl Default for RigidBodyForces { @@ -815,6 +864,8 @@ impl Default for RigidBodyForces { gravity_scale: 1.0, user_force: na::zero(), user_torque: na::zero(), + #[cfg(feature = "dim3")] + gyroscopic_forces_enabled: false, } } } @@ -825,12 +876,11 @@ impl RigidBodyForces { pub fn integrate( &self, dt: Real, - init_vels: &RigidBodyVelocity, + init_vels: &RigidBodyVelocity, mprops: &RigidBodyMassProps, - ) -> RigidBodyVelocity { + ) -> RigidBodyVelocity { let linear_acc = self.force.component_mul(&mprops.effective_inv_mass); - let angular_acc = mprops.effective_world_inv_inertia_sqrt - * (mprops.effective_world_inv_inertia_sqrt * self.torque); + let angular_acc = mprops.effective_world_inv_inertia * self.torque; RigidBodyVelocity { linvel: init_vels.linvel + linear_acc * dt, @@ -898,7 +948,7 @@ impl Default for RigidBodyCcd { impl RigidBodyCcd { /// The maximum velocity any point of any collider attached to this rigid-body /// moving with the given velocity can have. - pub fn max_point_velocity(&self, vels: &RigidBodyVelocity) -> Real { + pub fn max_point_velocity(&self, vels: &RigidBodyVelocity) -> Real { #[cfg(feature = "dim2")] return vels.linvel.norm() + vels.angvel.abs() * self.ccd_max_dist; #[cfg(feature = "dim3")] @@ -909,7 +959,7 @@ impl RigidBodyCcd { pub fn is_moving_fast( &self, dt: Real, - vels: &RigidBodyVelocity, + vels: &RigidBodyVelocity, forces: Option<&RigidBodyForces>, ) -> bool { // NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we @@ -937,15 +987,26 @@ impl RigidBodyCcd { } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Default, Clone, Debug, Copy, PartialEq, Eq, Hash)] +#[derive(Clone, Debug, Copy, PartialEq, Eq, Hash)] /// Internal identifiers used by the physics engine. pub struct RigidBodyIds { pub(crate) active_island_id: usize, pub(crate) active_set_id: usize, - pub(crate) active_set_offset: usize, + pub(crate) active_set_offset: u32, pub(crate) active_set_timestamp: u32, } +impl Default for RigidBodyIds { + fn default() -> Self { + Self { + active_island_id: usize::MAX, + active_set_id: usize::MAX, + active_set_offset: u32::MAX, + active_set_timestamp: 0, + } + } +} + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Default, Clone, Debug, PartialEq, Eq)] /// The set of colliders attached to this rigid-bodies. @@ -974,6 +1035,7 @@ impl RigidBodyColliders { /// Attach a collider to this rigid-body. pub fn attach_collider( &mut self, + rb_type: RigidBodyType, rb_changes: &mut RigidBodyChanges, rb_ccd: &mut RigidBodyCcd, rb_mprops: &mut RigidBodyMassProps, @@ -1002,7 +1064,7 @@ impl RigidBodyColliders { .transform_by(&co_parent.pos_wrt_parent); self.0.push(co_handle); rb_mprops.local_mprops += mass_properties; - rb_mprops.update_world_mass_properties(&rb_pos.position); + rb_mprops.update_world_mass_properties(rb_type, &rb_pos.position); } /// Update the positions of all the colliders attached to this rigid-body. @@ -1035,7 +1097,7 @@ pub struct RigidBodyDominance(pub i8); impl RigidBodyDominance { /// The actual dominance group of this rigid-body, after taking into account its type. pub fn effective_group(&self, status: &RigidBodyType) -> i16 { - if status.is_dynamic() { + if status.is_dynamic_or_kinematic() { self.0 as i16 } else { i8::MAX as i16 + 1 diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 33e1873..cc233af 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -1,6 +1,7 @@ use crate::data::{Arena, HasModifiedFlag, ModifiedObjects}; use crate::dynamics::{ - ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyChanges, RigidBodyHandle, + ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyBuilder, + RigidBodyChanges, RigidBodyHandle, }; use crate::geometry::ColliderSet; use std::ops::{Index, IndexMut}; @@ -46,6 +47,8 @@ pub struct RigidBodySet { // Could we avoid this? pub(crate) bodies: Arena, pub(crate) modified_bodies: ModifiedRigidBodies, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) default_fixed: RigidBody, } impl RigidBodySet { @@ -54,6 +57,7 @@ impl RigidBodySet { RigidBodySet { bodies: Arena::new(), modified_bodies: ModifiedObjects::default(), + default_fixed: RigidBodyBuilder::fixed().build(), } } @@ -62,6 +66,7 @@ impl RigidBodySet { RigidBodySet { bodies: Arena::with_capacity(capacity), modified_bodies: ModifiedRigidBodies::with_capacity(capacity), + default_fixed: RigidBodyBuilder::fixed().build(), } } diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index cef9c9e..fe9560a 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -6,9 +6,7 @@ pub(crate) fn categorize_contacts( multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], - out_one_body: &mut Vec, out_two_body: &mut Vec, - out_generic_one_body: &mut Vec, out_generic_two_body: &mut Vec, ) { for manifold_i in manifold_indices { @@ -25,13 +23,7 @@ pub(crate) fn categorize_contacts( .and_then(|h| multibody_joints.rigid_body_link(h)) .is_some() { - if manifold.data.relative_dominance != 0 { - out_generic_one_body.push(*manifold_i); - } else { - out_generic_two_body.push(*manifold_i); - } - } else if manifold.data.relative_dominance != 0 { - out_one_body.push(*manifold_i) + out_generic_two_body.push(*manifold_i); } else { out_two_body.push(*manifold_i) } @@ -39,30 +31,19 @@ pub(crate) fn categorize_contacts( } pub(crate) fn categorize_joints( - bodies: &RigidBodySet, multibody_joints: &MultibodyJointSet, impulse_joints: &[JointGraphEdge], joint_indices: &[JointIndex], - one_body_joints: &mut Vec, two_body_joints: &mut Vec, - generic_one_body_joints: &mut Vec, generic_two_body_joints: &mut Vec, ) { for joint_i in joint_indices { let joint = &impulse_joints[*joint_i].weight; - let rb1 = &bodies[joint.body1.0]; - let rb2 = &bodies[joint.body2.0]; if multibody_joints.rigid_body_link(joint.body1).is_some() || multibody_joints.rigid_body_link(joint.body2).is_some() { - if !rb1.is_dynamic() || !rb2.is_dynamic() { - generic_one_body_joints.push(*joint_i); - } else { - generic_two_body_joints.push(*joint_i); - } - } else if !rb1.is_dynamic() || !rb2.is_dynamic() { - one_body_joints.push(*joint_i); + generic_two_body_joints.push(*joint_i); } else { two_body_joints.push(*joint_i); } diff --git a/src/dynamics/solver/contact_constraint/any_contact_constraint.rs b/src/dynamics/solver/contact_constraint/any_contact_constraint.rs new file mode 100644 index 0000000..31f9e67 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/any_contact_constraint.rs @@ -0,0 +1,63 @@ +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::dynamics::solver::{ContactWithCoulombFriction, GenericContactConstraint}; +use crate::math::Real; +use na::DVector; + +#[cfg(feature = "dim3")] +use crate::dynamics::solver::ContactWithTwistFriction; +use crate::prelude::ContactManifold; + +#[derive(Debug)] +pub enum AnyContactConstraintMut<'a> { + Generic(&'a mut GenericContactConstraint), + WithCoulombFriction(&'a mut ContactWithCoulombFriction), + #[cfg(feature = "dim3")] + WithTwistFriction(&'a mut ContactWithTwistFriction), +} + +impl AnyContactConstraintMut<'_> { + pub fn remove_bias(&mut self) { + match self { + Self::Generic(c) => c.remove_cfm_and_bias_from_rhs(), + Self::WithCoulombFriction(c) => c.remove_cfm_and_bias_from_rhs(), + #[cfg(feature = "dim3")] + Self::WithTwistFriction(c) => c.remove_cfm_and_bias_from_rhs(), + } + } + pub fn warmstart( + &mut self, + generic_jacobians: &DVector, + solver_vels: &mut SolverBodies, + generic_solver_vels: &mut DVector, + ) { + match self { + Self::Generic(c) => c.warmstart(generic_jacobians, solver_vels, generic_solver_vels), + Self::WithCoulombFriction(c) => c.warmstart(solver_vels), + #[cfg(feature = "dim3")] + Self::WithTwistFriction(c) => c.warmstart(solver_vels), + } + } + + pub fn solve( + &mut self, + generic_jacobians: &DVector, + bodies: &mut SolverBodies, + generic_solver_vels: &mut DVector, + ) { + match self { + Self::Generic(c) => c.solve(generic_jacobians, bodies, generic_solver_vels, true, true), + Self::WithCoulombFriction(c) => c.solve(bodies, true, true), + #[cfg(feature = "dim3")] + Self::WithTwistFriction(c) => c.solve(bodies, true, true), + } + } + + pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) { + match self { + Self::Generic(c) => c.writeback_impulses(manifolds_all), + Self::WithCoulombFriction(c) => c.writeback_impulses(manifolds_all), + #[cfg(feature = "dim3")] + Self::WithTwistFriction(c) => c.writeback_impulses(manifolds_all), + } + } +} diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/contact_constraint_element.rs similarity index 57% rename from src/dynamics/solver/contact_constraint/two_body_constraint_element.rs rename to src/dynamics/solver/contact_constraint/contact_constraint_element.rs index e7bdfd7..103983b 100644 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraint_element.rs @@ -1,14 +1,57 @@ -use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; use crate::dynamics::solver::SolverVel; use crate::math::{AngVector, DIM, TangentImpulse, Vector}; -use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; +use crate::utils::{SimdDot, SimdRealCopy}; use na::Vector2; use simba::simd::SimdValue; +#[cfg(feature = "dim3")] #[derive(Copy, Clone, Debug)] -pub(crate) struct TwoBodyConstraintTangentPart { - pub gcross1: [AngVector; DIM - 1], - pub gcross2: [AngVector; DIM - 1], +pub(crate) struct ContactConstraintTwistPart { + // pub twist_dir: AngVector, // NOTE: The torque direction equals the normal in 3D and 1.0 in 2D. + pub ii_twist_dir1: AngVector, + pub ii_twist_dir2: AngVector, + pub rhs: N, + pub impulse: N, + pub impulse_accumulator: N, + pub r: N, +} + +#[cfg(feature = "dim3")] +impl ContactConstraintTwistPart { + #[inline] + pub fn warmstart(&mut self, solver_vel1: &mut SolverVel, solver_vel2: &mut SolverVel) + where + AngVector: SimdDot, Result = N>, + { + solver_vel1.angular += self.ii_twist_dir1 * self.impulse; + solver_vel2.angular += self.ii_twist_dir2 * self.impulse; + } + + #[inline] + pub fn solve( + &mut self, + twist_dir1: &AngVector, + limit: N, + solver_vel1: &mut SolverVel, + solver_vel2: &mut SolverVel, + ) where + AngVector: SimdDot, Result = N>, + { + let dvel = twist_dir1.gdot(solver_vel1.angular - solver_vel2.angular) + self.rhs; + let new_impulse = (self.impulse - self.r * dvel).simd_clamp(-limit, limit); + let dlambda = new_impulse - self.impulse; + self.impulse = new_impulse; + solver_vel1.angular += self.ii_twist_dir1 * dlambda; + solver_vel2.angular += self.ii_twist_dir2 * dlambda; + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct ContactConstraintTangentPart { + pub torque_dir1: [AngVector; DIM - 1], + pub torque_dir2: [AngVector; DIM - 1], + pub ii_torque_dir1: [AngVector; DIM - 1], + pub ii_torque_dir2: [AngVector; DIM - 1], pub rhs: [N; DIM - 1], pub rhs_wo_bias: [N; DIM - 1], #[cfg(feature = "dim2")] @@ -25,11 +68,13 @@ pub(crate) struct TwoBodyConstraintTangentPart { pub r: [N; DIM], } -impl TwoBodyConstraintTangentPart { - fn zero() -> Self { +impl ContactConstraintTangentPart { + pub fn zero() -> Self { Self { - gcross1: [na::zero(); DIM - 1], - gcross2: [na::zero(); DIM - 1], + torque_dir1: [na::zero(); DIM - 1], + torque_dir2: [na::zero(); DIM - 1], + ii_torque_dir1: [na::zero(); DIM - 1], + ii_torque_dir2: [na::zero(); DIM - 1], rhs: [na::zero(); DIM - 1], rhs_wo_bias: [na::zero(); DIM - 1], impulse: na::zero(), @@ -61,23 +106,24 @@ impl TwoBodyConstraintTangentPart { #[cfg(feature = "dim2")] { solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0]; - solver_vel1.angular += self.gcross1[0] * self.impulse[0]; + solver_vel1.angular += self.ii_torque_dir1[0] * self.impulse[0]; solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]; - solver_vel2.angular += self.gcross2[0] * self.impulse[0]; + solver_vel2.angular += self.ii_torque_dir2[0] * self.impulse[0]; } #[cfg(feature = "dim3")] { - solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0] - + tangents1[1].component_mul(im1) * self.impulse[1]; + solver_vel1.linear += (tangents1[0] * self.impulse[0] + tangents1[1] * self.impulse[1]) + .component_mul(im1); solver_vel1.angular += - self.gcross1[0] * self.impulse[0] + self.gcross1[1] * self.impulse[1]; + self.ii_torque_dir1[0] * self.impulse[0] + self.ii_torque_dir1[1] * self.impulse[1]; - solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0] - + tangents1[1].component_mul(im2) * -self.impulse[1]; + solver_vel2.linear += (tangents1[0] * -self.impulse[0] + + tangents1[1] * -self.impulse[1]) + .component_mul(im2); solver_vel2.angular += - self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1]; + self.ii_torque_dir2[0] * self.impulse[0] + self.ii_torque_dir2[1] * self.impulse[1]; } } @@ -96,32 +142,32 @@ impl TwoBodyConstraintTangentPart { #[cfg(feature = "dim2")] { let dvel = tangents1[0].dot(&solver_vel1.linear) - + self.gcross1[0].gdot(solver_vel1.angular) + + self.torque_dir1[0].gdot(solver_vel1.angular) - tangents1[0].dot(&solver_vel2.linear) - + self.gcross2[0].gdot(solver_vel2.angular) + + self.torque_dir2[0].gdot(solver_vel2.angular) + self.rhs[0]; let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); let dlambda = new_impulse - self.impulse[0]; self.impulse[0] = new_impulse; solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda; - solver_vel1.angular += self.gcross1[0] * dlambda; + solver_vel1.angular += self.ii_torque_dir1[0] * dlambda; solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; - solver_vel2.angular += self.gcross2[0] * dlambda; + solver_vel2.angular += self.ii_torque_dir2[0] * dlambda; } #[cfg(feature = "dim3")] { let dvel_0 = tangents1[0].dot(&solver_vel1.linear) - + self.gcross1[0].gdot(solver_vel1.angular) + + self.torque_dir1[0].gdot(solver_vel1.angular) - tangents1[0].dot(&solver_vel2.linear) - + self.gcross2[0].gdot(solver_vel2.angular) + + self.torque_dir2[0].gdot(solver_vel2.angular) + self.rhs[0]; let dvel_1 = tangents1[1].dot(&solver_vel1.linear) - + self.gcross1[1].gdot(solver_vel1.angular) + + self.torque_dir1[1].gdot(solver_vel1.angular) - tangents1[1].dot(&solver_vel2.linear) - + self.gcross2[1].gdot(solver_vel2.angular) + + self.torque_dir2[1].gdot(solver_vel2.angular) + self.rhs[1]; let dvel_00 = dvel_0 * dvel_0; @@ -143,21 +189,25 @@ impl TwoBodyConstraintTangentPart { let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; - solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0] - + tangents1[1].component_mul(im1) * dlambda[1]; - solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1]; + solver_vel1.linear += + (tangents1[0] * dlambda[0] + tangents1[1] * dlambda[1]).component_mul(im1); + solver_vel1.angular += + self.ii_torque_dir1[0] * dlambda[0] + self.ii_torque_dir1[1] * dlambda[1]; - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] - + tangents1[1].component_mul(im2) * -dlambda[1]; - solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; + solver_vel2.linear += + (tangents1[0] * -dlambda[0] + tangents1[1] * -dlambda[1]).component_mul(im2); + solver_vel2.angular += + self.ii_torque_dir2[0] * dlambda[0] + self.ii_torque_dir2[1] * dlambda[1]; } } } #[derive(Copy, Clone, Debug)] -pub(crate) struct TwoBodyConstraintNormalPart { - pub gcross1: AngVector, - pub gcross2: AngVector, +pub(crate) struct ContactConstraintNormalPart { + pub torque_dir1: AngVector, + pub torque_dir2: AngVector, + pub ii_torque_dir1: AngVector, + pub ii_torque_dir2: AngVector, pub rhs: N, pub rhs_wo_bias: N, pub impulse: N, @@ -170,11 +220,13 @@ pub(crate) struct TwoBodyConstraintNormalPart { pub r_mat_elts: [N; 2], } -impl TwoBodyConstraintNormalPart { - fn zero() -> Self { +impl ContactConstraintNormalPart { + pub fn zero() -> Self { Self { - gcross1: na::zero(), - gcross2: na::zero(), + torque_dir1: na::zero(), + torque_dir2: na::zero(), + ii_torque_dir1: na::zero(), + ii_torque_dir2: na::zero(), rhs: na::zero(), rhs_wo_bias: na::zero(), impulse: na::zero(), @@ -200,10 +252,10 @@ impl TwoBodyConstraintNormalPart { solver_vel2: &mut SolverVel, ) { solver_vel1.linear += dir1.component_mul(im1) * self.impulse; - solver_vel1.angular += self.gcross1 * self.impulse; + solver_vel1.angular += self.ii_torque_dir1 * self.impulse; solver_vel2.linear += dir1.component_mul(im2) * -self.impulse; - solver_vel2.angular += self.gcross2 * self.impulse; + solver_vel2.angular += self.ii_torque_dir2 * self.impulse; } #[inline] @@ -218,22 +270,22 @@ impl TwoBodyConstraintNormalPart { ) where AngVector: SimdDot, Result = N>, { - let dvel = dir1.dot(&solver_vel1.linear) + self.gcross1.gdot(solver_vel1.angular) + let dvel = dir1.dot(&solver_vel1.linear) + self.torque_dir1.gdot(solver_vel1.angular) - dir1.dot(&solver_vel2.linear) - + self.gcross2.gdot(solver_vel2.angular) + + self.torque_dir2.gdot(solver_vel2.angular) + self.rhs; let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); let dlambda = new_impulse - self.impulse; self.impulse = new_impulse; solver_vel1.linear += dir1.component_mul(im1) * dlambda; - solver_vel1.angular += self.gcross1 * dlambda; + solver_vel1.angular += self.ii_torque_dir1 * dlambda; solver_vel2.linear += dir1.component_mul(im2) * -dlambda; - solver_vel2.angular += self.gcross2 * dlambda; + solver_vel2.angular += self.ii_torque_dir2 * dlambda; } - #[inline(always)] + #[inline] pub(crate) fn solve_mlcp_two_constraints( dvel: Vector2, prev_impulse: Vector2, @@ -280,12 +332,12 @@ impl TwoBodyConstraintNormalPart { { let dvel_lin = dir1.dot(&solver_vel1.linear) - dir1.dot(&solver_vel2.linear); let dvel_a = dvel_lin - + constraint_a.gcross1.gdot(solver_vel1.angular) - + constraint_a.gcross2.gdot(solver_vel2.angular) + + constraint_a.torque_dir1.gdot(solver_vel1.angular) + + constraint_a.torque_dir2.gdot(solver_vel2.angular) + constraint_a.rhs; let dvel_b = dvel_lin - + constraint_b.gcross1.gdot(solver_vel1.angular) - + constraint_b.gcross2.gdot(solver_vel2.angular) + + constraint_b.torque_dir1.gdot(solver_vel1.angular) + + constraint_b.torque_dir2.gdot(solver_vel2.angular) + constraint_b.rhs; let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse); @@ -305,114 +357,10 @@ impl TwoBodyConstraintNormalPart { constraint_b.impulse = new_impulse.y; solver_vel1.linear += dir1.component_mul(im1) * (dlambda.x + dlambda.y); - solver_vel1.angular += constraint_a.gcross1 * dlambda.x + constraint_b.gcross1 * dlambda.y; + solver_vel1.angular += + constraint_a.ii_torque_dir1 * dlambda.x + constraint_b.ii_torque_dir1 * dlambda.y; solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y); - solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y; - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct TwoBodyConstraintElement { - pub normal_part: TwoBodyConstraintNormalPart, - pub tangent_part: TwoBodyConstraintTangentPart, -} - -impl TwoBodyConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: TwoBodyConstraintNormalPart::zero(), - tangent_part: TwoBodyConstraintTangentPart::zero(), - } - } - - #[inline] - pub fn warmstart_group( - elements: &mut [Self], - dir1: &Vector, - #[cfg(feature = "dim3")] tangent1: &Vector, - im1: &Vector, - im2: &Vector, - solver_vel1: &mut SolverVel, - solver_vel2: &mut SolverVel, - ) { - #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = [&dir1.orthonormal_vector()]; - - for element in elements.iter_mut() { - element - .normal_part - .warmstart(dir1, im1, im2, solver_vel1, solver_vel2); - element - .tangent_part - .warmstart(tangents1, im1, im2, solver_vel1, solver_vel2); - } - } - - #[inline] - pub fn solve_group( - cfm_factor: N, - elements: &mut [Self], - dir1: &Vector, - #[cfg(feature = "dim3")] tangent1: &Vector, - im1: &Vector, - im2: &Vector, - limit: N, - solver_vel1: &mut SolverVel, - solver_vel2: &mut SolverVel, - solve_restitution: bool, - solve_friction: bool, - ) where - Vector: SimdBasis, - AngVector: SimdDot, Result = N>, - { - if solve_restitution { - if BLOCK_SOLVER_ENABLED { - for elements in elements.chunks_exact_mut(2) { - let [element_a, element_b] = elements else { - unreachable!() - }; - - TwoBodyConstraintNormalPart::solve_pair( - &mut element_a.normal_part, - &mut element_b.normal_part, - cfm_factor, - dir1, - im1, - im2, - solver_vel1, - solver_vel2, - ); - } - - // There is one constraint left to solve if there isn’t an even number. - if elements.len() % 2 == 1 { - let element = elements.last_mut().unwrap(); - element - .normal_part - .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2); - } - } else { - for element in elements.iter_mut() { - element - .normal_part - .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2); - } - } - } - - if solve_friction { - #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = [&dir1.orthonormal_vector()]; - - for element in elements.iter_mut() { - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.solve(tangents1, im1, im2, limit, solver_vel1, solver_vel2); - } - } + solver_vel2.angular += + constraint_a.ii_torque_dir2 * dlambda.x + constraint_b.ii_torque_dir2 * dlambda.y; } } diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index 4328187..57523e2 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -1,28 +1,26 @@ use crate::dynamics::solver::categorization::categorize_contacts; use crate::dynamics::solver::contact_constraint::{ - GenericOneBodyConstraint, GenericOneBodyConstraintBuilder, GenericTwoBodyConstraint, - GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder, - TwoBodyConstraint, TwoBodyConstraintBuilder, + ContactWithCoulombFriction, ContactWithCoulombFrictionBuilder, GenericContactConstraint, + GenericContactConstraintBuilder, }; -use crate::dynamics::solver::solver_body::SolverBody; -use crate::dynamics::solver::solver_vel::SolverVel; -use crate::dynamics::solver::{ConstraintTypes, SolverConstraintsSet, reset_buffer}; +use crate::dynamics::solver::interaction_groups::InteractionGroups; +use crate::dynamics::solver::reset_buffer; +use crate::dynamics::solver::solver_body::SolverBodies; use crate::dynamics::{ ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet, RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::SIMD_WIDTH; use crate::math::{MAX_MANIFOLD_POINTS, Real}; use na::DVector; use parry::math::DIM; -#[cfg(feature = "simd-is-enabled")] -use { - crate::dynamics::solver::contact_constraint::{ - OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd, - TwoBodyConstraintSimd, - }, - crate::math::SIMD_WIDTH, +use crate::dynamics::solver::contact_constraint::any_contact_constraint::AnyContactConstraintMut; +#[cfg(feature = "dim3")] +use crate::dynamics::{ + FrictionModel, + solver::contact_constraint::{ContactWithTwistFriction, ContactWithTwistFrictionBuilder}, }; #[derive(Debug)] @@ -63,30 +61,85 @@ impl ConstraintsCounts { } } -#[derive(Copy, Clone, Debug)] -pub(crate) struct ContactConstraintTypes; +pub(crate) struct ContactConstraintsSet { + pub generic_jacobians: DVector, + pub two_body_interactions: Vec, + pub generic_two_body_interactions: Vec, + pub interaction_groups: InteractionGroups, -impl ConstraintTypes for ContactConstraintTypes { - type OneBody = OneBodyConstraint; - type TwoBodies = TwoBodyConstraint; - type GenericOneBody = GenericOneBodyConstraint; - type GenericTwoBodies = GenericTwoBodyConstraint; - #[cfg(feature = "simd-is-enabled")] - type SimdOneBody = OneBodyConstraintSimd; - #[cfg(feature = "simd-is-enabled")] - type SimdTwoBodies = TwoBodyConstraintSimd; + pub generic_velocity_constraints: Vec, + pub simd_velocity_coulomb_constraints: Vec, + #[cfg(feature = "dim3")] + pub simd_velocity_twist_constraints: Vec, - type BuilderOneBody = OneBodyConstraintBuilder; - type BuilderTwoBodies = TwoBodyConstraintBuilder; - type GenericBuilderOneBody = GenericOneBodyConstraintBuilder; - type GenericBuilderTwoBodies = GenericTwoBodyConstraintBuilder; - #[cfg(feature = "simd-is-enabled")] - type SimdBuilderOneBody = SimdOneBodyConstraintBuilder; - #[cfg(feature = "simd-is-enabled")] - type SimdBuilderTwoBodies = TwoBodyConstraintBuilderSimd; + pub generic_velocity_constraints_builder: Vec, + pub simd_velocity_coulomb_constraints_builder: Vec, + #[cfg(feature = "dim3")] + pub simd_velocity_twist_constraints_builder: Vec, } -pub type ContactConstraintsSet = SolverConstraintsSet; +impl ContactConstraintsSet { + pub fn new() -> Self { + Self { + generic_jacobians: DVector::zeros(0), + two_body_interactions: vec![], + generic_two_body_interactions: vec![], + interaction_groups: InteractionGroups::new(), + generic_velocity_constraints: vec![], + simd_velocity_coulomb_constraints: vec![], + generic_velocity_constraints_builder: vec![], + simd_velocity_coulomb_constraints_builder: vec![], + #[cfg(feature = "dim3")] + simd_velocity_twist_constraints: vec![], + #[cfg(feature = "dim3")] + simd_velocity_twist_constraints_builder: vec![], + } + } + + pub fn clear_constraints(&mut self) { + self.generic_jacobians.fill(0.0); + self.generic_velocity_constraints.clear(); + self.simd_velocity_coulomb_constraints.clear(); + #[cfg(feature = "dim3")] + self.simd_velocity_twist_constraints.clear(); + } + + pub fn clear_builders(&mut self) { + self.generic_velocity_constraints_builder.clear(); + self.simd_velocity_coulomb_constraints_builder.clear(); + #[cfg(feature = "dim3")] + self.simd_velocity_twist_constraints_builder.clear(); + } + + // Returns the generic jacobians and a mutable iterator through all the constraints. + pub fn iter_constraints_mut( + &mut self, + ) -> ( + &DVector, + impl Iterator>, + ) { + let jac = &self.generic_jacobians; + let a = self + .generic_velocity_constraints + .iter_mut() + .map(AnyContactConstraintMut::Generic); + let b = self + .simd_velocity_coulomb_constraints + .iter_mut() + .map(AnyContactConstraintMut::WithCoulombFriction); + #[cfg(feature = "dim3")] + { + let c = self + .simd_velocity_twist_constraints + .iter_mut() + .map(AnyContactConstraintMut::WithTwistFriction); + (jac, a.chain(b).chain(c)) + } + + #[cfg(feature = "dim2")] + return (jac, a.chain(b)); + } +} impl ContactConstraintsSet { pub fn init_constraint_groups( @@ -99,18 +152,14 @@ impl ContactConstraintsSet { manifold_indices: &[ContactManifoldIndex], ) { self.two_body_interactions.clear(); - self.one_body_interactions.clear(); self.generic_two_body_interactions.clear(); - self.generic_one_body_interactions.clear(); categorize_contacts( bodies, multibody_joints, manifolds, manifold_indices, - &mut self.one_body_interactions, &mut self.two_body_interactions, - &mut self.generic_one_body_interactions, &mut self.generic_two_body_interactions, ); @@ -123,22 +172,13 @@ impl ContactConstraintsSet { &self.two_body_interactions, ); - self.one_body_interaction_groups.clear_groups(); - self.one_body_interaction_groups.group_manifolds( - island_id, - islands, - bodies, - manifolds, - &self.one_body_interactions, - ); - // NOTE: uncomment this do disable SIMD contact resolution. - // self.interaction_groups - // .nongrouped_interactions - // .append(&mut self.interaction_groups.simd_interactions); - // self.one_body_interaction_groups - // .nongrouped_interactions - // .append(&mut self.one_body_interaction_groups.simd_interactions); + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.simd_interactions); + // self.one_body_interaction_groups + // .nongrouped_interactions + // .append(&mut self.one_body_interaction_groups.simd_interactions); } pub fn init( @@ -146,10 +186,13 @@ impl ContactConstraintsSet { island_id: usize, islands: &IslandManager, bodies: &RigidBodySet, + solver_bodies: &SolverBodies, multibody_joints: &MultibodyJointSet, manifolds: &[&mut ContactManifold], manifold_indices: &[ContactManifoldIndex], + #[cfg(feature = "dim3")] friction_model: FrictionModel, ) { + // let t0 = std::time::Instant::now(); self.clear_constraints(); self.clear_builders(); @@ -162,32 +205,133 @@ impl ContactConstraintsSet { manifold_indices, ); + // let t_init_groups = t0.elapsed().as_secs_f32(); + // let t0 = std::time::Instant::now(); let mut jacobian_id = 0; - #[cfg(feature = "simd-is-enabled")] - { - self.simd_compute_constraints(bodies, manifolds); - } - self.compute_constraints(bodies, manifolds); self.compute_generic_constraints(bodies, multibody_joints, manifolds, &mut jacobian_id); + // let t_init_constraints = t0.elapsed().as_secs_f32(); - #[cfg(feature = "simd-is-enabled")] - { - self.simd_compute_one_body_constraints(bodies, manifolds); + // let t0 = std::time::Instant::now(); + // #[cfg(feature = "simd-is-enabled")] + // { + // self.simd_compute_constraints_bench(bodies, solver_bodies, manifolds); + // } + // let t_init_constraint_bench = t0.elapsed().as_secs_f32(); + + // let t0 = std::time::Instant::now(); + #[cfg(feature = "dim2")] + self.simd_compute_coulomb_constraints(bodies, solver_bodies, manifolds); + + #[cfg(feature = "dim3")] + match friction_model { + FrictionModel::Simplified => { + self.simd_compute_twist_constraints(bodies, solver_bodies, manifolds) + } + FrictionModel::Coulomb => { + self.simd_compute_coulomb_constraints(bodies, solver_bodies, manifolds) + } } - self.compute_one_body_constraints(bodies, manifolds); - self.compute_generic_one_body_constraints( - bodies, - multibody_joints, - manifolds, - &mut jacobian_id, - ); + + // let t_init_constraints_simd = t0.elapsed().as_secs_f32(); + // let num_simd_constraints = self.simd_velocity_constraints.len(); + // println!( + // "t_init_group: {:?}, t_init_constraints_simd: {}: {:?}, t_debug: {:?}", + // t_init_groups * 1000.0, + // num_simd_constraints, + // t_init_constraints_simd * 1000.0, + // t_init_constraint_bench * 1000.0, + // ); + // println!( + // "Solver constraints init: {}", + // t0.elapsed().as_secs_f32() * 1000.0 + // ); } - #[cfg(feature = "simd-is-enabled")] - fn simd_compute_constraints( + // #[cfg(feature = "simd-is-enabled")] + // fn simd_compute_constraints_bench( + // &mut self, + // bodies: &RigidBodySet, + // solver_bodies: &SolverBodies, + // manifolds_all: &[&mut ContactManifold], + // ) { + // let total_num_constraints = self + // .interaction_groups + // .simd_interactions + // .chunks_exact(SIMD_WIDTH) + // .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0] as usize]).num_constraints) + // .sum::(); + // + // unsafe { + // reset_buffer( + // &mut self.simd_velocity_constraints_builder, + // total_num_constraints as usize, + // ); + // reset_buffer( + // &mut self.simd_velocity_constraints, + // total_num_constraints as usize, + // ); + // } + // + // let mut curr_start = 0; + // + // let t0 = std::time::Instant::now(); + // let preload = TwoBodyConstraintBuilderSimd::collect_constraint_gen_data( + // bodies, + // &*manifolds_all, + // &self.interaction_groups.simd_interactions, + // ); + // println!("Preload: {:?}", t0.elapsed().as_secs_f32() * 1000.0); + // + // let t0 = std::time::Instant::now(); + // for i in (0..self.interaction_groups.simd_interactions.len()).step_by(SIMD_WIDTH) { + // let num_to_add = 1; // preload.solver_contact_headers[i].num_contacts; + // TwoBodyConstraintBuilderSimd::generate_bench_preloaded( + // &preload, + // i, + // solver_bodies, + // &mut self.simd_velocity_constraints_builder[curr_start..], + // &mut self.simd_velocity_constraints[curr_start..], + // ); + // + // curr_start += num_to_add; + // } + // println!("Preloaded init: {:?}", t0.elapsed().as_secs_f32() * 1000.0); + // + // /* + // for manifolds_i in self + // .interaction_groups + // .simd_interactions + // .chunks_exact(SIMD_WIDTH) + // { + // let num_to_add = + // ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; + // let manifold_id = array![|ii| manifolds_i[ii]]; + // let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]]; + // + // TwoBodyConstraintBuilderSimd::generate_bench( + // manifold_id, + // manifolds, + // bodies, + // solver_bodies, + // &mut self.simd_velocity_constraints_builder[curr_start..], + // &mut self.simd_velocity_constraints[curr_start..], + // ); + // + // curr_start += num_to_add; + // } + // */ + // + // // assert_eq!(curr_start, total_num_constraints); + // } + + // TODO: could we somehow combine that with the simd_compute_coulomb_constraints function since + // both are very similar and mutually exclusive? + #[cfg(feature = "dim3")] + fn simd_compute_twist_constraints( &mut self, bodies: &RigidBodySet, + solver_bodies: &SolverBodies, manifolds_all: &[&mut ContactManifold], ) { let total_num_constraints = self @@ -195,14 +339,23 @@ impl ContactConstraintsSet { .simd_interactions .chunks_exact(SIMD_WIDTH) .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) - .sum(); + .sum::() + + self + .interaction_groups + .nongrouped_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) + .sum::(); unsafe { reset_buffer( - &mut self.simd_velocity_constraints_builder, + &mut self.simd_velocity_twist_constraints_builder, + total_num_constraints, + ); + reset_buffer( + &mut self.simd_velocity_twist_constraints, total_num_constraints, ); - reset_buffer(&mut self.simd_velocity_constraints, total_num_constraints); } let mut curr_start = 0; @@ -214,15 +367,35 @@ impl ContactConstraintsSet { { let num_to_add = ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; - let manifold_id = gather![|ii| manifolds_i[ii]]; - let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; + let manifold_id = array![|ii| manifolds_i[ii]]; + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]]; - TwoBodyConstraintBuilderSimd::generate( + ContactWithTwistFrictionBuilder::generate( manifold_id, manifolds, bodies, - &mut self.simd_velocity_constraints_builder[curr_start..], - &mut self.simd_velocity_constraints[curr_start..], + solver_bodies, + &mut self.simd_velocity_twist_constraints_builder[curr_start..], + &mut self.simd_velocity_twist_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + + for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() { + let num_to_add = + ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints; + let mut manifold_id = [usize::MAX; SIMD_WIDTH]; + manifold_id[0] = *manifolds_i; + let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH]; + + ContactWithTwistFrictionBuilder::generate( + manifold_id, + manifolds, + bodies, + solver_bodies, + &mut self.simd_velocity_twist_constraints_builder[curr_start..], + &mut self.simd_velocity_twist_constraints[curr_start..], ); curr_start += num_to_add; @@ -231,42 +404,79 @@ impl ContactConstraintsSet { assert_eq!(curr_start, total_num_constraints); } - fn compute_constraints( + fn simd_compute_coulomb_constraints( &mut self, bodies: &RigidBodySet, + solver_bodies: &SolverBodies, manifolds_all: &[&mut ContactManifold], ) { let total_num_constraints = self .interaction_groups - .nongrouped_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum(); + .simd_interactions + .chunks_exact(SIMD_WIDTH) + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) + .sum::() + + self + .interaction_groups + .nongrouped_interactions + .iter() + .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) + .sum::(); unsafe { reset_buffer( - &mut self.velocity_constraints_builder, + &mut self.simd_velocity_coulomb_constraints_builder, + total_num_constraints, + ); + reset_buffer( + &mut self.simd_velocity_coulomb_constraints, total_num_constraints, ); - reset_buffer(&mut self.velocity_constraints, total_num_constraints); } let mut curr_start = 0; - for manifold_i in &self.interaction_groups.nongrouped_interactions { - let manifold = &manifolds_all[*manifold_i]; - let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; + for manifolds_i in self + .interaction_groups + .simd_interactions + .chunks_exact(SIMD_WIDTH) + { + let num_to_add = + ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; + let manifold_id = array![|ii| manifolds_i[ii]]; + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]]; - TwoBodyConstraintBuilder::generate( - *manifold_i, - manifold, + ContactWithCoulombFrictionBuilder::generate( + manifold_id, + manifolds, bodies, - &mut self.velocity_constraints_builder[curr_start..], - &mut self.velocity_constraints[curr_start..], + solver_bodies, + &mut self.simd_velocity_coulomb_constraints_builder[curr_start..], + &mut self.simd_velocity_coulomb_constraints[curr_start..], ); curr_start += num_to_add; } + + for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() { + let num_to_add = + ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints; + let mut manifold_id = [usize::MAX; SIMD_WIDTH]; + manifold_id[0] = *manifolds_i; + let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH]; + + ContactWithCoulombFrictionBuilder::generate( + manifold_id, + manifolds, + bodies, + solver_bodies, + &mut self.simd_velocity_coulomb_constraints_builder[curr_start..], + &mut self.simd_velocity_coulomb_constraints[curr_start..], + ); + + curr_start += num_to_add; + } + assert_eq!(curr_start, total_num_constraints); } @@ -281,14 +491,14 @@ impl ContactConstraintsSet { .generic_two_body_interactions .iter() .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum(); + .sum::(); self.generic_velocity_constraints_builder.resize( total_num_constraints, - GenericTwoBodyConstraintBuilder::invalid(), + GenericContactConstraintBuilder::invalid(), ); self.generic_velocity_constraints - .resize(total_num_constraints, GenericTwoBodyConstraint::invalid()); + .resize(total_num_constraints, GenericContactConstraint::invalid()); let mut curr_start = 0; @@ -296,7 +506,7 @@ impl ContactConstraintsSet { let manifold = &manifolds_all[*manifold_i]; let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; - GenericTwoBodyConstraintBuilder::generate( + GenericContactConstraintBuilder::generate( *manifold_i, manifold, bodies, @@ -313,181 +523,39 @@ impl ContactConstraintsSet { assert_eq!(curr_start, total_num_constraints); } - fn compute_generic_one_body_constraints( - &mut self, - bodies: &RigidBodySet, - multibody_joints: &MultibodyJointSet, - manifolds_all: &[&mut ContactManifold], - jacobian_id: &mut usize, - ) { - let total_num_constraints = self - .generic_one_body_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum(); - self.generic_velocity_one_body_constraints_builder.resize( - total_num_constraints, - GenericOneBodyConstraintBuilder::invalid(), - ); - self.generic_velocity_one_body_constraints - .resize(total_num_constraints, GenericOneBodyConstraint::invalid()); - - let mut curr_start = 0; - - for manifold_i in &self.generic_one_body_interactions { - let manifold = &manifolds_all[*manifold_i]; - let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; - - GenericOneBodyConstraintBuilder::generate( - *manifold_i, - manifold, - bodies, - multibody_joints, - &mut self.generic_velocity_one_body_constraints_builder[curr_start..], - &mut self.generic_velocity_one_body_constraints[curr_start..], - &mut self.generic_jacobians, - jacobian_id, - ); - - curr_start += num_to_add; - } - assert_eq!(curr_start, total_num_constraints); - } - - #[cfg(feature = "simd-is-enabled")] - fn simd_compute_one_body_constraints( - &mut self, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - ) { - let total_num_constraints = self - .one_body_interaction_groups - .simd_interactions - .chunks_exact(SIMD_WIDTH) - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) - .sum(); - - unsafe { - reset_buffer( - &mut self.simd_velocity_one_body_constraints_builder, - total_num_constraints, - ); - reset_buffer( - &mut self.simd_velocity_one_body_constraints, - total_num_constraints, - ); - } - - let mut curr_start = 0; - - for manifolds_i in self - .one_body_interaction_groups - .simd_interactions - .chunks_exact(SIMD_WIDTH) - { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; - let manifold_id = gather![|ii| manifolds_i[ii]]; - let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]]; - SimdOneBodyConstraintBuilder::generate( - manifold_id, - manifolds, - bodies, - &mut self.simd_velocity_one_body_constraints_builder[curr_start..], - &mut self.simd_velocity_one_body_constraints[curr_start..], - ); - curr_start += num_to_add; - } - assert_eq!(curr_start, total_num_constraints); - } - - fn compute_one_body_constraints( - &mut self, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - ) { - let total_num_constraints = self - .one_body_interaction_groups - .nongrouped_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum(); - - unsafe { - reset_buffer( - &mut self.velocity_one_body_constraints_builder, - total_num_constraints, - ); - reset_buffer( - &mut self.velocity_one_body_constraints, - total_num_constraints, - ); - } - - let mut curr_start = 0; - - for manifold_i in &self.one_body_interaction_groups.nongrouped_interactions { - let manifold = &manifolds_all[*manifold_i]; - let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; - - OneBodyConstraintBuilder::generate( - *manifold_i, - manifold, - bodies, - &mut self.velocity_one_body_constraints_builder[curr_start..], - &mut self.velocity_one_body_constraints[curr_start..], - ); - - curr_start += num_to_add; - } - assert_eq!(curr_start, total_num_constraints); - } - pub fn warmstart( &mut self, - solver_vels: &mut [SolverVel], + solver_bodies: &mut SolverBodies, generic_solver_vels: &mut DVector, ) { let (jac, constraints) = self.iter_constraints_mut(); for mut c in constraints { - c.warmstart(jac, solver_vels, generic_solver_vels); + c.warmstart(jac, solver_bodies, generic_solver_vels); } } #[profiling::function] - pub fn solve_restitution( + pub fn solve( &mut self, - solver_vels: &mut [SolverVel], + solver_bodies: &mut SolverBodies, generic_solver_vels: &mut DVector, ) { let (jac, constraints) = self.iter_constraints_mut(); for mut c in constraints { - c.solve_restitution(jac, solver_vels, generic_solver_vels); + c.solve(jac, solver_bodies, generic_solver_vels); } } #[profiling::function] - pub fn solve_restitution_wo_bias( + pub fn solve_wo_bias( &mut self, - solver_vels: &mut [SolverVel], + solver_bodies: &mut SolverBodies, generic_solver_vels: &mut DVector, ) { let (jac, constraints) = self.iter_constraints_mut(); for mut c in constraints { c.remove_bias(); - c.solve_restitution(jac, solver_vels, generic_solver_vels); - } - } - - #[profiling::function] - pub fn solve_friction( - &mut self, - solver_vels: &mut [SolverVel], - generic_solver_vels: &mut DVector, - ) { - let (jac, constraints) = self.iter_constraints_mut(); - for mut c in constraints { - c.solve_friction(jac, solver_vels, generic_solver_vels); + c.solve(jac, solver_bodies, generic_solver_vels); } } @@ -504,7 +572,7 @@ impl ContactConstraintsSet { params: &IntegrationParameters, small_step_id: usize, multibodies: &MultibodyJointSet, - solver_bodies: &[SolverBody], + solver_bodies: &SolverBodies, ) { macro_rules! update_contacts( ($builders: ident, $constraints: ident) => { @@ -524,22 +592,14 @@ impl ContactConstraintsSet { generic_velocity_constraints_builder, generic_velocity_constraints ); - update_contacts!(velocity_constraints_builder, velocity_constraints); - #[cfg(feature = "simd-is-enabled")] - update_contacts!(simd_velocity_constraints_builder, simd_velocity_constraints); - update_contacts!( - generic_velocity_one_body_constraints_builder, - generic_velocity_one_body_constraints + simd_velocity_coulomb_constraints_builder, + simd_velocity_coulomb_constraints ); + #[cfg(feature = "dim3")] update_contacts!( - velocity_one_body_constraints_builder, - velocity_one_body_constraints - ); - #[cfg(feature = "simd-is-enabled")] - update_contacts!( - simd_velocity_one_body_constraints_builder, - simd_velocity_one_body_constraints + simd_velocity_twist_constraints_builder, + simd_velocity_twist_constraints ); } } diff --git a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs new file mode 100644 index 0000000..595dbb0 --- /dev/null +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -0,0 +1,505 @@ +use super::{ContactConstraintNormalPart, ContactConstraintTangentPart}; +use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex, SimdSolverContact}; +use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH, SimdReal, Vector}; +#[cfg(feature = "dim2")] +use crate::utils::SimdBasis; +use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy}; +use num::Zero; +use parry::utils::SdpMatrix2; +use simba::simd::{SimdPartialOrd, SimdValue}; + +#[derive(Copy, Clone, Debug)] +pub struct CoulombContactPointInfos { + pub tangent_vel: Vector, // PERF: could be one float less, be shared by both contact point infos? + pub normal_vel: N, + pub local_p1: Point, + pub local_p2: Point, + pub dist: N, +} + +impl Default for CoulombContactPointInfos { + fn default() -> Self { + Self { + tangent_vel: Vector::zeros(), + normal_vel: N::zero(), + local_p1: Point::origin(), + local_p2: Point::origin(), + dist: N::zero(), + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct ContactWithCoulombFrictionBuilder { + infos: [CoulombContactPointInfos; MAX_MANIFOLD_POINTS], +} + +impl ContactWithCoulombFrictionBuilder { + pub fn generate( + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + solver_bodies: &SolverBodies, + out_builders: &mut [ContactWithCoulombFrictionBuilder], + out_constraints: &mut [ContactWithCoulombFriction], + ) { + // TODO: could we avoid having to fetch the ids here? It’s the only thing we + // read from the original rigid-bodies. + let ids1: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance <= 0 + && manifold_id[ii] != usize::MAX + { + let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check. + bodies[handle].ids.active_set_offset + } else { + u32::MAX + }]; + let ids2: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance >= 0 + && manifold_id[ii] != usize::MAX + { + let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check. + bodies[handle].ids.active_set_offset + } else { + u32::MAX + }]; + + let vels1 = solver_bodies.gather_vels(ids1); + let poses1 = solver_bodies.gather_poses(ids1); + let vels2 = solver_bodies.gather_vels(ids2); + let poses2 = solver_bodies.gather_poses(ids2); + + let world_com1 = Point::from(poses1.pose.translation.vector); + let world_com2 = Point::from(poses2.pose.translation.vector); + + // TODO PERF: implement SIMD gather + let force_dir1 = -Vector::::from(gather![|ii| manifolds[ii].data.normal]); + let num_active_contacts = manifolds[0].data.num_active_contacts(); + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear); + + for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = + array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; + let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; + + constraint.dir1 = force_dir1; + constraint.im1 = poses1.im; + constraint.im2 = poses2.im; + constraint.solver_vel1 = ids1; + constraint.solver_vel2 = ids2; + constraint.manifold_id = manifold_id; + constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + constraint.tangent1 = tangents1[0]; + } + + for k in 0..num_points { + // SAFETY: we already know that the `manifold_points` has `num_points` elements + // so `k` isn’t out of bounds. + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; + + let is_bouncy = solver_contact.is_bouncy(); + + let dp1 = solver_contact.point - world_com1; + let dp2 = solver_contact.point - world_com2; + + let vel1 = vels1.linear + vels1.angular.gcross(dp1); + let vel2 = vels2.linear + vels2.angular.gcross(dp2); + + constraint.limit = solver_contact.friction; + constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); + + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + let projected_mass = utils::simd_inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2), + ); + + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + normal_rhs_wo_bias = + is_bouncy * solver_contact.restitution * projected_velocity; + + constraint.normal_part[k].torque_dir1 = torque_dir1; + constraint.normal_part[k].torque_dir2 = torque_dir2; + constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; + constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; + constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; + constraint.normal_part[k].r = projected_mass; + } + + // tangent parts. + constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse; + + for j in 0..DIM - 1 { + let torque_dir1 = dp1.gcross(tangents1[j]); + let torque_dir2 = dp2.gcross(-tangents1[j]); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2); + let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]); + + constraint.tangent_part[k].torque_dir1[j] = torque_dir1; + constraint.tangent_part[k].torque_dir2[j] = torque_dir2; + constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; + constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; + constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; + constraint.tangent_part[k].rhs[j] = rhs_wo_bias; + constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") { + utils::simd_inv(r) + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + constraint.tangent_part[k].r[2] = SimdReal::splat(2.0) + * (constraint.tangent_part[k].ii_torque_dir1[0] + .gdot(constraint.tangent_part[k].torque_dir1[1]) + + constraint.tangent_part[k].ii_torque_dir2[0] + .gdot(constraint.tangent_part[k].torque_dir2[1])); + } + + // Builder. + builder.infos[k].local_p1 = + poses1.pose.inverse_transform_point(&solver_contact.point); + builder.infos[k].local_p2 = + poses2.pose.inverse_transform_point(&solver_contact.point); + builder.infos[k].tangent_vel = solver_contact.tangent_velocity; + builder.infos[k].dist = solver_contact.dist; + builder.infos[k].normal_vel = normal_rhs_wo_bias; + } + + if BLOCK_SOLVER_ENABLED { + // Coupling between consecutive pairs. + for k in 0..num_points / 2 { + let k0 = k * 2; + let k1 = k * 2 + 1; + + let imsum = poses1.im + poses2.im; + let r0 = constraint.normal_part[k0].r; + let r1 = constraint.normal_part[k1].r; + + let mut r_mat = SdpMatrix2::zero(); + + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) + + constraint.normal_part[k0] + .ii_torque_dir1 + .gdot(constraint.normal_part[k1].torque_dir1) + + constraint.normal_part[k0] + .ii_torque_dir2 + .gdot(constraint.normal_part[k1].torque_dir2); + r_mat.m11 = utils::simd_inv(r0); + r_mat.m22 = utils::simd_inv(r1); + + let (inv, det) = { + let _disable_fe_except = + crate::utils::DisableFloatingPointExceptionsFlags:: + disable_floating_point_exceptions(); + r_mat.inverse_and_get_determinant_unchecked() + }; + let is_invertible = det.simd_gt(SimdReal::zero()); + + // If inversion failed, the contacts are redundant. + // Ignore the one with the smallest depth (it is too late to + // have the constraint removed from the constraint set, so just + // set the mass (r) matrix elements to 0. + constraint.normal_part[k0].r_mat_elts = [ + inv.m11.select(is_invertible, r0), + inv.m22.select(is_invertible, SimdReal::zero()), + ]; + constraint.normal_part[k1].r_mat_elts = [ + inv.m12.select(is_invertible, SimdReal::zero()), + r_mat.m12.select(is_invertible, SimdReal::zero()), + ]; + } + } + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &SolverBodies, + _multibodies: &MultibodyJointSet, + constraint: &mut ContactWithCoulombFriction, + ) { + let cfm_factor = SimdReal::splat(params.contact_cfm_factor()); + let inv_dt = SimdReal::splat(params.inv_dt()); + let allowed_lin_err = SimdReal::splat(params.allowed_linear_error()); + let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt()); + let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity()); + let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); + + let poses1 = bodies.gather_poses(constraint.solver_vel1); + let poses2 = bodies.gather_poses(constraint.solver_vel2); + let all_infos = &self.infos[..constraint.num_contacts as usize]; + let normal_parts = &mut constraint.normal_part[..constraint.num_contacts as usize]; + let tangent_parts = &mut constraint.tangent_part[..constraint.num_contacts as usize]; + + #[cfg(feature = "dim2")] + let tangents1 = constraint.dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = [ + constraint.tangent1, + constraint.dir1.cross(&constraint.tangent1), + ]; + + let solved_dt = SimdReal::splat(solved_dt); + + for ((info, normal_part), tangent_part) in all_infos + .iter() + .zip(normal_parts.iter_mut()) + .zip(tangent_parts.iter_mut()) + { + // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. + let p1 = poses1.pose * info.local_p1 + info.tangent_vel * solved_dt; + let p2 = poses2.pose * info.local_p2; + let dist = info.dist + (p1 - p2).dot(&constraint.dir1); + + // Normal part. + { + let rhs_wo_bias = info.normal_vel + dist.simd_max(SimdReal::zero()) * inv_dt; + let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt) + .simd_clamp(-max_corrective_velocity, SimdReal::zero()); + let new_rhs = rhs_wo_bias + rhs_bias; + + normal_part.rhs_wo_bias = rhs_wo_bias; + normal_part.rhs = new_rhs; + normal_part.impulse_accumulator += normal_part.impulse; + normal_part.impulse *= warmstart_coeff; + } + + // tangent parts. + { + tangent_part.impulse_accumulator += tangent_part.impulse; + tangent_part.impulse *= warmstart_coeff; + + for j in 0..DIM - 1 { + let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; + tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias; + } + } + } + + constraint.cfm_factor = cfm_factor; + } +} + +#[derive(Copy, Clone, Debug)] +#[repr(C)] +pub(crate) struct ContactWithCoulombFriction { + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im1: Vector, + pub im2: Vector, + pub cfm_factor: SimdReal, + pub limit: SimdReal, + + #[cfg(feature = "dim3")] + pub tangent1: Vector, // One of the friction force directions. + pub normal_part: [ContactConstraintNormalPart; MAX_MANIFOLD_POINTS], + pub tangent_part: [ContactConstraintTangentPart; MAX_MANIFOLD_POINTS], + pub solver_vel1: [u32; SIMD_WIDTH], + pub solver_vel2: [u32; SIMD_WIDTH], + pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + pub num_contacts: u8, + pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], +} + +impl ContactWithCoulombFriction { + pub fn warmstart(&mut self, bodies: &mut SolverBodies) { + let mut solver_vel1 = bodies.gather_vels(self.solver_vel1); + let mut solver_vel2 = bodies.gather_vels(self.solver_vel2); + + let normal_parts = &mut self.normal_part[..self.num_contacts as usize]; + let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize]; + + /* + * Warmstart restitution. + */ + for normal_part in normal_parts.iter_mut() { + normal_part.warmstart( + &self.dir1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + } + + /* + * Warmstart friction. + */ + #[cfg(feature = "dim3")] + let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&self.dir1.orthonormal_vector()]; + + for tangent_part in tangent_parts.iter_mut() { + tangent_part.warmstart( + tangents1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + } + + bodies.scatter_vels(self.solver_vel1, solver_vel1); + bodies.scatter_vels(self.solver_vel2, solver_vel2); + } + + pub fn solve( + &mut self, + bodies: &mut SolverBodies, + solve_restitution: bool, + solve_friction: bool, + ) { + let mut solver_vel1 = bodies.gather_vels(self.solver_vel1); + let mut solver_vel2 = bodies.gather_vels(self.solver_vel2); + + let normal_parts = &mut self.normal_part[..self.num_contacts as usize]; + let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize]; + + /* + * Solve restitution. + */ + if solve_restitution { + if BLOCK_SOLVER_ENABLED { + for normal_part in normal_parts.chunks_exact_mut(2) { + let [normal_part_a, normal_part_b] = normal_part else { + unreachable!() + }; + + ContactConstraintNormalPart::solve_pair( + normal_part_a, + normal_part_b, + self.cfm_factor, + &self.dir1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + } + + // There is one constraint left to solve if there isn’t an even number. + if normal_parts.len() % 2 == 1 { + let normal_part = normal_parts.last_mut().unwrap(); + normal_part.solve( + self.cfm_factor, + &self.dir1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + } + } else { + for normal_part in normal_parts.iter_mut() { + normal_part.solve( + self.cfm_factor, + &self.dir1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + } + } + } + + /* + * Solve friction. + */ + if solve_friction { + #[cfg(feature = "dim3")] + let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&self.dir1.orthonormal_vector()]; + + for (tangent_part, normal_part) in tangent_parts.iter_mut().zip(normal_parts.iter()) { + let limit = self.limit * normal_part.impulse; + tangent_part.solve( + tangents1, + &self.im1, + &self.im2, + limit, + &mut solver_vel1, + &mut solver_vel2, + ); + } + } + + bodies.scatter_vels(self.solver_vel1, solver_vel1); + bodies.scatter_vels(self.solver_vel2, solver_vel2); + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + for k in 0..self.num_contacts as usize { + #[cfg(not(feature = "simd-is-enabled"))] + let warmstart_impulses: [_; SIMD_WIDTH] = [self.normal_part[k].impulse]; + #[cfg(feature = "simd-is-enabled")] + let warmstart_impulses: [_; SIMD_WIDTH] = self.normal_part[k].impulse.into(); + let warmstart_tangent_impulses = self.tangent_part[k].impulse; + #[cfg(not(feature = "simd-is-enabled"))] + let impulses: [_; SIMD_WIDTH] = [self.normal_part[k].total_impulse()]; + #[cfg(feature = "simd-is-enabled")] + let impulses: [_; SIMD_WIDTH] = self.normal_part[k].total_impulse().into(); + let tangent_impulses = self.tangent_part[k].total_impulse(); + + for ii in 0..SIMD_WIDTH { + if self.manifold_id[ii] != usize::MAX { + let manifold = &mut manifolds_all[self.manifold_id[ii]]; + let contact_id = self.manifold_contact_id[k][ii]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.warmstart_impulse = warmstart_impulses[ii]; + active_contact.data.warmstart_tangent_impulse = + warmstart_tangent_impulses.extract(ii); + active_contact.data.impulse = impulses[ii]; + active_contact.data.tangent_impulse = tangent_impulses.extract(ii); + } + } + } + } + + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = SimdReal::splat(1.0); + for elt in &mut self.normal_part { + elt.rhs = elt.rhs_wo_bias; + } + for elt in &mut self.tangent_part { + elt.rhs = elt.rhs_wo_bias; + } + } +} diff --git a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs new file mode 100644 index 0000000..0dd927a --- /dev/null +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -0,0 +1,515 @@ +use super::{ + ContactConstraintNormalPart, ContactConstraintTangentPart, ContactConstraintTwistPart, +}; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex, SimdSolverContact}; +use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH, SimdReal, Vector}; +#[cfg(feature = "dim2")] +use crate::utils::SimdBasis; +use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy}; +use num::Zero; +use simba::simd::{SimdPartialOrd, SimdValue}; + +#[derive(Copy, Clone, Debug)] +pub struct TwistContactPointInfos { + // This is different from the Coulomb version because it doesn’t + // have the `tangent_vel` per-contact here. + pub normal_vel: N, + pub local_p1: Point, + pub local_p2: Point, + pub dist: N, +} + +impl Default for TwistContactPointInfos { + fn default() -> Self { + Self { + normal_vel: N::zero(), + local_p1: Point::origin(), + local_p2: Point::origin(), + dist: N::zero(), + } + } +} + +/* + * FIXME: this involves a lot of duplicate code wrt. the contact with coulomb friction. + * Find a way to refactor so we can at least share the code for the ristution part. + */ +#[derive(Copy, Clone, Debug)] +pub(crate) struct ContactWithTwistFrictionBuilder { + infos: [TwistContactPointInfos; MAX_MANIFOLD_POINTS], + local_friction_center1: Point, + local_friction_center2: Point, + tangent_vel: Vector, +} + +impl ContactWithTwistFrictionBuilder { + pub fn generate( + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + solver_bodies: &SolverBodies, + out_builders: &mut [ContactWithTwistFrictionBuilder], + out_constraints: &mut [ContactWithTwistFriction], + ) { + // TODO: could we avoid having to fetch the ids here? It’s the only thing we + // read from the original rigid-bodies. + let ids1: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance <= 0 + && manifold_id[ii] != usize::MAX + { + let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check. + bodies[handle].ids.active_set_offset + } else { + u32::MAX + }]; + let ids2: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance >= 0 + && manifold_id[ii] != usize::MAX + { + let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check. + bodies[handle].ids.active_set_offset + } else { + u32::MAX + }]; + + let vels1 = solver_bodies.gather_vels(ids1); + let poses1 = solver_bodies.gather_poses(ids1); + let vels2 = solver_bodies.gather_vels(ids2); + let poses2 = solver_bodies.gather_poses(ids2); + + let world_com1 = Point::from(poses1.pose.translation.vector); + let world_com2 = Point::from(poses2.pose.translation.vector); + + // TODO PERF: implement SIMD gather + let force_dir1 = -Vector::::from(gather![|ii| manifolds[ii].data.normal]); + let num_active_contacts = manifolds[0].data.num_active_contacts(); + + #[cfg(feature = "dim2")] + let tangents1 = force_dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = + super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear); + + for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = + array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let inv_num_points = SimdReal::splat(1.0 / num_points as Real); + + let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; + let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; + + constraint.dir1 = force_dir1; + constraint.im1 = poses1.im; + constraint.im2 = poses2.im; + constraint.solver_vel1 = ids1; + constraint.solver_vel2 = ids2; + constraint.manifold_id = manifold_id; + constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + constraint.tangent1 = tangents1[0]; + } + + let mut friction_center = Point::origin(); + let mut twist_warmstart = na::zero(); + let mut tangent_warmstart = na::zero(); + let mut tangent_vel: Vector<_> = na::zero(); + + for k in 0..num_points { + // SAFETY: we already know that the `manifold_points` has `num_points` elements + // so `k` isn’t out of bounds. + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; + + let is_bouncy = solver_contact.is_bouncy(); + + friction_center += solver_contact.point.coords * inv_num_points; + + let dp1 = solver_contact.point - world_com1; + let dp2 = solver_contact.point - world_com2; + + let vel1 = vels1.linear + vels1.angular.gcross(dp1); + let vel2 = vels2.linear + vels2.angular.gcross(dp2); + + twist_warmstart += solver_contact.warmstart_twist_impulse * inv_num_points; + tangent_warmstart += solver_contact.warmstart_tangent_impulse * inv_num_points; + tangent_vel += solver_contact.tangent_velocity * inv_num_points; + + constraint.limit = solver_contact.friction; + constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); + + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + let projected_mass = utils::simd_inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2), + ); + + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + normal_rhs_wo_bias = + is_bouncy * solver_contact.restitution * projected_velocity; + + constraint.normal_part[k].torque_dir1 = torque_dir1; + constraint.normal_part[k].torque_dir2 = torque_dir2; + constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; + constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; + constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; + constraint.normal_part[k].r = projected_mass; + } + + // Builder. + builder.infos[k].local_p1 = + poses1.pose.inverse_transform_point(&solver_contact.point); + builder.infos[k].local_p2 = + poses2.pose.inverse_transform_point(&solver_contact.point); + builder.infos[k].dist = solver_contact.dist; + builder.infos[k].normal_vel = normal_rhs_wo_bias; + } + + /* + * Tangent/twist part + */ + constraint.tangent_part.impulse = tangent_warmstart; + constraint.twist_part.impulse = twist_warmstart; + + builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center); + builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center); + + let dp1 = friction_center - world_com1; + let dp2 = friction_center - world_com2; + + // Twist part. It has no effect when there is only one point. + if num_points > 1 { + let mut twist_dists = [SimdReal::zero(); MAX_MANIFOLD_POINTS]; + for k in 0..num_points { + // FIXME PERF: we don’t want to re-fetch here just to get the solver contact point! + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; + twist_dists[k] = nalgebra::distance(&friction_center, &solver_contact.point); + } + + let ii_twist_dir1 = poses1.ii.transform_vector(force_dir1); + let ii_twist_dir2 = poses2.ii.transform_vector(-force_dir1); + constraint.twist_part.rhs = SimdReal::zero(); + constraint.twist_part.ii_twist_dir1 = ii_twist_dir1; + constraint.twist_part.ii_twist_dir2 = ii_twist_dir2; + constraint.twist_part.r = utils::simd_inv( + ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1), + ); + constraint.twist_dists = twist_dists; + } + + // Tangent part. + for j in 0..2 { + let torque_dir1 = dp1.gcross(tangents1[j]); + let torque_dir2 = dp2.gcross(-tangents1[j]); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2); + + // TODO: add something similar to tangent velocity to the twist + // constraint for the case where the different points don’t + // have the same tangent vel? + let rhs_wo_bias = tangent_vel.dot(&tangents1[j]); + + constraint.tangent_part.torque_dir1[j] = torque_dir1; + constraint.tangent_part.torque_dir2[j] = torque_dir2; + constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1; + constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2; + constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + constraint.tangent_part.rhs[j] = rhs_wo_bias; + constraint.tangent_part.r[j] = if cfg!(feature = "dim2") { + utils::simd_inv(r) + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + constraint.tangent_part.r[2] = SimdReal::splat(2.0) + * (constraint.tangent_part.ii_torque_dir1[0] + .gdot(constraint.tangent_part.torque_dir1[1]) + + constraint.tangent_part.ii_torque_dir2[0] + .gdot(constraint.tangent_part.torque_dir2[1])); + } + } + } + + pub fn update( + &self, + params: &IntegrationParameters, + solved_dt: Real, + bodies: &SolverBodies, + _multibodies: &MultibodyJointSet, + constraint: &mut ContactWithTwistFriction, + ) { + let cfm_factor = SimdReal::splat(params.contact_cfm_factor()); + let inv_dt = SimdReal::splat(params.inv_dt()); + let allowed_lin_err = SimdReal::splat(params.allowed_linear_error()); + let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt()); + let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity()); + let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); + + let poses1 = bodies.gather_poses(constraint.solver_vel1); + let poses2 = bodies.gather_poses(constraint.solver_vel2); + let all_infos = &self.infos[..constraint.num_contacts as usize]; + let normal_parts = &mut constraint.normal_part[..constraint.num_contacts as usize]; + let tangent_part = &mut constraint.tangent_part; + let twist_part = &mut constraint.twist_part; + + #[cfg(feature = "dim2")] + let tangents1 = constraint.dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = [ + constraint.tangent1, + constraint.dir1.cross(&constraint.tangent1), + ]; + + let solved_dt = SimdReal::splat(solved_dt); + let tangent_delta = self.tangent_vel * solved_dt; + + for (info, normal_part) in all_infos.iter().zip(normal_parts.iter_mut()) { + // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. + let p1 = poses1.pose * info.local_p1 + tangent_delta; + let p2 = poses2.pose * info.local_p2; + let dist = info.dist + (p1 - p2).dot(&constraint.dir1); + + // Normal part. + { + let rhs_wo_bias = info.normal_vel + dist.simd_max(SimdReal::zero()) * inv_dt; + let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt) + .simd_clamp(-max_corrective_velocity, SimdReal::zero()); + let new_rhs = rhs_wo_bias + rhs_bias; + + normal_part.rhs_wo_bias = rhs_wo_bias; + normal_part.rhs = new_rhs; + normal_part.impulse_accumulator += normal_part.impulse; + normal_part.impulse *= warmstart_coeff; + } + } + + // tangent parts. + { + let p1 = poses1.pose * self.local_friction_center1 + tangent_delta; + let p2 = poses2.pose * self.local_friction_center2; + + for j in 0..DIM - 1 { + let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; + tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias; + } + tangent_part.impulse_accumulator += tangent_part.impulse; + tangent_part.impulse *= warmstart_coeff; + twist_part.impulse_accumulator += twist_part.impulse; + twist_part.impulse *= warmstart_coeff; + } + + constraint.cfm_factor = cfm_factor; + } +} + +#[derive(Copy, Clone, Debug)] +#[repr(C)] +pub(crate) struct ContactWithTwistFriction { + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im1: Vector, + pub im2: Vector, + pub cfm_factor: SimdReal, + pub limit: SimdReal, + + #[cfg(feature = "dim3")] + pub tangent1: Vector, // One of the friction force directions. + pub normal_part: [ContactConstraintNormalPart; MAX_MANIFOLD_POINTS], + // The twist friction model emulates coulomb with only one tangent + // constraint + one twist constraint per manifold. + pub tangent_part: ContactConstraintTangentPart, + // Twist constraint (angular-only) to compensate the lack of angular resistance on the tangent plane. + pub twist_part: ContactConstraintTwistPart, + // Distances between the friction center and the contact point. + pub twist_dists: [SimdReal; MAX_MANIFOLD_POINTS], + + pub solver_vel1: [u32; SIMD_WIDTH], + pub solver_vel2: [u32; SIMD_WIDTH], + pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + pub num_contacts: u8, + pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], +} + +impl ContactWithTwistFriction { + pub fn warmstart(&mut self, bodies: &mut SolverBodies) { + let mut solver_vel1 = bodies.gather_vels(self.solver_vel1); + let mut solver_vel2 = bodies.gather_vels(self.solver_vel2); + + let normal_parts = &mut self.normal_part[..self.num_contacts as usize]; + + /* + * Warmstart restitution. + */ + for normal_part in normal_parts.iter_mut() { + normal_part.warmstart( + &self.dir1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + } + + /* + * Warmstart friction. + */ + #[cfg(feature = "dim3")] + let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&self.dir1.orthonormal_vector()]; + + self.tangent_part.warmstart( + tangents1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + self.twist_part + .warmstart(&mut solver_vel1, &mut solver_vel2); + + bodies.scatter_vels(self.solver_vel1, solver_vel1); + bodies.scatter_vels(self.solver_vel2, solver_vel2); + } + + pub fn solve( + &mut self, + bodies: &mut SolverBodies, + solve_restitution: bool, + solve_friction: bool, + ) { + let mut solver_vel1 = bodies.gather_vels(self.solver_vel1); + let mut solver_vel2 = bodies.gather_vels(self.solver_vel2); + + let normal_parts = &mut self.normal_part[..self.num_contacts as usize]; + + /* + * Solve restitution. + */ + if solve_restitution { + for normal_part in normal_parts.iter_mut() { + normal_part.solve( + self.cfm_factor, + &self.dir1, + &self.im1, + &self.im2, + &mut solver_vel1, + &mut solver_vel2, + ); + } + } + + /* + * Solve friction. + */ + if solve_friction { + #[cfg(feature = "dim3")] + let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)]; + #[cfg(feature = "dim2")] + let tangents1 = [&self.dir1.orthonormal_vector()]; + + let mut tangent_limit = SimdReal::zero(); + let mut twist_limit = SimdReal::zero(); + for (normal_part, dist) in normal_parts.iter().zip(self.twist_dists.iter()) { + tangent_limit += normal_part.impulse; + // The twist limit is computed as the sum of impulses multiplied by the + // lever-arm length relative to the friction center. The rational is that + // the further the point is from the friction center, the stronger angular + // resistance it can offer. + twist_limit += normal_part.impulse * *dist; + } + + // Multiply by the friction coefficient. + tangent_limit *= self.limit; + twist_limit *= self.limit; + + self.tangent_part.solve( + tangents1, + &self.im1, + &self.im2, + tangent_limit, + &mut solver_vel1, + &mut solver_vel2, + ); + + // NOTE: if there is only 1 contact, the twist part has no effect. + if self.num_contacts > 1 { + self.twist_part + .solve(&self.dir1, twist_limit, &mut solver_vel1, &mut solver_vel2); + } + } + + bodies.scatter_vels(self.solver_vel1, solver_vel1); + bodies.scatter_vels(self.solver_vel2, solver_vel2); + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + let warmstart_tangent_impulses = self.tangent_part.impulse; + #[cfg(feature = "simd-is-enabled")] + let warmstart_twist_impulses: [_; SIMD_WIDTH] = self.twist_part.impulse.into(); + #[cfg(not(feature = "simd-is-enabled"))] + let warmstart_twist_impulses: Real = self.twist_part.impulse; + + for k in 0..self.num_contacts as usize { + #[cfg(not(feature = "simd-is-enabled"))] + let warmstart_impulses: [_; SIMD_WIDTH] = [self.normal_part[k].impulse]; + #[cfg(feature = "simd-is-enabled")] + let warmstart_impulses: [_; SIMD_WIDTH] = self.normal_part[k].impulse.into(); + #[cfg(not(feature = "simd-is-enabled"))] + let impulses: [_; SIMD_WIDTH] = [self.normal_part[k].total_impulse()]; + #[cfg(feature = "simd-is-enabled")] + let impulses: [_; SIMD_WIDTH] = self.normal_part[k].total_impulse().into(); + + for ii in 0..SIMD_WIDTH { + if self.manifold_id[ii] != usize::MAX { + let manifold = &mut manifolds_all[self.manifold_id[ii]]; + let contact_id = self.manifold_contact_id[k][ii]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.warmstart_impulse = warmstart_impulses[ii]; + active_contact.data.impulse = impulses[ii]; + active_contact.data.warmstart_tangent_impulse = + warmstart_tangent_impulses.extract(ii); + #[cfg(feature = "simd-is-enabled")] + { + active_contact.data.warmstart_twist_impulse = warmstart_twist_impulses[ii]; + } + #[cfg(not(feature = "simd-is-enabled"))] + { + active_contact.data.warmstart_twist_impulse = warmstart_twist_impulses; + } + } + } + } + } + + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = SimdReal::splat(1.0); + for elt in &mut self.normal_part { + elt.rhs = elt.rhs_wo_bias; + } + + self.tangent_part.rhs = self.tangent_part.rhs_wo_bias; + } +} diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs similarity index 50% rename from src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs rename to src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index aec5824..17ef870 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -1,32 +1,33 @@ -use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint}; +use crate::dynamics::solver::GenericRhs; use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{DIM, MAX_MANIFOLD_POINTS, Real}; use crate::utils::{SimdAngularInertia, SimdCross, SimdDot}; -use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; -use crate::dynamics::solver::solver_body::SolverBody; -use crate::dynamics::solver::{ContactPointInfos, SolverVel}; +use super::{ContactConstraintNormalPart, ContactConstraintTangentPart}; +use crate::dynamics::solver::CoulombContactPointInfos; +use crate::dynamics::solver::solver_body::SolverBodies; use crate::prelude::RigidBodyHandle; #[cfg(feature = "dim2")] use crate::utils::SimdBasis; use na::DVector; +use parry::math::Vector; #[derive(Copy, Clone)] -pub(crate) struct GenericTwoBodyConstraintBuilder { +pub(crate) struct GenericContactConstraintBuilder { + infos: [CoulombContactPointInfos; MAX_MANIFOLD_POINTS], handle1: RigidBodyHandle, handle2: RigidBodyHandle, ccd_thickness: Real, - inner: TwoBodyConstraintBuilder, } -impl GenericTwoBodyConstraintBuilder { +impl GenericContactConstraintBuilder { pub fn invalid() -> Self { Self { + infos: [CoulombContactPointInfos::default(); MAX_MANIFOLD_POINTS], handle1: RigidBodyHandle::invalid(), handle2: RigidBodyHandle::invalid(), ccd_thickness: Real::MAX, - inner: TwoBodyConstraintBuilder::invalid(), } } @@ -35,16 +36,24 @@ impl GenericTwoBodyConstraintBuilder { manifold: &ContactManifold, bodies: &RigidBodySet, multibodies: &MultibodyJointSet, - out_builders: &mut [GenericTwoBodyConstraintBuilder], - out_constraints: &mut [GenericTwoBodyConstraint], + out_builders: &mut [GenericContactConstraintBuilder], + out_constraints: &mut [GenericContactConstraint], jacobians: &mut DVector, jacobian_id: &mut usize, ) { - let handle1 = manifold.data.rigid_body1.unwrap(); - let handle2 = manifold.data.rigid_body2.unwrap(); + // TODO PERF: we haven’t tried to optimized this codepath yet (since it relies + // on multibodies which are already much slower than regular bodies). + let handle1 = manifold + .data + .rigid_body1 + .unwrap_or(RigidBodyHandle::invalid()); + let handle2 = manifold + .data + .rigid_body2 + .unwrap_or(RigidBodyHandle::invalid()); - let rb1 = &bodies[handle1]; - let rb2 = &bodies[handle2]; + let rb1 = &bodies.get(handle1).unwrap_or(&bodies.default_fixed); + let rb2 = &bodies.get(handle2).unwrap_or(&bodies.default_fixed); let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type); let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type); @@ -55,20 +64,22 @@ impl GenericTwoBodyConstraintBuilder { let multibody2 = multibodies .rigid_body_link(handle2) .map(|m| (&multibodies[m.multibody], m.id)); - let solver_vel1 = multibody1 - .map(|mb| mb.0.solver_id) - .unwrap_or(if type1.is_dynamic() { - rb1.ids.active_set_offset - } else { - 0 - }); - let solver_vel2 = multibody2 - .map(|mb| mb.0.solver_id) - .unwrap_or(if type2.is_dynamic() { - rb2.ids.active_set_offset - } else { - 0 - }); + let solver_vel1 = + multibody1 + .map(|mb| mb.0.solver_id) + .unwrap_or(if type1.is_dynamic_or_kinematic() { + rb1.ids.active_set_offset + } else { + u32::MAX + }); + let solver_vel2 = + multibody2 + .map(|mb| mb.0.solver_id) + .unwrap_or(if type2.is_dynamic_or_kinematic() { + rb2.ids.active_set_offset + } else { + u32::MAX + }); let force_dir1 = -manifold.data.normal; #[cfg(feature = "dim2")] @@ -98,24 +109,24 @@ impl GenericTwoBodyConstraintBuilder { let builder = &mut out_builders[l]; let constraint = &mut out_constraints[l]; - constraint.inner.dir1 = force_dir1; - constraint.inner.im1 = if type1.is_dynamic() { + constraint.dir1 = force_dir1; + constraint.im1 = if type1.is_dynamic_or_kinematic() { mprops1.effective_inv_mass } else { na::zero() }; - constraint.inner.im2 = if type2.is_dynamic() { + constraint.im2 = if type2.is_dynamic_or_kinematic() { mprops2.effective_inv_mass } else { na::zero() }; - constraint.inner.solver_vel1 = solver_vel1; - constraint.inner.solver_vel2 = solver_vel2; - constraint.inner.manifold_id = manifold_id; - constraint.inner.num_contacts = manifold_points.len() as u8; + constraint.solver_vel1 = solver_vel1; + constraint.solver_vel2 = solver_vel2; + constraint.manifold_id = manifold_id; + constraint.num_contacts = manifold_points.len() as u8; #[cfg(feature = "dim3")] { - constraint.inner.tangent1 = tangents1[0]; + constraint.tangent1 = tangents1[0]; } for k in 0..manifold_points.len() { @@ -127,8 +138,8 @@ impl GenericTwoBodyConstraintBuilder { let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - constraint.inner.limit = manifold_point.friction; - constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + constraint.limit = manifold_point.friction; + constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; // Normal part. let normal_rhs_wo_bias; @@ -136,16 +147,16 @@ impl GenericTwoBodyConstraintBuilder { let torque_dir1 = dp1.gcross(force_dir1); let torque_dir2 = dp2.gcross(-force_dir1); - let gcross1 = if type1.is_dynamic() { + let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { mprops1 - .effective_world_inv_inertia_sqrt + .effective_world_inv_inertia .transform_vector(torque_dir1) } else { na::zero() }; - let gcross2 = if type2.is_dynamic() { + let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { mprops2 - .effective_world_inv_inertia_sqrt + .effective_world_inv_inertia .transform_vector(torque_dir2) } else { na::zero() @@ -163,9 +174,9 @@ impl GenericTwoBodyConstraintBuilder { jacobians, ) .0 - } else if type1.is_dynamic() { + } else if type1.is_dynamic_or_kinematic() { force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) - + gcross1.gdot(gcross1) + + ii_torque_dir1.gdot(torque_dir1) } else { 0.0 }; @@ -182,9 +193,9 @@ impl GenericTwoBodyConstraintBuilder { jacobians, ) .0 - } else if type2.is_dynamic() { + } else if type2.is_dynamic_or_kinematic() { force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + gcross2.gdot(gcross2) + + ii_torque_dir2.gdot(torque_dir2) } else { 0.0 }; @@ -196,9 +207,11 @@ impl GenericTwoBodyConstraintBuilder { normal_rhs_wo_bias = (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - constraint.inner.elements[k].normal_part = TwoBodyConstraintNormalPart { - gcross1, - gcross2, + constraint.normal_part[k] = ContactConstraintNormalPart { + torque_dir1, + torque_dir2, + ii_torque_dir1, + ii_torque_dir2, rhs: na::zero(), rhs_wo_bias: na::zero(), impulse_accumulator: na::zero(), @@ -210,29 +223,30 @@ impl GenericTwoBodyConstraintBuilder { // Tangent parts. { - constraint.inner.elements[k].tangent_part.impulse = - manifold_point.warmstart_tangent_impulse; + constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse; for j in 0..DIM - 1 { let torque_dir1 = dp1.gcross(tangents1[j]); - let gcross1 = if type1.is_dynamic() { + let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { mprops1 - .effective_world_inv_inertia_sqrt + .effective_world_inv_inertia .transform_vector(torque_dir1) } else { na::zero() }; - constraint.inner.elements[k].tangent_part.gcross1[j] = gcross1; + constraint.tangent_part[k].torque_dir1[j] = torque_dir1; + constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; let torque_dir2 = dp2.gcross(-tangents1[j]); - let gcross2 = if type2.is_dynamic() { + let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { mprops2 - .effective_world_inv_inertia_sqrt + .effective_world_inv_inertia .transform_vector(torque_dir2) } else { na::zero() }; - constraint.inner.elements[k].tangent_part.gcross2[j] = gcross2; + constraint.tangent_part[k].torque_dir2[j] = torque_dir2; + constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { mb1.fill_jacobians( @@ -246,9 +260,9 @@ impl GenericTwoBodyConstraintBuilder { jacobians, ) .0 - } else if type1.is_dynamic() { + } else if type1.is_dynamic_or_kinematic() { force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) - + gcross1.gdot(gcross1) + + ii_torque_dir1.gdot(torque_dir1) } else { 0.0 }; @@ -265,9 +279,9 @@ impl GenericTwoBodyConstraintBuilder { jacobians, ) .0 - } else if type2.is_dynamic() { + } else if type2.is_dynamic_or_kinematic() { force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + gcross2.gdot(gcross2) + + ii_torque_dir2.gdot(torque_dir2) } else { 0.0 }; @@ -275,18 +289,18 @@ impl GenericTwoBodyConstraintBuilder { let r = crate::utils::inv(inv_r1 + inv_r2); let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); - constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias; + constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; + constraint.tangent_part[k].rhs[j] = rhs_wo_bias; // TODO: in 3D, we should take into account gcross[0].dot(gcross[1]) // in lhs. See the corresponding code on the `velocity_constraint.rs` // file. - constraint.inner.elements[k].tangent_part.r[j] = r; + constraint.tangent_part[k].r[j] = r; } } // Builder. - let infos = ContactPointInfos { + let infos = CoulombContactPointInfos { local_p1: rb1 .pos .position @@ -297,14 +311,14 @@ impl GenericTwoBodyConstraintBuilder { .inverse_transform_point(&manifold_point.point), tangent_vel: manifold_point.tangent_velocity, dist: manifold_point.dist, - normal_rhs_wo_bias, + normal_vel: normal_rhs_wo_bias, }; builder.handle1 = handle1; builder.handle2 = handle2; builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; - builder.inner.infos[k] = infos; - constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; + builder.infos[k] = infos; + constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; } let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); @@ -314,8 +328,8 @@ impl GenericTwoBodyConstraintBuilder { // reduce all ops to nothing because its ndofs will be zero. let generic_constraint_mask = (multibody1.is_some() as u8) | ((multibody2.is_some() as u8) << 1) - | (!type1.is_dynamic() as u8) - | ((!type2.is_dynamic() as u8) << 1); + | (!type1.is_dynamic_or_kinematic() as u8) + | ((!type2.is_dynamic_or_kinematic() as u8) << 1); constraint.j_id = chunk_j_id; constraint.ndofs1 = ndofs1; @@ -328,74 +342,160 @@ impl GenericTwoBodyConstraintBuilder { &self, params: &IntegrationParameters, solved_dt: Real, - bodies: &[SolverBody], + bodies: &SolverBodies, multibodies: &MultibodyJointSet, - constraint: &mut GenericTwoBodyConstraint, + constraint: &mut GenericContactConstraint, ) { - // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. - let pos1 = multibodies - .rigid_body_link(self.handle1) - .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world) - .unwrap_or_else(|| &bodies[constraint.inner.solver_vel1].position); - let pos2 = multibodies - .rigid_body_link(self.handle2) - .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world) - .unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position); + let cfm_factor = params.contact_cfm_factor(); + let inv_dt = params.inv_dt(); + let erp_inv_dt = params.contact_erp_inv_dt(); - self.inner - .update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner); + // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. + let pose1 = multibodies + .rigid_body_link(self.handle1) + .map(|m| multibodies[m.multibody].link(m.id).unwrap().local_to_world) + .unwrap_or_else(|| bodies.get_pose(constraint.solver_vel1).pose); + let pose2 = multibodies + .rigid_body_link(self.handle2) + .map(|m| multibodies[m.multibody].link(m.id).unwrap().local_to_world) + .unwrap_or_else(|| bodies.get_pose(constraint.solver_vel2).pose); + let all_infos = &self.infos[..constraint.num_contacts as usize]; + let normal_parts = &mut constraint.normal_part[..constraint.num_contacts as usize]; + let tangent_parts = &mut constraint.tangent_part[..constraint.num_contacts as usize]; + + #[cfg(feature = "dim2")] + let tangents1 = constraint.dir1.orthonormal_basis(); + #[cfg(feature = "dim3")] + let tangents1 = [ + constraint.tangent1, + constraint.dir1.cross(&constraint.tangent1), + ]; + + for ((info, normal_part), tangent_part) in all_infos + .iter() + .zip(normal_parts.iter_mut()) + .zip(tangent_parts.iter_mut()) + { + // Tangent velocity is equivalent to the first body’s surface moving artificially. + let p1 = pose1 * info.local_p1 + info.tangent_vel * solved_dt; + let p2 = pose2 * info.local_p2; + let dist = info.dist + (p1 - p2).dot(&constraint.dir1); + + // Normal part. + { + let rhs_wo_bias = info.normal_vel + dist.max(0.0) * inv_dt; + let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error())) + .clamp(-params.max_corrective_velocity(), 0.0); + let new_rhs = rhs_wo_bias + rhs_bias; + + normal_part.rhs_wo_bias = rhs_wo_bias; + normal_part.rhs = new_rhs; + normal_part.impulse_accumulator += normal_part.impulse; + normal_part.impulse *= params.warmstart_coefficient; + } + + // Tangent part. + { + tangent_part.impulse_accumulator += tangent_part.impulse; + tangent_part.impulse *= params.warmstart_coefficient; + + for j in 0..DIM - 1 { + let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; + tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias; + } + } + } + + constraint.cfm_factor = cfm_factor; } } #[derive(Copy, Clone, Debug)] -pub(crate) struct GenericTwoBodyConstraint { - // We just build the generic constraint on top of the velocity constraint, - // adding some information we can use in the generic case. - pub inner: TwoBodyConstraint, +pub(crate) struct GenericContactConstraint { + /* + * Fields specific to multibodies. + */ pub j_id: usize, pub ndofs1: usize, pub ndofs2: usize, pub generic_constraint_mask: u8, + + /* + * Fields similar to the rigid-body constraints. + */ + pub dir1: Vector, // Non-penetration force direction for the first body. + #[cfg(feature = "dim3")] + pub tangent1: Vector, // One of the friction force directions. + pub im1: Vector, + pub im2: Vector, + pub cfm_factor: Real, + pub limit: Real, + pub solver_vel1: u32, + pub solver_vel2: u32, + pub manifold_id: ContactManifoldIndex, + pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], + pub num_contacts: u8, + pub normal_part: [ContactConstraintNormalPart; MAX_MANIFOLD_POINTS], + pub tangent_part: [ContactConstraintTangentPart; MAX_MANIFOLD_POINTS], } -impl GenericTwoBodyConstraint { +impl GenericContactConstraint { pub fn invalid() -> Self { Self { - inner: TwoBodyConstraint::invalid(), j_id: usize::MAX, ndofs1: usize::MAX, ndofs2: usize::MAX, generic_constraint_mask: u8::MAX, + dir1: Vector::zeros(), + #[cfg(feature = "dim3")] + tangent1: Vector::zeros(), + im1: Vector::zeros(), + im2: Vector::zeros(), + cfm_factor: 0.0, + limit: 0.0, + solver_vel1: u32::MAX, + solver_vel2: u32::MAX, + manifold_id: ContactManifoldIndex::MAX, + manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS], + num_contacts: u8::MAX, + normal_part: [ContactConstraintNormalPart::zero(); MAX_MANIFOLD_POINTS], + tangent_part: [ContactConstraintTangentPart::zero(); MAX_MANIFOLD_POINTS], } } pub fn warmstart( &mut self, jacobians: &DVector, - solver_vels: &mut [SolverVel], + bodies: &mut SolverBodies, generic_solver_vels: &mut DVector, ) { - let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 { - GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1]) + let mut solver_vel1 = if self.solver_vel1 == u32::MAX { + GenericRhs::Fixed + } else if self.generic_constraint_mask & 0b01 == 0 { + GenericRhs::SolverVel(bodies.vels[self.solver_vel1 as usize]) } else { - GenericRhs::GenericId(self.inner.solver_vel1) + GenericRhs::GenericId(self.solver_vel1) }; - let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 { - GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2]) + let mut solver_vel2 = if self.solver_vel2 == u32::MAX { + GenericRhs::Fixed + } else if self.generic_constraint_mask & 0b10 == 0 { + GenericRhs::SolverVel(bodies.vels[self.solver_vel2 as usize]) } else { - GenericRhs::GenericId(self.inner.solver_vel2) + GenericRhs::GenericId(self.solver_vel2) }; - let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; - TwoBodyConstraintElement::generic_warmstart_group( - elements, + let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize]; + let normal_parts = &mut self.normal_part[..self.num_contacts as usize]; + Self::generic_warmstart_group( + normal_parts, + tangent_parts, jacobians, - &self.inner.dir1, + &self.dir1, #[cfg(feature = "dim3")] - &self.inner.tangent1, - &self.inner.im1, - &self.inner.im2, + &self.tangent1, + &self.im1, + &self.im2, self.ndofs1, self.ndofs2, self.j_id, @@ -405,45 +505,51 @@ impl GenericTwoBodyConstraint { ); if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 { - solver_vels[self.inner.solver_vel1] = solver_vel1; + bodies.vels[self.solver_vel1 as usize] = solver_vel1; } if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 { - solver_vels[self.inner.solver_vel2] = solver_vel2; + bodies.vels[self.solver_vel2 as usize] = solver_vel2; } } pub fn solve( &mut self, jacobians: &DVector, - solver_vels: &mut [SolverVel], + bodies: &mut SolverBodies, generic_solver_vels: &mut DVector, solve_restitution: bool, solve_friction: bool, ) { - let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 { - GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1]) + let mut solver_vel1 = if self.solver_vel1 == u32::MAX { + GenericRhs::Fixed + } else if self.generic_constraint_mask & 0b01 == 0 { + GenericRhs::SolverVel(bodies.vels[self.solver_vel1 as usize]) } else { - GenericRhs::GenericId(self.inner.solver_vel1) + GenericRhs::GenericId(self.solver_vel1) }; - let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 { - GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2]) + let mut solver_vel2 = if self.solver_vel2 == u32::MAX { + GenericRhs::Fixed + } else if self.generic_constraint_mask & 0b10 == 0 { + GenericRhs::SolverVel(bodies.vels[self.solver_vel2 as usize]) } else { - GenericRhs::GenericId(self.inner.solver_vel2) + GenericRhs::GenericId(self.solver_vel2) }; - let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; - TwoBodyConstraintElement::generic_solve_group( - self.inner.cfm_factor, - elements, + let normal_parts = &mut self.normal_part[..self.num_contacts as usize]; + let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize]; + Self::generic_solve_group( + self.cfm_factor, + normal_parts, + tangent_parts, jacobians, - &self.inner.dir1, + &self.dir1, #[cfg(feature = "dim3")] - &self.inner.tangent1, - &self.inner.im1, - &self.inner.im2, - self.inner.limit, + &self.tangent1, + &self.im1, + &self.im2, + self.limit, self.ndofs1, self.ndofs2, self.j_id, @@ -455,19 +561,34 @@ impl GenericTwoBodyConstraint { ); if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 { - solver_vels[self.inner.solver_vel1] = solver_vel1; + bodies.vels[self.solver_vel1 as usize] = solver_vel1; } if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 { - solver_vels[self.inner.solver_vel2] = solver_vel2; + bodies.vels[self.solver_vel2 as usize] = solver_vel2; } } pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - self.inner.writeback_impulses(manifolds_all); + let manifold = &mut manifolds_all[self.manifold_id]; + + for k in 0..self.num_contacts as usize { + let contact_id = self.manifold_contact_id[k]; + let active_contact = &mut manifold.points[contact_id as usize]; + active_contact.data.warmstart_impulse = self.normal_part[k].impulse; + active_contact.data.warmstart_tangent_impulse = self.tangent_part[k].impulse; + active_contact.data.impulse = self.normal_part[k].total_impulse(); + active_contact.data.tangent_impulse = self.tangent_part[k].total_impulse(); + } } pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.inner.remove_cfm_and_bias_from_rhs(); + self.cfm_factor = 1.0; + for normal_part in &mut self.normal_part { + normal_part.rhs = normal_part.rhs_wo_bias; + } + for tangent_part in &mut self.tangent_part { + tangent_part.rhs = tangent_part.rhs_wo_bias; + } } } diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint_element.rs similarity index 83% rename from src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs rename to src/dynamics/solver/contact_constraint/generic_contact_constraint_element.rs index 05279a5..b90eb46 100644 --- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint_element.rs @@ -1,7 +1,6 @@ use crate::dynamics::solver::SolverVel; -use crate::dynamics::solver::{ - TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart, -}; +use crate::dynamics::solver::contact_constraint::GenericContactConstraint; +use crate::dynamics::solver::{ContactConstraintNormalPart, ContactConstraintTangentPart}; use crate::math::{AngVector, DIM, Real, Vector}; use crate::utils::SimdDot; use na::DVector; @@ -10,37 +9,38 @@ use {crate::utils::SimdBasis, na::SimdPartialOrd}; pub(crate) enum GenericRhs { SolverVel(SolverVel), - GenericId(usize), + GenericId(u32), + Fixed, } // Offset between the jacobians of two consecutive constraints. -#[inline(always)] +#[inline] fn j_step(ndofs1: usize, ndofs2: usize) -> usize { (ndofs1 + ndofs2) * 2 } -#[inline(always)] +#[inline] fn j_id1(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize { j_id } -#[inline(always)] +#[inline] fn j_id2(j_id: usize, ndofs1: usize, _ndofs2: usize) -> usize { j_id + ndofs1 * 2 } -#[inline(always)] +#[inline] fn normal_j_id(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize { j_id } -#[inline(always)] +#[inline] fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize { j_id + (ndofs1 + ndofs2) * 2 } impl GenericRhs { - #[inline(always)] + #[inline] fn dvel( &self, j_id: usize, @@ -54,13 +54,14 @@ impl GenericRhs { GenericRhs::SolverVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular), GenericRhs::GenericId(solver_vel) => { let j = jacobians.rows(j_id, ndofs); - let rhs = solver_vels.rows(*solver_vel, ndofs); + let rhs = solver_vels.rows(*solver_vel as usize, ndofs); j.dot(&rhs) } + GenericRhs::Fixed => 0.0, } } - #[inline(always)] + #[inline] fn apply_impulse( &mut self, j_id: usize, @@ -68,26 +69,27 @@ impl GenericRhs { impulse: Real, jacobians: &DVector, dir: &Vector, - gcross: &AngVector, + ii_torque_dir: &AngVector, solver_vels: &mut DVector, inv_mass: &Vector, ) { match self { GenericRhs::SolverVel(rhs) => { rhs.linear += dir.component_mul(inv_mass) * impulse; - rhs.angular += gcross * impulse; + rhs.angular += ii_torque_dir * impulse; } GenericRhs::GenericId(solver_vel) => { let wj_id = j_id + ndofs; let wj = jacobians.rows(wj_id, ndofs); - let mut rhs = solver_vels.rows_mut(*solver_vel, ndofs); + let mut rhs = solver_vels.rows_mut(*solver_vel as usize, ndofs); rhs.axpy(impulse, &wj, 1.0); } + GenericRhs::Fixed => {} } } } -impl TwoBodyConstraintTangentPart { +impl ContactConstraintTangentPart { #[inline] pub fn generic_warmstart( &mut self, @@ -115,7 +117,7 @@ impl TwoBodyConstraintTangentPart { self.impulse[0], jacobians, tangents1[0], - &self.gcross1[0], + &self.ii_torque_dir1[0], solver_vels, im1, ); @@ -125,7 +127,7 @@ impl TwoBodyConstraintTangentPart { self.impulse[0], jacobians, &-tangents1[0], - &self.gcross2[0], + &self.ii_torque_dir2[0], solver_vels, im2, ); @@ -139,7 +141,7 @@ impl TwoBodyConstraintTangentPart { self.impulse[0], jacobians, tangents1[0], - &self.gcross1[0], + &self.ii_torque_dir1[0], solver_vels, im1, ); @@ -149,7 +151,7 @@ impl TwoBodyConstraintTangentPart { self.impulse[1], jacobians, tangents1[1], - &self.gcross1[1], + &self.ii_torque_dir1[1], solver_vels, im1, ); @@ -160,7 +162,7 @@ impl TwoBodyConstraintTangentPart { self.impulse[0], jacobians, &-tangents1[0], - &self.gcross2[0], + &self.ii_torque_dir2[0], solver_vels, im2, ); @@ -170,7 +172,7 @@ impl TwoBodyConstraintTangentPart { self.impulse[1], jacobians, &-tangents1[1], - &self.gcross2[1], + &self.ii_torque_dir2[1], solver_vels, im2, ); @@ -204,14 +206,14 @@ impl TwoBodyConstraintTangentPart { ndofs1, jacobians, tangents1[0], - &self.gcross1[0], + &self.torque_dir1[0], solver_vels, ) + solver_vel2.dvel( j_id2, ndofs2, jacobians, &-tangents1[0], - &self.gcross2[0], + &self.torque_dir2[0], solver_vels, ) + self.rhs[0]; @@ -225,7 +227,7 @@ impl TwoBodyConstraintTangentPart { dlambda, jacobians, tangents1[0], - &self.gcross1[0], + &self.ii_torque_dir1[0], solver_vels, im1, ); @@ -235,7 +237,7 @@ impl TwoBodyConstraintTangentPart { dlambda, jacobians, &-tangents1[0], - &self.gcross2[0], + &self.ii_torque_dir2[0], solver_vels, im2, ); @@ -248,14 +250,14 @@ impl TwoBodyConstraintTangentPart { ndofs1, jacobians, tangents1[0], - &self.gcross1[0], + &self.torque_dir1[0], solver_vels, ) + solver_vel2.dvel( j_id2, ndofs2, jacobians, &-tangents1[0], - &self.gcross2[0], + &self.torque_dir2[0], solver_vels, ) + self.rhs[0]; let dvel_1 = solver_vel1.dvel( @@ -263,14 +265,14 @@ impl TwoBodyConstraintTangentPart { ndofs1, jacobians, tangents1[1], - &self.gcross1[1], + &self.torque_dir1[1], solver_vels, ) + solver_vel2.dvel( j_id2 + j_step, ndofs2, jacobians, &-tangents1[1], - &self.gcross2[1], + &self.torque_dir2[1], solver_vels, ) + self.rhs[1]; @@ -289,7 +291,7 @@ impl TwoBodyConstraintTangentPart { dlambda[0], jacobians, tangents1[0], - &self.gcross1[0], + &self.ii_torque_dir1[0], solver_vels, im1, ); @@ -299,7 +301,7 @@ impl TwoBodyConstraintTangentPart { dlambda[1], jacobians, tangents1[1], - &self.gcross1[1], + &self.ii_torque_dir1[1], solver_vels, im1, ); @@ -310,7 +312,7 @@ impl TwoBodyConstraintTangentPart { dlambda[0], jacobians, &-tangents1[0], - &self.gcross2[0], + &self.ii_torque_dir2[0], solver_vels, im2, ); @@ -320,7 +322,7 @@ impl TwoBodyConstraintTangentPart { dlambda[1], jacobians, &-tangents1[1], - &self.gcross2[1], + &self.ii_torque_dir2[1], solver_vels, im2, ); @@ -328,7 +330,7 @@ impl TwoBodyConstraintTangentPart { } } -impl TwoBodyConstraintNormalPart { +impl ContactConstraintNormalPart { #[inline] pub fn generic_warmstart( &mut self, @@ -352,7 +354,7 @@ impl TwoBodyConstraintNormalPart { self.impulse, jacobians, dir1, - &self.gcross1, + &self.ii_torque_dir1, solver_vels, im1, ); @@ -362,7 +364,7 @@ impl TwoBodyConstraintNormalPart { self.impulse, jacobians, &-dir1, - &self.gcross2, + &self.ii_torque_dir2, solver_vels, im2, ); @@ -386,9 +388,21 @@ impl TwoBodyConstraintNormalPart { let j_id1 = j_id1(j_id, ndofs1, ndofs2); let j_id2 = j_id2(j_id, ndofs1, ndofs2); - let dvel = solver_vel1.dvel(j_id1, ndofs1, jacobians, dir1, &self.gcross1, solver_vels) - + solver_vel2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, solver_vels) - + self.rhs; + let dvel = solver_vel1.dvel( + j_id1, + ndofs1, + jacobians, + dir1, + &self.torque_dir1, + solver_vels, + ) + solver_vel2.dvel( + j_id2, + ndofs2, + jacobians, + &-dir1, + &self.torque_dir2, + solver_vels, + ) + self.rhs; let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); let dlambda = new_impulse - self.impulse; @@ -400,7 +414,7 @@ impl TwoBodyConstraintNormalPart { dlambda, jacobians, dir1, - &self.gcross1, + &self.ii_torque_dir1, solver_vels, im1, ); @@ -410,17 +424,18 @@ impl TwoBodyConstraintNormalPart { dlambda, jacobians, &-dir1, - &self.gcross2, + &self.ii_torque_dir2, solver_vels, im2, ); } } -impl TwoBodyConstraintElement { +impl GenericContactConstraint { #[inline] pub fn generic_warmstart_group( - elements: &mut [Self], + normal_parts: &mut [ContactConstraintNormalPart], + tangent_parts: &mut [ContactConstraintTangentPart], jacobians: &DVector, dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, @@ -442,8 +457,8 @@ impl TwoBodyConstraintElement { { let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2); - for element in elements.iter_mut() { - element.normal_part.generic_warmstart( + for normal_part in normal_parts { + normal_part.generic_warmstart( nrm_j_id, jacobians, dir1, @@ -467,9 +482,8 @@ impl TwoBodyConstraintElement { let tangents1 = [&dir1.orthonormal_vector()]; let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2); - for element in elements.iter_mut() { - let part = &mut element.tangent_part; - part.generic_warmstart( + for tangent_part in tangent_parts { + tangent_part.generic_warmstart( tng_j_id, jacobians, tangents1, @@ -489,7 +503,8 @@ impl TwoBodyConstraintElement { #[inline] pub fn generic_solve_group( cfm_factor: Real, - elements: &mut [Self], + normal_parts: &mut [ContactConstraintNormalPart], + tangent_parts: &mut [ContactConstraintTangentPart], jacobians: &DVector, dir1: &Vector, #[cfg(feature = "dim3")] tangent1: &Vector, @@ -514,8 +529,8 @@ impl TwoBodyConstraintElement { if solve_restitution { let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2); - for element in elements.iter_mut() { - element.normal_part.generic_solve( + for normal_part in &mut *normal_parts { + normal_part.generic_solve( cfm_factor, nrm_j_id, jacobians, @@ -540,10 +555,9 @@ impl TwoBodyConstraintElement { let tangents1 = [&dir1.orthonormal_vector()]; let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2); - for element in elements.iter_mut() { - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.generic_solve( + for (normal_part, tangent_part) in normal_parts.iter().zip(tangent_parts.iter_mut()) { + let limit = limit * normal_part.impulse; + tangent_part.generic_solve( tng_j_id, jacobians, tangents1, diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs deleted file mode 100644 index b2ad70c..0000000 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs +++ /dev/null @@ -1,307 +0,0 @@ -use crate::dynamics::solver::OneBodyConstraint; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity, -}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real}; -use crate::utils::SimdCross; - -use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; -use crate::dynamics::solver::solver_body::SolverBody; -use crate::dynamics::solver::{ContactPointInfos, OneBodyConstraintBuilder}; -#[cfg(feature = "dim2")] -use crate::utils::SimdBasis; -use na::DVector; - -#[derive(Copy, Clone)] -pub(crate) struct GenericOneBodyConstraintBuilder { - link2: MultibodyLinkId, - ccd_thickness: Real, - inner: OneBodyConstraintBuilder, -} - -impl GenericOneBodyConstraintBuilder { - pub fn invalid() -> Self { - Self { - link2: MultibodyLinkId::default(), - ccd_thickness: 0.0, - inner: OneBodyConstraintBuilder::invalid(), - } - } - - pub fn generate( - manifold_id: ContactManifoldIndex, - manifold: &ContactManifold, - bodies: &RigidBodySet, - multibodies: &MultibodyJointSet, - out_builders: &mut [GenericOneBodyConstraintBuilder], - out_constraints: &mut [GenericOneBodyConstraint], - jacobians: &mut DVector, - jacobian_id: &mut usize, - ) { - let mut handle1 = manifold.data.rigid_body1; - let mut handle2 = manifold.data.rigid_body2; - let flipped = manifold.data.relative_dominance < 0; - - let (force_dir1, flipped_multiplier) = if flipped { - std::mem::swap(&mut handle1, &mut handle2); - (manifold.data.normal, -1.0) - } else { - (-manifold.data.normal, 1.0) - }; - - let (vels1, world_com1) = if let Some(handle1) = handle1 { - let rb1 = &bodies[handle1]; - (rb1.vels, rb1.mprops.world_com) - } else { - (RigidBodyVelocity::zero(), Point::origin()) - }; - - let rb1 = handle1 - .map(|h| SolverBody::from(&bodies[h])) - .unwrap_or_default(); - - let rb2 = &bodies[handle2.unwrap()]; - let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); - - let link2 = *multibodies.rigid_body_link(handle2.unwrap()).unwrap(); - let (mb2, link_id2) = (&multibodies[link2.multibody], link2.id); - let solver_vel2 = mb2.solver_id; - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = - super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - - let multibodies_ndof = mb2.ndofs(); - // For each solver contact we generate DIM constraints, and each constraints appends - // the multibodies jacobian and weighted jacobians - let required_jacobian_len = - *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM; - - if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - - for (l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { - let chunk_j_id = *jacobian_id; - - let builder = &mut out_builders[l]; - let constraint = &mut out_constraints[l]; - - builder.inner.rb1 = rb1; - builder.inner.vels1 = vels1; - - constraint.inner.dir1 = force_dir1; - constraint.inner.im2 = mprops2.effective_inv_mass; - constraint.inner.solver_vel2 = solver_vel2; - constraint.inner.manifold_id = manifold_id; - constraint.inner.num_contacts = manifold_points.len() as u8; - #[cfg(feature = "dim3")] - { - constraint.inner.tangent1 = tangents1[0]; - } - - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - let point = manifold_point.point; - let dp1 = point - world_com1; - let dp2 = point - mprops2.world_com; - - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - - constraint.inner.limit = manifold_point.friction; - constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; - - // Normal part. - let normal_rhs_wo_bias; - { - let torque_dir2 = dp2.gcross(-force_dir1); - let inv_r2 = mb2 - .fill_jacobians( - link_id2, - -force_dir1, - #[cfg(feature = "dim2")] - na::vector!(torque_dir2), - #[cfg(feature = "dim3")] - torque_dir2, - jacobian_id, - jacobians, - ) - .0; - - let r = crate::utils::inv(inv_r2); - - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - - let proj_vel1 = vel1.dot(&force_dir1); - let proj_vel2 = vel2.dot(&force_dir1); - let dvel = proj_vel1 - proj_vel2; - // NOTE: we add proj_vel1 since it’s not accessible through solver_vel. - normal_rhs_wo_bias = - proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel; - - constraint.inner.elements[k].normal_part = OneBodyConstraintNormalPart { - gcross2: na::zero(), // Unused for generic constraints. - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: na::zero(), - impulse_accumulator: na::zero(), - r, - r_mat_elts: [0.0; 2], - }; - } - - // Tangent parts. - { - constraint.inner.elements[k].tangent_part.impulse = na::zero(); - - for j in 0..DIM - 1 { - let torque_dir2 = dp2.gcross(-tangents1[j]); - let inv_r2 = mb2 - .fill_jacobians( - link_id2, - -tangents1[j], - #[cfg(feature = "dim2")] - na::vector![torque_dir2], - #[cfg(feature = "dim3")] - torque_dir2, - jacobian_id, - jacobians, - ) - .0; - - let r = crate::utils::inv(inv_r2); - - let rhs_wo_bias = (vel1 - + flipped_multiplier * manifold_point.tangent_velocity) - .dot(&tangents1[j]); - - constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias; - - // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1]) - // in lhs. See the corresponding code on the `velocity_constraint.rs` - // file. - constraint.inner.elements[k].tangent_part.r[j] = r; - } - } - - // Builder. - let infos = ContactPointInfos { - local_p1: rb1.position.inverse_transform_point(&manifold_point.point), - local_p2: rb2 - .pos - .position - .inverse_transform_point(&manifold_point.point), - tangent_vel: manifold_point.tangent_velocity, - dist: manifold_point.dist, - normal_rhs_wo_bias, - }; - - builder.link2 = link2; - builder.ccd_thickness = rb2.ccd.ccd_thickness; - builder.inner.infos[k] = infos; - constraint.inner.manifold_contact_id[k] = manifold_point.contact_id; - } - - constraint.j_id = chunk_j_id; - constraint.ndofs2 = mb2.ndofs(); - } - } - - pub fn update( - &self, - params: &IntegrationParameters, - solved_dt: Real, - _solver_bodies: &[SolverBody], - multibodies: &MultibodyJointSet, - constraint: &mut GenericOneBodyConstraint, - ) { - // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint. - let pos2 = &multibodies[self.link2.multibody] - .link(self.link2.id) - .unwrap() - .local_to_world; - - self.inner - .update_with_positions(params, solved_dt, pos2, &mut constraint.inner); - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct GenericOneBodyConstraint { - // We just build the generic constraint on top of the velocity constraint, - // adding some information we can use in the generic case. - pub inner: OneBodyConstraint, - pub j_id: usize, - pub ndofs2: usize, -} - -impl GenericOneBodyConstraint { - pub fn invalid() -> Self { - Self { - inner: OneBodyConstraint::invalid(), - j_id: usize::MAX, - ndofs2: usize::MAX, - } - } - - pub fn warmstart( - &mut self, - jacobians: &DVector, - generic_solver_vels: &mut DVector, - ) { - let solver_vel2 = self.inner.solver_vel2; - - let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; - OneBodyConstraintElement::generic_warmstart_group( - elements, - jacobians, - self.ndofs2, - self.j_id, - solver_vel2, - generic_solver_vels, - ); - } - - #[profiling::function] - pub fn solve( - &mut self, - jacobians: &DVector, - generic_solver_vels: &mut DVector, - solve_restitution: bool, - solve_friction: bool, - ) { - let solver_vel2 = self.inner.solver_vel2; - - let elements = &mut self.inner.elements[..self.inner.num_contacts as usize]; - OneBodyConstraintElement::generic_solve_group( - self.inner.cfm_factor, - elements, - jacobians, - self.inner.limit, - self.ndofs2, - self.j_id, - solver_vel2, - generic_solver_vels, - solve_restitution, - solve_friction, - ); - } - - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - self.inner.writeback_impulses(manifolds_all); - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.inner.remove_cfm_and_bias_from_rhs(); - } -} diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs deleted file mode 100644 index 4467ea0..0000000 --- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs +++ /dev/null @@ -1,233 +0,0 @@ -use crate::dynamics::solver::{ - OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart, -}; -use crate::math::{DIM, Real}; -use na::DVector; -#[cfg(feature = "dim2")] -use na::SimdPartialOrd; - -impl OneBodyConstraintTangentPart { - #[inline] - pub fn generic_warmstart( - &mut self, - j_id2: usize, - jacobians: &DVector, - ndofs2: usize, - solver_vel2: usize, - solver_vels: &mut DVector, - ) { - #[cfg(feature = "dim2")] - { - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - self.impulse[0], - &jacobians.rows(j_id2 + ndofs2, ndofs2), - 1.0, - ); - } - - #[cfg(feature = "dim3")] - { - let j_step = ndofs2 * 2; - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - self.impulse[0], - &jacobians.rows(j_id2 + ndofs2, ndofs2), - 1.0, - ); - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - self.impulse[1], - &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2), - 1.0, - ); - } - } - - #[inline] - pub fn generic_solve( - &mut self, - j_id2: usize, - jacobians: &DVector, - ndofs2: usize, - limit: Real, - solver_vel2: usize, - solver_vels: &mut DVector, - ) { - #[cfg(feature = "dim2")] - { - let dvel_0 = jacobians - .rows(j_id2, ndofs2) - .dot(&solver_vels.rows(solver_vel2, ndofs2)) - + self.rhs[0]; - - let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit); - let dlambda = new_impulse - self.impulse[0]; - self.impulse[0] = new_impulse; - - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - dlambda, - &jacobians.rows(j_id2 + ndofs2, ndofs2), - 1.0, - ); - } - - #[cfg(feature = "dim3")] - { - let j_step = ndofs2 * 2; - let dvel_0 = jacobians - .rows(j_id2, ndofs2) - .dot(&solver_vels.rows(solver_vel2, ndofs2)) - + self.rhs[0]; - let dvel_1 = jacobians - .rows(j_id2 + j_step, ndofs2) - .dot(&solver_vels.rows(solver_vel2, ndofs2)) - + self.rhs[1]; - - let new_impulse = na::Vector2::new( - self.impulse[0] - self.r[0] * dvel_0, - self.impulse[1] - self.r[1] * dvel_1, - ); - let new_impulse = new_impulse.cap_magnitude(limit); - - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - dlambda[0], - &jacobians.rows(j_id2 + ndofs2, ndofs2), - 1.0, - ); - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - dlambda[1], - &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2), - 1.0, - ); - } - } -} - -impl OneBodyConstraintNormalPart { - #[inline] - pub fn generic_warmstart( - &mut self, - j_id2: usize, - jacobians: &DVector, - ndofs2: usize, - solver_vel2: usize, - solver_vels: &mut DVector, - ) { - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - self.impulse, - &jacobians.rows(j_id2 + ndofs2, ndofs2), - 1.0, - ); - } - - #[inline] - pub fn generic_solve( - &mut self, - cfm_factor: Real, - j_id2: usize, - jacobians: &DVector, - ndofs2: usize, - solver_vel2: usize, - solver_vels: &mut DVector, - ) { - let dvel = jacobians - .rows(j_id2, ndofs2) - .dot(&solver_vels.rows(solver_vel2, ndofs2)) - + self.rhs; - - let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0); - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - solver_vels.rows_mut(solver_vel2, ndofs2).axpy( - dlambda, - &jacobians.rows(j_id2 + ndofs2, ndofs2), - 1.0, - ); - } -} - -impl OneBodyConstraintElement { - #[inline] - pub fn generic_warmstart_group( - elements: &mut [Self], - jacobians: &DVector, - ndofs2: usize, - // Jacobian index of the first constraint. - j_id: usize, - solver_vel2: usize, - solver_vels: &mut DVector, - ) { - let j_step = ndofs2 * 2 * DIM; - - // Solve penetration. - let mut nrm_j_id = j_id; - - for element in elements.iter_mut() { - element.normal_part.generic_warmstart( - nrm_j_id, - jacobians, - ndofs2, - solver_vel2, - solver_vels, - ); - nrm_j_id += j_step; - } - - // Solve friction. - let mut tng_j_id = j_id + ndofs2 * 2; - - for element in elements.iter_mut() { - let part = &mut element.tangent_part; - part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels); - tng_j_id += j_step; - } - } - - #[inline] - pub fn generic_solve_group( - cfm_factor: Real, - elements: &mut [Self], - jacobians: &DVector, - limit: Real, - ndofs2: usize, - // Jacobian index of the first constraint. - j_id: usize, - solver_vel2: usize, - solver_vels: &mut DVector, - solve_restitution: bool, - solve_friction: bool, - ) { - let j_step = ndofs2 * 2 * DIM; - - // Solve penetration. - if solve_restitution { - 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, - solver_vel2, - solver_vels, - ); - nrm_j_id += j_step; - } - } - - // Solve friction. - if solve_friction { - let mut tng_j_id = j_id + ndofs2 * 2; - - for element in elements.iter_mut() { - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.generic_solve(tng_j_id, jacobians, ndofs2, limit, solver_vel2, solver_vels); - tng_j_id += j_step; - } - } - } -} diff --git a/src/dynamics/solver/contact_constraint/mod.rs b/src/dynamics/solver/contact_constraint/mod.rs index 09099dd..b542ff9 100644 --- a/src/dynamics/solver/contact_constraint/mod.rs +++ b/src/dynamics/solver/contact_constraint/mod.rs @@ -1,29 +1,61 @@ -pub(crate) use generic_one_body_constraint::*; -// pub(crate) use generic_one_body_constraint_element::*; -pub(crate) use contact_constraints_set::{ - ConstraintsCounts, ContactConstraintTypes, ContactConstraintsSet, -}; -pub(crate) use generic_two_body_constraint::*; -pub(crate) use generic_two_body_constraint_element::*; -pub(crate) use one_body_constraint::*; -pub(crate) use one_body_constraint_element::*; -#[cfg(feature = "simd-is-enabled")] -pub(crate) use one_body_constraint_simd::*; -pub(crate) use two_body_constraint::*; -pub(crate) use two_body_constraint_element::*; -#[cfg(feature = "simd-is-enabled")] -pub(crate) use two_body_constraint_simd::*; +pub(crate) use contact_constraint_element::*; +pub(crate) use contact_constraints_set::{ConstraintsCounts, ContactConstraintsSet}; +pub(crate) use contact_with_coulomb_friction::*; +pub(crate) use generic_contact_constraint::*; +pub(crate) use generic_contact_constraint_element::*; +#[cfg(feature = "dim3")] +pub(crate) use contact_with_twist_friction::*; + +mod contact_constraint_element; mod contact_constraints_set; -mod generic_one_body_constraint; -mod generic_one_body_constraint_element; -mod generic_two_body_constraint; -mod generic_two_body_constraint_element; -mod one_body_constraint; -mod one_body_constraint_element; -#[cfg(feature = "simd-is-enabled")] -mod one_body_constraint_simd; -mod two_body_constraint; -mod two_body_constraint_element; -#[cfg(feature = "simd-is-enabled")] -mod two_body_constraint_simd; +mod contact_with_coulomb_friction; +mod generic_contact_constraint; +mod generic_contact_constraint_element; + +mod any_contact_constraint; +#[cfg(feature = "dim3")] +mod contact_with_twist_friction; + +#[cfg(feature = "dim3")] +use crate::{ + math::{DIM, Real, Vector}, + utils::{DisableFloatingPointExceptionsFlags, SimdBasis, SimdRealCopy}, +}; + +#[inline] +#[cfg(feature = "dim3")] +pub(crate) fn compute_tangent_contact_directions( + force_dir1: &Vector, + linvel1: &Vector, + linvel2: &Vector, +) -> [Vector; DIM - 1] +where + N: SimdRealCopy, + Vector: SimdBasis, +{ + use SimdBasis; + use na::SimdValue; + + // Compute the tangent direction. Pick the direction of + // the linear relative velocity, if it is not too small. + // Otherwise use a fallback direction. + let relative_linvel = linvel1 - linvel2; + let mut tangent_relative_linvel = + relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel)); + + let tangent_linvel_norm = { + let _disable_fe_except = + DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions(); + tangent_relative_linvel.normalize_mut() + }; + + const THRESHOLD: Real = 1.0e-4; + let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD)); + let tangent_fallback = force_dir1.orthonormal_vector(); + + let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel); + let bitangent1 = force_dir1.cross(&tangent1); + + [tangent1, bitangent1] +} diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs deleted file mode 100644 index 72ac497..0000000 --- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs +++ /dev/null @@ -1,424 +0,0 @@ -use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; -use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, Vector}; -#[cfg(feature = "dim2")] -use crate::utils::SimdBasis; -use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy}; -use na::Matrix2; -use parry::math::Isometry; - -use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; -use crate::dynamics::solver::SolverVel; -use crate::dynamics::solver::solver_body::SolverBody; -use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; - -// TODO: move this struct somewhere else. -#[derive(Copy, Clone, Debug)] -pub struct ContactPointInfos { - pub tangent_vel: Vector, - pub local_p1: Point, - pub local_p2: Point, - pub dist: N, - pub normal_rhs_wo_bias: N, -} - -impl Default for ContactPointInfos { - fn default() -> Self { - Self { - tangent_vel: Vector::zeros(), - local_p1: Point::origin(), - local_p2: Point::origin(), - dist: N::zero(), - normal_rhs_wo_bias: N::zero(), - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct OneBodyConstraintBuilder { - // PERF: only store what’s necessary for the bias updates instead of the complete solver body. - pub rb1: SolverBody, - pub vels1: RigidBodyVelocity, - pub infos: [ContactPointInfos; MAX_MANIFOLD_POINTS], -} - -impl OneBodyConstraintBuilder { - pub fn invalid() -> Self { - Self { - rb1: SolverBody::default(), - vels1: RigidBodyVelocity::zero(), - infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS], - } - } - - pub fn generate( - manifold_id: ContactManifoldIndex, - manifold: &ContactManifold, - bodies: &RigidBodySet, - out_builders: &mut [OneBodyConstraintBuilder], - out_constraints: &mut [OneBodyConstraint], - ) { - let mut handle1 = manifold.data.rigid_body1; - let mut handle2 = manifold.data.rigid_body2; - let flipped = manifold.data.relative_dominance < 0; - - let (force_dir1, flipped_multiplier) = if flipped { - std::mem::swap(&mut handle1, &mut handle2); - (manifold.data.normal, -1.0) - } else { - (-manifold.data.normal, 1.0) - }; - - let (vels1, world_com1) = if let Some(handle1) = handle1 { - let rb1 = &bodies[handle1]; - (rb1.vels, rb1.mprops.world_com) - } else { - (RigidBodyVelocity::zero(), Point::origin()) - }; - - let rb1 = handle1 - .map(|h| SolverBody::from(&bodies[h])) - .unwrap_or_default(); - - let rb2 = &bodies[handle2.unwrap()]; - let vels2 = &rb2.vels; - let mprops2 = &rb2.mprops; - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = - super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - - let solver_vel2 = rb2.ids.active_set_offset; - - for (l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { - let builder = &mut out_builders[l]; - let constraint = &mut out_constraints[l]; - - builder.rb1 = rb1; - builder.vels1 = vels1; - - constraint.dir1 = force_dir1; - constraint.im2 = mprops2.effective_inv_mass; - constraint.solver_vel2 = solver_vel2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = manifold_points.len() as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - - let dp2 = manifold_point.point - mprops2.world_com; - let dp1 = manifold_point.point - world_com1; - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id; - - // Normal part. - let normal_rhs_wo_bias; - { - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-force_dir1)); - - let projected_lin_mass = - force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)); - let projected_ang_mass = gcross2.gdot(gcross2); - - let projected_mass = utils::inv(projected_lin_mass + projected_ang_mass); - - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - - let proj_vel1 = vel1.dot(&force_dir1); - let proj_vel2 = vel2.dot(&force_dir1); - let dvel = proj_vel1 - proj_vel2; - // NOTE: we add proj_vel1 since it’s not accessible through solver_vel. - normal_rhs_wo_bias = - proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel; - - constraint.elements[k].normal_part = OneBodyConstraintNormalPart { - gcross2, - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: manifold_point.warmstart_impulse, - impulse_accumulator: na::zero(), - r: projected_mass, - r_mat_elts: [0.0; 2], - }; - } - - // Tangent parts. - { - constraint.elements[k].tangent_part.impulse = - manifold_point.warmstart_tangent_impulse; - - for j in 0..DIM - 1 { - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-tangents1[j])); - let r = tangents1[j] - .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j])) - + gcross2.gdot(gcross2); - let rhs_wo_bias = (vel1 - + flipped_multiplier * manifold_point.tangent_velocity) - .dot(&tangents1[j]); - - constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::inv(r) - } else { - r - }; - } - - #[cfg(feature = "dim3")] - { - constraint.elements[k].tangent_part.r[2] = 2.0 - * constraint.elements[k].tangent_part.gcross2[0] - .gdot(constraint.elements[k].tangent_part.gcross2[1]); - } - } - - // Builder. - { - let local_p1 = rb1.position.inverse_transform_point(&manifold_point.point); - let local_p2 = rb2 - .pos - .position - .inverse_transform_point(&manifold_point.point); - let infos = ContactPointInfos { - local_p1, - local_p2, - tangent_vel: flipped_multiplier * manifold_point.tangent_velocity, - dist: manifold_point.dist, - normal_rhs_wo_bias, - }; - - builder.infos[k] = infos; - } - } - - if BLOCK_SOLVER_ENABLED { - // Coupling between consecutive pairs. - for k in 0..manifold_points.len() / 2 { - let k0 = k * 2; - let k1 = k * 2 + 1; - - let mut r_mat = Matrix2::zeros(); - let r0 = constraint.elements[k0].normal_part.r; - let r1 = constraint.elements[k1].normal_part.r; - r_mat.m12 = force_dir1 - .dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + constraint.elements[k0] - .normal_part - .gcross2 - .gdot(constraint.elements[k1].normal_part.gcross2); - r_mat.m21 = r_mat.m12; - r_mat.m11 = utils::inv(r0); - r_mat.m22 = utils::inv(r1); - - if let Some(inv) = r_mat.try_inverse() { - constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22]; - constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12]; - } else { - // If inversion failed, the contacts are redundant. - // Ignore the one with the smallest depth (it is too late to - // have the constraint removed from the constraint set, so just - // set the mass (r) matrix elements to 0. - constraint.elements[k0].normal_part.r_mat_elts = - if manifold_points[k0].dist <= manifold_points[k1].dist { - [r0, 0.0] - } else { - [0.0, r1] - }; - constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2]; - } - } - } - } - } - - pub fn update( - &self, - params: &IntegrationParameters, - solved_dt: Real, - bodies: &[SolverBody], - _multibodies: &MultibodyJointSet, - constraint: &mut OneBodyConstraint, - ) { - let rb2 = &bodies[constraint.solver_vel2]; - self.update_with_positions(params, solved_dt, &rb2.position, constraint) - } - - // TODO: this code is SOOOO similar to TwoBodyConstraint::update. - // In fact the only differences are types and the `rb1` and ignoring its ccd thickness. - pub fn update_with_positions( - &self, - params: &IntegrationParameters, - solved_dt: Real, - rb2_pos: &Isometry, - constraint: &mut OneBodyConstraint, - ) { - let cfm_factor = params.contact_cfm_factor(); - let inv_dt = params.inv_dt(); - let erp_inv_dt = params.contact_erp_inv_dt(); - - let all_infos = &self.infos[..constraint.num_contacts as usize]; - let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; - let rb1 = &self.rb1; - // Integrate the velocity of the static rigid-body, if it’s kinematic. - let new_pos1 = self - .vels1 - .integrate(solved_dt, &rb1.position, &rb1.local_com); - - #[cfg(feature = "dim2")] - let tangents1 = constraint.dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = [ - constraint.tangent1, - constraint.dir1.cross(&constraint.tangent1), - ]; - - for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { - // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. - let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt; - let p2 = rb2_pos * info.local_p2; - let dist = info.dist + (p1 - p2).dot(&constraint.dir1); - - // Normal part. - { - let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt; - let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error())) - .clamp(-params.max_corrective_velocity(), 0.0); - let new_rhs = rhs_wo_bias + rhs_bias; - - element.normal_part.rhs_wo_bias = rhs_wo_bias; - element.normal_part.rhs = new_rhs; - element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse *= params.warmstart_coefficient; - } - - // Tangent part. - { - element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse *= params.warmstart_coefficient; - - for j in 0..DIM - 1 { - let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; - element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; - } - } - } - - constraint.cfm_factor = cfm_factor; - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct OneBodyConstraint { - pub solver_vel2: usize, - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub im2: Vector, - pub cfm_factor: Real, - pub limit: Real, - pub elements: [OneBodyConstraintElement; MAX_MANIFOLD_POINTS], - - pub manifold_id: ContactManifoldIndex, - pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, -} - -impl OneBodyConstraint { - pub fn invalid() -> Self { - Self { - solver_vel2: usize::MAX, - dir1: Vector::zeros(), - #[cfg(feature = "dim3")] - tangent1: Vector::zeros(), - im2: Vector::zeros(), - cfm_factor: 0.0, - limit: 0.0, - elements: [OneBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS], - manifold_id: ContactManifoldIndex::MAX, - manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS], - num_contacts: u8::MAX, - } - } - - pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel2 = solver_vels[self.solver_vel2]; - - OneBodyConstraintElement::warmstart_group( - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im2, - &mut solver_vel2, - ); - - solver_vels[self.solver_vel2] = solver_vel2; - } - - pub fn solve( - &mut self, - solver_vels: &mut [SolverVel], - solve_normal: bool, - solve_friction: bool, - ) { - let mut solver_vel2 = solver_vels[self.solver_vel2]; - - OneBodyConstraintElement::solve_group( - self.cfm_factor, - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im2, - self.limit, - &mut solver_vel2, - solve_normal, - solve_friction, - ); - - solver_vels[self.solver_vel2] = solver_vel2; - } - - // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint. - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - let manifold = &mut manifolds_all[self.manifold_id]; - - for k in 0..self.num_contacts as usize { - let contact_id = self.manifold_contact_id[k]; - let active_contact = &mut manifold.points[contact_id as usize]; - - active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse; - active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse; - active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); - active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); - } - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.cfm_factor = 1.0; - for elt in &mut self.elements { - elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; - elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; - } - } -} diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs deleted file mode 100644 index 9debcbb..0000000 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_element.rs +++ /dev/null @@ -1,311 +0,0 @@ -use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; -use crate::dynamics::solver::SolverVel; -use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart; -use crate::math::{AngVector, DIM, TangentImpulse, Vector}; -use crate::utils::{SimdBasis, SimdDot, SimdRealCopy}; -use na::Vector2; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct OneBodyConstraintTangentPart { - pub gcross2: [AngVector; DIM - 1], - pub rhs: [N; DIM - 1], - pub rhs_wo_bias: [N; DIM - 1], - pub impulse: TangentImpulse, - pub impulse_accumulator: TangentImpulse, - #[cfg(feature = "dim2")] - pub r: [N; 1], - #[cfg(feature = "dim3")] - pub r: [N; DIM], -} - -impl OneBodyConstraintTangentPart { - fn zero() -> Self { - Self { - gcross2: [na::zero(); DIM - 1], - rhs: [na::zero(); DIM - 1], - rhs_wo_bias: [na::zero(); DIM - 1], - impulse: na::zero(), - impulse_accumulator: na::zero(), - #[cfg(feature = "dim2")] - r: [na::zero(); 1], - #[cfg(feature = "dim3")] - r: [na::zero(); DIM], - } - } - - /// Total impulse applied across all the solver substeps. - #[inline] - pub fn total_impulse(&self) -> TangentImpulse { - self.impulse_accumulator + self.impulse - } - - #[inline] - pub fn warmstart( - &mut self, - tangents1: [&Vector; DIM - 1], - im2: &Vector, - solver_vel2: &mut SolverVel, - ) { - #[cfg(feature = "dim2")] - { - solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]; - solver_vel2.angular += self.gcross2[0] * self.impulse[0]; - } - - #[cfg(feature = "dim3")] - { - solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0] - + tangents1[1].component_mul(im2) * -self.impulse[1]; - solver_vel2.angular += - self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1]; - } - } - - #[inline] - pub fn solve( - &mut self, - tangents1: [&Vector; DIM - 1], - im2: &Vector, - limit: N, - solver_vel2: &mut SolverVel, - ) where - AngVector: SimdDot, Result = N>, - { - #[cfg(feature = "dim2")] - { - let dvel = -tangents1[0].dot(&solver_vel2.linear) - + self.gcross2[0].gdot(solver_vel2.angular) - + self.rhs[0]; - let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit); - let dlambda = new_impulse - self.impulse[0]; - self.impulse[0] = new_impulse; - - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda; - solver_vel2.angular += self.gcross2[0] * dlambda; - } - - #[cfg(feature = "dim3")] - { - let dvel_0 = -tangents1[0].dot(&solver_vel2.linear) - + self.gcross2[0].gdot(solver_vel2.angular) - + self.rhs[0]; - let dvel_1 = -tangents1[1].dot(&solver_vel2.linear) - + self.gcross2[1].gdot(solver_vel2.angular) - + self.rhs[1]; - - let dvel_00 = dvel_0 * dvel_0; - let dvel_11 = dvel_1 * dvel_1; - let dvel_01 = dvel_0 * dvel_1; - let inv_lhs = (dvel_00 + dvel_11) - * crate::utils::simd_inv( - dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2], - ); - let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1]; - let new_impulse = self.impulse - delta_impulse; - let new_impulse = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - new_impulse.simd_cap_magnitude(limit) - }; - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0] - + tangents1[1].component_mul(im2) * -dlambda[1]; - solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1]; - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct OneBodyConstraintNormalPart { - pub gcross2: AngVector, - pub rhs: N, - pub rhs_wo_bias: N, - pub impulse: N, - pub impulse_accumulator: N, - pub r: N, - pub r_mat_elts: [N; 2], -} - -impl OneBodyConstraintNormalPart { - fn zero() -> Self { - Self { - gcross2: na::zero(), - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: na::zero(), - impulse_accumulator: na::zero(), - r: na::zero(), - r_mat_elts: [N::zero(); 2], - } - } - - /// Total impulse applied across all the solver substeps. - #[inline] - pub fn total_impulse(&self) -> N { - self.impulse_accumulator + self.impulse - } - - #[inline] - pub fn warmstart(&mut self, dir1: &Vector, im2: &Vector, solver_vel2: &mut SolverVel) { - solver_vel2.linear += dir1.component_mul(im2) * -self.impulse; - solver_vel2.angular += self.gcross2 * self.impulse; - } - - #[inline] - pub fn solve( - &mut self, - cfm_factor: N, - dir1: &Vector, - im2: &Vector, - solver_vel2: &mut SolverVel, - ) where - AngVector: SimdDot, Result = N>, - { - let dvel = - -dir1.dot(&solver_vel2.linear) + self.gcross2.gdot(solver_vel2.angular) + self.rhs; - let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero()); - let dlambda = new_impulse - self.impulse; - self.impulse = new_impulse; - - solver_vel2.linear += dir1.component_mul(im2) * -dlambda; - solver_vel2.angular += self.gcross2 * dlambda; - } - - #[inline] - pub fn solve_pair( - constraint_a: &mut Self, - constraint_b: &mut Self, - cfm_factor: N, - dir1: &Vector, - im2: &Vector, - solver_vel2: &mut SolverVel, - ) where - AngVector: SimdDot, Result = N>, - { - let dvel_a = -dir1.dot(&solver_vel2.linear) - + constraint_a.gcross2.gdot(solver_vel2.angular) - + constraint_a.rhs; - let dvel_b = -dir1.dot(&solver_vel2.linear) - + constraint_b.gcross2.gdot(solver_vel2.angular) - + constraint_b.rhs; - - let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse); - let new_impulse = TwoBodyConstraintNormalPart::solve_mlcp_two_constraints( - Vector2::new(dvel_a, dvel_b), - prev_impulse, - constraint_a.r, - constraint_b.r, - constraint_a.r_mat_elts, - constraint_b.r_mat_elts, - cfm_factor, - ); - - let dlambda = new_impulse - prev_impulse; - - constraint_a.impulse = new_impulse.x; - constraint_b.impulse = new_impulse.y; - - solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y); - solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y; - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct OneBodyConstraintElement { - pub normal_part: OneBodyConstraintNormalPart, - pub tangent_part: OneBodyConstraintTangentPart, -} - -impl OneBodyConstraintElement { - pub fn zero() -> Self { - Self { - normal_part: OneBodyConstraintNormalPart::zero(), - tangent_part: OneBodyConstraintTangentPart::zero(), - } - } - - #[inline] - pub fn warmstart_group( - elements: &mut [Self], - dir1: &Vector, - #[cfg(feature = "dim3")] tangent1: &Vector, - im2: &Vector, - solver_vel2: &mut SolverVel, - ) { - #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = [&dir1.orthonormal_vector()]; - - for element in elements.iter_mut() { - element.normal_part.warmstart(dir1, im2, solver_vel2); - element.tangent_part.warmstart(tangents1, im2, solver_vel2); - } - } - - #[inline] - pub fn solve_group( - cfm_factor: N, - elements: &mut [Self], - dir1: &Vector, - #[cfg(feature = "dim3")] tangent1: &Vector, - im2: &Vector, - limit: N, - solver_vel2: &mut SolverVel, - solve_normal: bool, - solve_friction: bool, - ) where - Vector: SimdBasis, - AngVector: SimdDot, Result = N>, - { - #[cfg(feature = "dim3")] - let tangents1 = [tangent1, &dir1.cross(tangent1)]; - #[cfg(feature = "dim2")] - let tangents1 = [&dir1.orthonormal_vector()]; - - // Solve penetration. - if solve_normal { - if BLOCK_SOLVER_ENABLED { - for elements in elements.chunks_exact_mut(2) { - let [element_a, element_b] = elements else { - unreachable!() - }; - - OneBodyConstraintNormalPart::solve_pair( - &mut element_a.normal_part, - &mut element_b.normal_part, - cfm_factor, - dir1, - im2, - solver_vel2, - ); - } - - if elements.len() % 2 == 1 { - let element = elements.last_mut().unwrap(); - element - .normal_part - .solve(cfm_factor, dir1, im2, solver_vel2); - } - } else { - for element in elements.iter_mut() { - element - .normal_part - .solve(cfm_factor, dir1, im2, solver_vel2); - } - } - } - - // Solve friction. - if solve_friction { - for element in elements.iter_mut() { - let limit = limit * element.normal_part.impulse; - let part = &mut element.tangent_part; - part.solve(tangents1, im2, limit, solver_vel2); - } - } - } -} diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs deleted file mode 100644 index aca1a00..0000000 --- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs +++ /dev/null @@ -1,426 +0,0 @@ -use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart}; -use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; -use crate::dynamics::solver::solver_body::SolverBody; -use crate::dynamics::solver::{ContactPointInfos, SolverVel}; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, - RigidBodyVelocity, -}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{ - AngVector, AngularInertia, DIM, Isometry, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH, - SimdReal, TangentImpulse, Vector, -}; -#[cfg(feature = "dim2")] -use crate::utils::SimdBasis; -use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot}; -use num::Zero; -use parry::utils::SdpMatrix2; -use simba::simd::{SimdPartialOrd, SimdValue}; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct SimdOneBodyConstraintBuilder { - // PERF: only store what’s needed, and store it in simd form. - rb1: [SolverBody; SIMD_WIDTH], - vels1: [RigidBodyVelocity; SIMD_WIDTH], - infos: [ContactPointInfos; MAX_MANIFOLD_POINTS], -} - -impl SimdOneBodyConstraintBuilder { - pub fn generate( - manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &RigidBodySet, - out_builders: &mut [SimdOneBodyConstraintBuilder], - out_constraints: &mut [OneBodyConstraintSimd], - ) { - let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1]; - let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2]; - let mut flipped = [1.0; SIMD_WIDTH]; - - for ii in 0..SIMD_WIDTH { - if manifolds[ii].data.relative_dominance < 0 { - std::mem::swap(&mut handles1[ii], &mut handles2[ii]); - flipped[ii] = -1.0; - } - } - - let rb1: [SolverBody; SIMD_WIDTH] = gather![|ii| { - handles1[ii] - .map(|h| SolverBody::from(&bodies[h])) - .unwrap_or_else(SolverBody::default) - }]; - - let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| { - handles1[ii] - .map(|h| bodies[h].vels) - .unwrap_or_else(RigidBodyVelocity::default) - }]; - - let world_com1 = Point::from(gather![|ii| { rb1[ii].world_com }]); - let poss1 = Isometry::from(gather![|ii| rb1[ii].position]); - - let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]]; - - let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels]; - let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids]; - let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops]; - - let flipped_sign = SimdReal::from(flipped); - - let im2 = Vector::from(gather![|ii| mprops2[ii].effective_inv_mass]); - let ii2: AngularInertia = - AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); - - let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); - let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); - - let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); - let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); - - let poss2 = Isometry::from(gather![|ii| bodies2[ii].pos.position]); - let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); - - let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]); - let force_dir1 = normal1 * -flipped_sign; - - let solver_vel2 = gather![|ii| ids2[ii].active_set_offset]; - - let num_active_contacts = manifolds[0].data.num_active_contacts(); - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2); - - for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]]; - let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - - let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; - let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; - - builder.rb1 = rb1; - builder.vels1 = vels1; - - constraint.dir1 = force_dir1; - constraint.im2 = im2; - constraint.solver_vel2 = solver_vel2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = num_points as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - for k in 0..num_points { - let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]); - let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]); - let is_bouncy = SimdReal::from(gather![ - |ii| manifold_points[ii][k].is_bouncy() as u32 as Real - ]); - let warmstart_impulse = - SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]); - let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points - [ii][k] - .warmstart_tangent_impulse]); - - let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); - let point = Point::from(gather![|ii| manifold_points[ii][k].point]); - - let tangent_velocity = - Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); - - let dp1 = point - world_com1; - let dp2 = point - world_com2; - - let vel1 = linvel1 + angvel1.gcross(dp1); - let vel2 = linvel2 + angvel2.gcross(dp2); - - constraint.limit = friction; - constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id]; - - // Normal part. - let normal_rhs_wo_bias; - { - let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - - let projected_mass = utils::simd_inv( - force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2), - ); - - let projected_vel1 = vel1.dot(&force_dir1); - let projected_vel2 = vel2.dot(&force_dir1); - let projected_velocity = projected_vel1 - projected_vel2; - normal_rhs_wo_bias = - (is_bouncy * restitution) * projected_velocity + projected_vel1; // Add projected_vel1 since it’s not accessible through solver_vel. - - constraint.elements[k].normal_part = OneBodyConstraintNormalPart { - gcross2, - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: warmstart_impulse, - impulse_accumulator: na::zero(), - r: projected_mass, - r_mat_elts: [SimdReal::zero(); 2], - }; - } - - // tangent parts. - constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse; - - for j in 0..DIM - 1 { - let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let r = - tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2); - let rhs_wo_bias = (vel1 + tangent_velocity * flipped_sign).dot(&tangents1[j]); - - constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::simd_inv(r) - } else { - r - }; - } - - #[cfg(feature = "dim3")] - { - constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0) - * constraint.elements[k].tangent_part.gcross2[0] - .gdot(constraint.elements[k].tangent_part.gcross2[1]); - } - - // Builder. - { - let local_p1 = poss1.inverse_transform_point(&point); - let local_p2 = poss2.inverse_transform_point(&point); - let infos = ContactPointInfos { - local_p1, - local_p2, - tangent_vel: tangent_velocity * flipped_sign, - dist, - normal_rhs_wo_bias, - }; - - builder.infos[k] = infos; - } - } - - if BLOCK_SOLVER_ENABLED { - // Coupling between consecutive pairs. - for k in 0..num_points / 2 { - let k0 = k * 2; - let k1 = k * 2 + 1; - - let r0 = constraint.elements[k0].normal_part.r; - let r1 = constraint.elements[k1].normal_part.r; - - let mut r_mat = SdpMatrix2::zero(); - r_mat.m12 = force_dir1.dot(&im2.component_mul(&force_dir1)) - + constraint.elements[k0] - .normal_part - .gcross2 - .gdot(constraint.elements[k1].normal_part.gcross2); - r_mat.m11 = utils::simd_inv(r0); - r_mat.m22 = utils::simd_inv(r1); - - let (inv, det) = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - r_mat.inverse_and_get_determinant_unchecked() - }; - let is_invertible = det.simd_gt(SimdReal::zero()); - - // If inversion failed, the contacts are redundant. - // Ignore the one with the smallest depth (it is too late to - // have the constraint removed from the constraint set, so just - // set the mass (r) matrix elements to 0. - constraint.elements[k0].normal_part.r_mat_elts = [ - inv.m11.select(is_invertible, r0), - inv.m22.select(is_invertible, SimdReal::zero()), - ]; - constraint.elements[k1].normal_part.r_mat_elts = [ - inv.m12.select(is_invertible, SimdReal::zero()), - r_mat.m12.select(is_invertible, SimdReal::zero()), - ]; - } - } - } - } - - // TODO: this code is SOOOO similar to TwoBodyConstraintSimd::update. - // In fact the only differences are types and the `rb1` and ignoring its ccd thickness. - pub fn update( - &self, - params: &IntegrationParameters, - solved_dt: Real, - bodies: &[SolverBody], - _multibodies: &MultibodyJointSet, - constraint: &mut OneBodyConstraintSimd, - ) { - let cfm_factor = SimdReal::splat(params.contact_cfm_factor()); - let inv_dt = SimdReal::splat(params.inv_dt()); - let allowed_lin_err = SimdReal::splat(params.allowed_linear_error()); - let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt()); - let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity()); - let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); - - let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]]; - let poss2 = Isometry::from(gather![|ii| rb2[ii].position]); - - let all_infos = &self.infos[..constraint.num_contacts as usize]; - let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; - - // Integrate the velocity of the static rigid-body, if it’s kinematic. - let new_pos1 = Isometry::from(gather![|ii| self.vels1[ii].integrate( - solved_dt, - &self.rb1[ii].position, - &self.rb1[ii].local_com - )]); - - #[cfg(feature = "dim2")] - let tangents1 = constraint.dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = [ - constraint.tangent1, - constraint.dir1.cross(&constraint.tangent1), - ]; - - let solved_dt = SimdReal::splat(solved_dt); - - for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { - // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. - let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt; - let p2 = poss2 * info.local_p2; - let dist = info.dist + (p1 - p2).dot(&constraint.dir1); - - // Normal part. - { - let rhs_wo_bias = - info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt; - let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt) - .simd_clamp(-max_corrective_velocity, SimdReal::zero()); - let new_rhs = rhs_wo_bias + rhs_bias; - - element.normal_part.rhs_wo_bias = rhs_wo_bias; - element.normal_part.rhs = new_rhs; - element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse *= warmstart_coeff; - } - - // tangent parts. - { - element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse *= warmstart_coeff; - - for j in 0..DIM - 1 { - let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; - element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; - } - } - } - - constraint.cfm_factor = cfm_factor; - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct OneBodyConstraintSimd { - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub elements: [OneBodyConstraintElement; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, - pub im2: Vector, - pub cfm_factor: SimdReal, - pub limit: SimdReal, - pub solver_vel2: [usize; SIMD_WIDTH], - pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], -} - -impl OneBodyConstraintSimd { - pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel2 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), - }; - - OneBodyConstraintElement::warmstart_group( - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im2, - &mut solver_vel2, - ); - - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); - solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); - } - } - - pub fn solve( - &mut self, - solver_vels: &mut [SolverVel], - solve_normal: bool, - solve_friction: bool, - ) { - let mut solver_vel2 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), - }; - - OneBodyConstraintElement::solve_group( - self.cfm_factor, - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im2, - self.limit, - &mut solver_vel2, - solve_normal, - solve_friction, - ); - - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); - solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); - } - } - - // FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint. - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - for k in 0..self.num_contacts as usize { - let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); - let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse; - let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into(); - let tangent_impulses = self.elements[k].tangent_part.total_impulse(); - - for ii in 0..SIMD_WIDTH { - let manifold = &mut manifolds_all[self.manifold_id[ii]]; - let contact_id = self.manifold_contact_id[k][ii]; - let active_contact = &mut manifold.points[contact_id as usize]; - - active_contact.data.warmstart_impulse = warmstart_impulses[ii]; - active_contact.data.warmstart_tangent_impulse = - warmstart_tangent_impulses.extract(ii); - active_contact.data.impulse = impulses[ii]; - active_contact.data.tangent_impulse = tangent_impulses.extract(ii); - } - } - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.cfm_factor = SimdReal::splat(1.0); - for elt in &mut self.elements { - elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; - elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; - } - } -} diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs deleted file mode 100644 index 31e6373..0000000 --- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs +++ /dev/null @@ -1,533 +0,0 @@ -use super::{ContactConstraintTypes, ContactPointInfos}; -use crate::dynamics::solver::SolverVel; -use crate::dynamics::solver::{AnyConstraintMut, SolverBody}; - -use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; -use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{DIM, Isometry, MAX_MANIFOLD_POINTS, Real, Vector}; -use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot}; -use na::{DVector, Matrix2}; - -use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; - -impl AnyConstraintMut<'_, ContactConstraintTypes> { - pub fn remove_bias(&mut self) { - match self { - Self::OneBody(c) => c.remove_cfm_and_bias_from_rhs(), - Self::TwoBodies(c) => c.remove_cfm_and_bias_from_rhs(), - Self::GenericOneBody(c) => c.remove_cfm_and_bias_from_rhs(), - Self::GenericTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(), - #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.remove_cfm_and_bias_from_rhs(), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(), - } - } - pub fn warmstart( - &mut self, - generic_jacobians: &DVector, - solver_vels: &mut [SolverVel], - generic_solver_vels: &mut DVector, - ) { - match self { - Self::OneBody(c) => c.warmstart(solver_vels), - Self::TwoBodies(c) => c.warmstart(solver_vels), - Self::GenericOneBody(c) => c.warmstart(generic_jacobians, generic_solver_vels), - Self::GenericTwoBodies(c) => { - c.warmstart(generic_jacobians, solver_vels, generic_solver_vels) - } - #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.warmstart(solver_vels), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.warmstart(solver_vels), - } - } - - pub fn solve_restitution( - &mut self, - generic_jacobians: &DVector, - solver_vels: &mut [SolverVel], - generic_solver_vels: &mut DVector, - ) { - match self { - Self::OneBody(c) => c.solve(solver_vels, true, false), - Self::TwoBodies(c) => c.solve(solver_vels, true, false), - Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, true, false), - Self::GenericTwoBodies(c) => c.solve( - generic_jacobians, - solver_vels, - generic_solver_vels, - true, - false, - ), - #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.solve(solver_vels, true, false), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.solve(solver_vels, true, false), - } - } - - pub fn solve_friction( - &mut self, - generic_jacobians: &DVector, - solver_vels: &mut [SolverVel], - generic_solver_vels: &mut DVector, - ) { - match self { - Self::OneBody(c) => c.solve(solver_vels, false, true), - Self::TwoBodies(c) => c.solve(solver_vels, false, true), - Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, false, true), - Self::GenericTwoBodies(c) => c.solve( - generic_jacobians, - solver_vels, - generic_solver_vels, - false, - true, - ), - #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.solve(solver_vels, false, true), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.solve(solver_vels, false, true), - } - } - - pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) { - match self { - Self::OneBody(c) => c.writeback_impulses(manifolds_all), - Self::TwoBodies(c) => c.writeback_impulses(manifolds_all), - Self::GenericOneBody(c) => c.writeback_impulses(manifolds_all), - Self::GenericTwoBodies(c) => c.writeback_impulses(manifolds_all), - #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.writeback_impulses(manifolds_all), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.writeback_impulses(manifolds_all), - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct TwoBodyConstraint { - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub im1: Vector, - pub im2: Vector, - pub cfm_factor: Real, - pub limit: Real, - pub solver_vel1: usize, - pub solver_vel2: usize, - pub manifold_id: ContactManifoldIndex, - pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, - pub elements: [TwoBodyConstraintElement; MAX_MANIFOLD_POINTS], -} - -impl TwoBodyConstraint { - pub fn invalid() -> Self { - Self { - dir1: Vector::zeros(), - #[cfg(feature = "dim3")] - tangent1: Vector::zeros(), - im1: Vector::zeros(), - im2: Vector::zeros(), - cfm_factor: 0.0, - limit: 0.0, - solver_vel1: usize::MAX, - solver_vel2: usize::MAX, - manifold_id: ContactManifoldIndex::MAX, - manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS], - num_contacts: u8::MAX, - elements: [TwoBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS], - } - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct TwoBodyConstraintBuilder { - pub infos: [ContactPointInfos; MAX_MANIFOLD_POINTS], -} - -impl TwoBodyConstraintBuilder { - pub fn invalid() -> Self { - Self { - infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS], - } - } - - pub fn generate( - manifold_id: ContactManifoldIndex, - manifold: &ContactManifold, - bodies: &RigidBodySet, - out_builders: &mut [TwoBodyConstraintBuilder], - out_constraints: &mut [TwoBodyConstraint], - ) { - assert_eq!(manifold.data.relative_dominance, 0); - - let handle1 = manifold.data.rigid_body1.unwrap(); - let handle2 = manifold.data.rigid_body2.unwrap(); - - let rb1 = &bodies[handle1]; - let (vels1, mprops1) = (&rb1.vels, &rb1.mprops); - let rb2 = &bodies[handle2]; - let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); - - let solver_vel1 = rb1.ids.active_set_offset; - let solver_vel2 = rb2.ids.active_set_offset; - let force_dir1 = -manifold.data.normal; - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = - super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel); - - for (l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() - { - let builder = &mut out_builders[l]; - let constraint = &mut out_constraints[l]; - constraint.dir1 = force_dir1; - constraint.im1 = mprops1.effective_inv_mass; - constraint.im2 = mprops2.effective_inv_mass; - constraint.solver_vel1 = solver_vel1; - constraint.solver_vel2 = solver_vel2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = manifold_points.len() as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - let point = manifold_point.point; - - let dp1 = point - mprops1.world_com; - let dp2 = point - mprops2.world_com; - - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); - - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id; - - // Normal part. - let normal_rhs_wo_bias; - { - let gcross1 = mprops1 - .effective_world_inv_inertia_sqrt - .transform_vector(dp1.gcross(force_dir1)); - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-force_dir1)); - - let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let projected_mass = utils::inv( - force_dir1.dot(&imsum.component_mul(&force_dir1)) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2), - ); - - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - - normal_rhs_wo_bias = - (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - - constraint.elements[k].normal_part = TwoBodyConstraintNormalPart { - gcross1, - gcross2, - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: manifold_point.warmstart_impulse, - impulse_accumulator: na::zero(), - r: projected_mass, - r_mat_elts: [0.0; 2], - }; - } - - // Tangent parts. - { - constraint.elements[k].tangent_part.impulse = - manifold_point.warmstart_tangent_impulse; - - for j in 0..DIM - 1 { - let gcross1 = mprops1 - .effective_world_inv_inertia_sqrt - .transform_vector(dp1.gcross(tangents1[j])); - let gcross2 = mprops2 - .effective_world_inv_inertia_sqrt - .transform_vector(dp2.gcross(-tangents1[j])); - let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2); - let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); - - constraint.elements[k].tangent_part.gcross1[j] = gcross1; - constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::inv(r) - } else { - r - }; - } - - #[cfg(feature = "dim3")] - { - constraint.elements[k].tangent_part.r[2] = 2.0 - * (constraint.elements[k].tangent_part.gcross1[0] - .gdot(constraint.elements[k].tangent_part.gcross1[1]) - + constraint.elements[k].tangent_part.gcross2[0] - .gdot(constraint.elements[k].tangent_part.gcross2[1])); - } - } - - // Builder. - let infos = ContactPointInfos { - local_p1: rb1 - .pos - .position - .inverse_transform_point(&manifold_point.point), - local_p2: rb2 - .pos - .position - .inverse_transform_point(&manifold_point.point), - tangent_vel: manifold_point.tangent_velocity, - dist: manifold_point.dist, - normal_rhs_wo_bias, - }; - - builder.infos[k] = infos; - constraint.manifold_contact_id[k] = manifold_point.contact_id; - } - - if BLOCK_SOLVER_ENABLED { - // Coupling between consecutive pairs. - for k in 0..manifold_points.len() / 2 { - let k0 = k * 2; - let k1 = k * 2 + 1; - - let mut r_mat = Matrix2::zeros(); - let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let r0 = constraint.elements[k0].normal_part.r; - let r1 = constraint.elements[k1].normal_part.r; - r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) - + constraint.elements[k0] - .normal_part - .gcross1 - .gdot(constraint.elements[k1].normal_part.gcross1) - + constraint.elements[k0] - .normal_part - .gcross2 - .gdot(constraint.elements[k1].normal_part.gcross2); - r_mat.m21 = r_mat.m12; - r_mat.m11 = utils::inv(r0); - r_mat.m22 = utils::inv(r1); - - if let Some(inv) = r_mat.try_inverse() { - constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22]; - constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12]; - } else { - // If inversion failed, the contacts are redundant. - // Ignore the one with the smallest depth (it is too late to - // have the constraint removed from the constraint set, so just - // set the mass (r) matrix elements to 0. - constraint.elements[k0].normal_part.r_mat_elts = - if manifold_points[k0].dist <= manifold_points[k1].dist { - [r0, 0.0] - } else { - [0.0, r1] - }; - constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2]; - } - } - } - } - } - - pub fn update( - &self, - params: &IntegrationParameters, - solved_dt: Real, - bodies: &[SolverBody], - _multibodies: &MultibodyJointSet, - constraint: &mut TwoBodyConstraint, - ) { - let rb1 = &bodies[constraint.solver_vel1]; - let rb2 = &bodies[constraint.solver_vel2]; - self.update_with_positions(params, solved_dt, &rb1.position, &rb2.position, constraint) - } - - // Used by both generic and non-generic builders.. - pub fn update_with_positions( - &self, - params: &IntegrationParameters, - solved_dt: Real, - rb1_pos: &Isometry, - rb2_pos: &Isometry, - constraint: &mut TwoBodyConstraint, - ) { - let cfm_factor = params.contact_cfm_factor(); - let inv_dt = params.inv_dt(); - let erp_inv_dt = params.contact_erp_inv_dt(); - - let all_infos = &self.infos[..constraint.num_contacts as usize]; - let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; - - #[cfg(feature = "dim2")] - let tangents1 = constraint.dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = [ - constraint.tangent1, - constraint.dir1.cross(&constraint.tangent1), - ]; - - for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { - // Tangent velocity is equivalent to the first body’s surface moving artificially. - let p1 = rb1_pos * info.local_p1 + info.tangent_vel * solved_dt; - let p2 = rb2_pos * info.local_p2; - let dist = info.dist + (p1 - p2).dot(&constraint.dir1); - - // Normal part. - { - let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt; - let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error())) - .clamp(-params.max_corrective_velocity(), 0.0); - let new_rhs = rhs_wo_bias + rhs_bias; - - element.normal_part.rhs_wo_bias = rhs_wo_bias; - element.normal_part.rhs = new_rhs; - element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse *= params.warmstart_coefficient; - } - - // Tangent part. - { - element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse *= params.warmstart_coefficient; - - for j in 0..DIM - 1 { - let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; - element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; - } - } - } - - constraint.cfm_factor = cfm_factor; - } -} - -impl TwoBodyConstraint { - pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel1 = solver_vels[self.solver_vel1]; - let mut solver_vel2 = solver_vels[self.solver_vel2]; - - TwoBodyConstraintElement::warmstart_group( - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im1, - &self.im2, - &mut solver_vel1, - &mut solver_vel2, - ); - - solver_vels[self.solver_vel1] = solver_vel1; - solver_vels[self.solver_vel2] = solver_vel2; - } - - pub fn solve( - &mut self, - solver_vels: &mut [SolverVel], - solve_normal: bool, - solve_friction: bool, - ) { - let mut solver_vel1 = solver_vels[self.solver_vel1]; - let mut solver_vel2 = solver_vels[self.solver_vel2]; - - TwoBodyConstraintElement::solve_group( - self.cfm_factor, - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im1, - &self.im2, - self.limit, - &mut solver_vel1, - &mut solver_vel2, - solve_normal, - solve_friction, - ); - - solver_vels[self.solver_vel1] = solver_vel1; - solver_vels[self.solver_vel2] = solver_vel2; - } - - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - let manifold = &mut manifolds_all[self.manifold_id]; - - for k in 0..self.num_contacts as usize { - let contact_id = self.manifold_contact_id[k]; - let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse; - active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse; - active_contact.data.impulse = self.elements[k].normal_part.total_impulse(); - active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse(); - } - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.cfm_factor = 1.0; - for elt in &mut self.elements { - elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; - // elt.normal_part.impulse = elt.normal_part.total_impulse; - - elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; - // elt.tangent_part.impulse = elt.tangent_part.total_impulse; - } - } -} - -#[inline(always)] -#[cfg(feature = "dim3")] -pub(crate) fn compute_tangent_contact_directions( - force_dir1: &Vector, - linvel1: &Vector, - linvel2: &Vector, -) -> [Vector; DIM - 1] -where - N: utils::SimdRealCopy, - Vector: SimdBasis, -{ - use na::SimdValue; - - // Compute the tangent direction. Pick the direction of - // the linear relative velocity, if it is not too small. - // Otherwise use a fallback direction. - let relative_linvel = linvel1 - linvel2; - let mut tangent_relative_linvel = - relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel)); - - let tangent_linvel_norm = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions(); - tangent_relative_linvel.normalize_mut() - }; - - const THRESHOLD: Real = 1.0e-4; - let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD)); - let tangent_fallback = force_dir1.orthonormal_vector(); - - let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel); - let bitangent1 = force_dir1.cross(&tangent1); - - [tangent1, bitangent1] -} diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs deleted file mode 100644 index 6fdfe71..0000000 --- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs +++ /dev/null @@ -1,433 +0,0 @@ -use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart}; -use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED; -use crate::dynamics::solver::solver_body::SolverBody; -use crate::dynamics::solver::{ContactPointInfos, SolverVel}; -use crate::dynamics::{ - IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet, - RigidBodyVelocity, -}; -use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::{ - AngVector, AngularInertia, DIM, Isometry, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH, - SimdReal, TangentImpulse, Vector, -}; -#[cfg(feature = "dim2")] -use crate::utils::SimdBasis; -use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot}; -use num::Zero; -use parry::utils::SdpMatrix2; -use simba::simd::{SimdPartialOrd, SimdValue}; - -#[derive(Copy, Clone, Debug)] -pub(crate) struct TwoBodyConstraintBuilderSimd { - infos: [super::ContactPointInfos; MAX_MANIFOLD_POINTS], -} - -impl TwoBodyConstraintBuilderSimd { - pub fn generate( - manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - manifolds: [&ContactManifold; SIMD_WIDTH], - bodies: &RigidBodySet, - out_builders: &mut [TwoBodyConstraintBuilderSimd], - out_constraints: &mut [TwoBodyConstraintSimd], - ) { - for ii in 0..SIMD_WIDTH { - assert_eq!(manifolds[ii].data.relative_dominance, 0); - } - - let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; - let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()]; - - let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].vels]; - let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].vels]; - let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].ids]; - let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].ids]; - let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops]; - let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops]; - - let poss1 = Isometry::from(gather![|ii| bodies[handles1[ii]].pos.position]); - let poss2 = Isometry::from(gather![|ii| bodies[handles2[ii]].pos.position]); - - let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); - let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]); - let ii1: AngularInertia = - AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]); - - let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]); - let angvel1 = AngVector::::from(gather![|ii| vels1[ii].angvel]); - - let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]); - let im2 = Vector::from(gather![|ii| mprops2[ii].effective_inv_mass]); - let ii2: AngularInertia = - AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]); - - let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]); - let angvel2 = AngVector::::from(gather![|ii| vels2[ii].angvel]); - - let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]); - - let solver_vel1 = gather![|ii| ids1[ii].active_set_offset]; - let solver_vel2 = gather![|ii| ids2[ii].active_set_offset]; - - let num_active_contacts = manifolds[0].data.num_active_contacts(); - - #[cfg(feature = "dim2")] - let tangents1 = force_dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2); - - for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = - gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; - let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - - let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; - let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; - - constraint.dir1 = force_dir1; - constraint.im1 = im1; - constraint.im2 = im2; - constraint.solver_vel1 = solver_vel1; - constraint.solver_vel2 = solver_vel2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = num_points as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - for k in 0..num_points { - let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]); - let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]); - let is_bouncy = SimdReal::from(gather![ - |ii| manifold_points[ii][k].is_bouncy() as u32 as Real - ]); - let warmstart_impulse = - SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]); - let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points - [ii][k] - .warmstart_tangent_impulse]); - - let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]); - let point = Point::from(gather![|ii| manifold_points[ii][k].point]); - - let tangent_velocity = - Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]); - let dp1 = point - world_com1; - let dp2 = point - world_com2; - - let vel1 = linvel1 + angvel1.gcross(dp1); - let vel2 = linvel2 + angvel2.gcross(dp2); - - constraint.limit = friction; - constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id]; - - // Normal part. - let normal_rhs_wo_bias; - { - let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); - let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - - let imsum = im1 + im2; - let projected_mass = utils::simd_inv( - force_dir1.dot(&imsum.component_mul(&force_dir1)) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2), - ); - - let projected_velocity = (vel1 - vel2).dot(&force_dir1); - normal_rhs_wo_bias = is_bouncy * restitution * projected_velocity; - - constraint.elements[k].normal_part = TwoBodyConstraintNormalPart { - gcross1, - gcross2, - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse: warmstart_impulse, - impulse_accumulator: SimdReal::splat(0.0), - r: projected_mass, - r_mat_elts: [SimdReal::zero(); 2], - }; - } - - // tangent parts. - constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse; - - for j in 0..DIM - 1 { - let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); - let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); - let imsum = im1 + im2; - let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2); - let rhs_wo_bias = tangent_velocity.dot(&tangents1[j]); - - constraint.elements[k].tangent_part.gcross1[j] = gcross1; - constraint.elements[k].tangent_part.gcross2[j] = gcross2; - constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias; - constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::simd_inv(r) - } else { - r - }; - } - - #[cfg(feature = "dim3")] - { - constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0) - * (constraint.elements[k].tangent_part.gcross1[0] - .gdot(constraint.elements[k].tangent_part.gcross1[1]) - + constraint.elements[k].tangent_part.gcross2[0] - .gdot(constraint.elements[k].tangent_part.gcross2[1])); - } - - // Builder. - let infos = ContactPointInfos { - local_p1: poss1.inverse_transform_point(&point), - local_p2: poss2.inverse_transform_point(&point), - tangent_vel: tangent_velocity, - dist, - normal_rhs_wo_bias, - }; - - builder.infos[k] = infos; - } - - if BLOCK_SOLVER_ENABLED { - // Coupling between consecutive pairs. - for k in 0..num_points / 2 { - let k0 = k * 2; - let k1 = k * 2 + 1; - - let imsum = im1 + im2; - let r0 = constraint.elements[k0].normal_part.r; - let r1 = constraint.elements[k1].normal_part.r; - - let mut r_mat = SdpMatrix2::zero(); - r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) - + constraint.elements[k0] - .normal_part - .gcross1 - .gdot(constraint.elements[k1].normal_part.gcross1) - + constraint.elements[k0] - .normal_part - .gcross2 - .gdot(constraint.elements[k1].normal_part.gcross2); - r_mat.m11 = utils::simd_inv(r0); - r_mat.m22 = utils::simd_inv(r1); - - let (inv, det) = { - let _disable_fe_except = - crate::utils::DisableFloatingPointExceptionsFlags:: - disable_floating_point_exceptions(); - r_mat.inverse_and_get_determinant_unchecked() - }; - let is_invertible = det.simd_gt(SimdReal::zero()); - - // If inversion failed, the contacts are redundant. - // Ignore the one with the smallest depth (it is too late to - // have the constraint removed from the constraint set, so just - // set the mass (r) matrix elements to 0. - constraint.elements[k0].normal_part.r_mat_elts = [ - inv.m11.select(is_invertible, r0), - inv.m22.select(is_invertible, SimdReal::zero()), - ]; - constraint.elements[k1].normal_part.r_mat_elts = [ - inv.m12.select(is_invertible, SimdReal::zero()), - r_mat.m12.select(is_invertible, SimdReal::zero()), - ]; - } - } - } - } - - pub fn update( - &self, - params: &IntegrationParameters, - solved_dt: Real, - bodies: &[SolverBody], - _multibodies: &MultibodyJointSet, - constraint: &mut TwoBodyConstraintSimd, - ) { - let cfm_factor = SimdReal::splat(params.contact_cfm_factor()); - let inv_dt = SimdReal::splat(params.inv_dt()); - let allowed_lin_err = SimdReal::splat(params.allowed_linear_error()); - let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt()); - let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity()); - let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient); - - let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]]; - let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]]; - - let poss1 = Isometry::from(gather![|ii| rb1[ii].position]); - let poss2 = Isometry::from(gather![|ii| rb2[ii].position]); - - let all_infos = &self.infos[..constraint.num_contacts as usize]; - let all_elements = &mut constraint.elements[..constraint.num_contacts as usize]; - - #[cfg(feature = "dim2")] - let tangents1 = constraint.dir1.orthonormal_basis(); - #[cfg(feature = "dim3")] - let tangents1 = [ - constraint.tangent1, - constraint.dir1.cross(&constraint.tangent1), - ]; - - let solved_dt = SimdReal::splat(solved_dt); - - for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) { - // NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface. - let p1 = poss1 * info.local_p1 + info.tangent_vel * solved_dt; - let p2 = poss2 * info.local_p2; - let dist = info.dist + (p1 - p2).dot(&constraint.dir1); - - // Normal part. - { - let rhs_wo_bias = - info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt; - let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt) - .simd_clamp(-max_corrective_velocity, SimdReal::zero()); - let new_rhs = rhs_wo_bias + rhs_bias; - - element.normal_part.rhs_wo_bias = rhs_wo_bias; - element.normal_part.rhs = new_rhs; - element.normal_part.impulse_accumulator += element.normal_part.impulse; - element.normal_part.impulse *= warmstart_coeff; - } - - // tangent parts. - { - element.tangent_part.impulse_accumulator += element.tangent_part.impulse; - element.tangent_part.impulse *= warmstart_coeff; - - for j in 0..DIM - 1 { - let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt; - element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias; - } - } - } - - constraint.cfm_factor = cfm_factor; - } -} - -#[derive(Copy, Clone, Debug)] -pub(crate) struct TwoBodyConstraintSimd { - pub dir1: Vector, // Non-penetration force direction for the first body. - #[cfg(feature = "dim3")] - pub tangent1: Vector, // One of the friction force directions. - pub elements: [TwoBodyConstraintElement; MAX_MANIFOLD_POINTS], - pub num_contacts: u8, - pub im1: Vector, - pub im2: Vector, - pub cfm_factor: SimdReal, - pub limit: SimdReal, - pub solver_vel1: [usize; SIMD_WIDTH], - pub solver_vel2: [usize; SIMD_WIDTH], - pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], - pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS], -} - -impl TwoBodyConstraintSimd { - pub fn warmstart(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel1 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]), - }; - - let mut solver_vel2 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), - }; - - TwoBodyConstraintElement::warmstart_group( - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im1, - &self.im2, - &mut solver_vel1, - &mut solver_vel2, - ); - - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii); - solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii); - } - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); - solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); - } - } - - pub fn solve( - &mut self, - solver_vels: &mut [SolverVel], - solve_normal: bool, - solve_friction: bool, - ) { - let mut solver_vel1 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]), - }; - - let mut solver_vel2 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), - }; - - TwoBodyConstraintElement::solve_group( - self.cfm_factor, - &mut self.elements[..self.num_contacts as usize], - &self.dir1, - #[cfg(feature = "dim3")] - &self.tangent1, - &self.im1, - &self.im2, - self.limit, - &mut solver_vel1, - &mut solver_vel2, - solve_normal, - solve_friction, - ); - - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii); - solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii); - } - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); - solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); - } - } - - pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { - for k in 0..self.num_contacts as usize { - let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); - let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse; - let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into(); - let tangent_impulses = self.elements[k].tangent_part.total_impulse(); - - for ii in 0..SIMD_WIDTH { - let manifold = &mut manifolds_all[self.manifold_id[ii]]; - let contact_id = self.manifold_contact_id[k][ii]; - let active_contact = &mut manifold.points[contact_id as usize]; - active_contact.data.warmstart_impulse = warmstart_impulses[ii]; - active_contact.data.warmstart_tangent_impulse = - warmstart_tangent_impulses.extract(ii); - active_contact.data.impulse = impulses[ii]; - active_contact.data.tangent_impulse = tangent_impulses.extract(ii); - } - } - } - - pub fn remove_cfm_and_bias_from_rhs(&mut self) { - self.cfm_factor = SimdReal::splat(1.0); - for elt in &mut self.elements { - elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; - elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias; - } - } -} diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 35e8c50..dcc20b4 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -117,26 +117,26 @@ impl ParallelInteractionGroups { (false, false) => { let rb1 = &bodies[body_pair.0.unwrap()]; let rb2 = &bodies[body_pair.1.unwrap()]; - let color_mask = - bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset]; + let color_mask = bcolors[rb1.ids.active_set_offset as usize] + | bcolors[rb2.ids.active_set_offset as usize]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.ids.active_set_offset] |= 1 << *color; - bcolors[rb2.ids.active_set_offset] |= 1 << *color; + bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color; + bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color; } (true, false) => { let rb2 = &bodies[body_pair.1.unwrap()]; - let color_mask = bcolors[rb2.ids.active_set_offset]; + let color_mask = bcolors[rb2.ids.active_set_offset as usize]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb2.ids.active_set_offset] |= 1 << *color; + bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color; } (false, true) => { let rb1 = &bodies[body_pair.0.unwrap()]; - let color_mask = bcolors[rb1.ids.active_set_offset]; + let color_mask = bcolors[rb1.ids.active_set_offset as usize]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.ids.active_set_offset] |= 1 << *color; + bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color; } (true, true) => unreachable!(), } @@ -173,9 +173,8 @@ pub(crate) struct InteractionGroups { buckets: VecMap<([usize; SIMD_WIDTH], usize)>, #[cfg(feature = "simd-is-enabled")] body_masks: Vec, - #[cfg(feature = "simd-is-enabled")] - pub simd_interactions: Vec, - pub nongrouped_interactions: Vec, + pub simd_interactions: Vec, + pub nongrouped_interactions: Vec, } impl InteractionGroups { @@ -185,7 +184,6 @@ impl InteractionGroups { buckets: VecMap::new(), #[cfg(feature = "simd-is-enabled")] body_masks: Vec::new(), - #[cfg(feature = "simd-is-enabled")] simd_interactions: Vec::new(), nongrouped_interactions: Vec::new(), } @@ -258,8 +256,8 @@ impl InteractionGroups { let rb1 = &bodies[interaction.body1]; let rb2 = &bodies[interaction.body2]; - let is_fixed1 = !rb1.is_dynamic(); - let is_fixed2 = !rb2.is_dynamic(); + let is_fixed1 = !rb1.is_dynamic_or_kinematic(); + let is_fixed2 = !rb2.is_dynamic_or_kinematic(); if is_fixed1 && is_fixed2 { continue; @@ -274,8 +272,17 @@ impl InteractionGroups { let ijoint = interaction.data.locked_axes.bits() as usize; let i1 = rb1.ids.active_set_offset; let i2 = rb2.ids.active_set_offset; - let conflicts = - self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; + let conflicts = self + .body_masks + .get(i1 as usize) + .copied() + .unwrap_or_default() + | self + .body_masks + .get(i2 as usize) + .copied() + .unwrap_or_default() + | joint_type_conflicts[ijoint]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; @@ -288,7 +295,7 @@ impl InteractionGroups { if target_index == 128 { // The interaction conflicts with every bucket we can manage. - // So push it in an nongrouped interaction list that won't be combined with + // So push it in a nongrouped interaction list that won't be combined with // any other interactions. self.nongrouped_interactions.push(*interaction_i); continue; @@ -327,11 +334,11 @@ impl InteractionGroups { // NOTE: fixed bodies don't transmit forces. Therefore they don't // imply any interaction conflicts. if !is_fixed1 { - self.body_masks[i1] |= target_mask_bit; + self.body_masks[i1 as usize] |= target_mask_bit; } if !is_fixed2 { - self.body_masks[i2] |= target_mask_bit; + self.body_masks[i2 as usize] |= target_mask_bit; } } @@ -356,7 +363,6 @@ impl InteractionGroups { } pub fn clear_groups(&mut self) { - #[cfg(feature = "simd-is-enabled")] self.simd_interactions.clear(); self.nongrouped_interactions.clear(); } @@ -419,18 +425,18 @@ impl InteractionGroups { let rb1 = &bodies[rb1]; (rb1.body_type, rb1.ids.active_set_offset) } else { - (RigidBodyType::Fixed, usize::MAX) + (RigidBodyType::Fixed, u32::MAX) }; let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 { let rb2 = &bodies[rb2]; (rb2.body_type, rb2.ids.active_set_offset) } else { - (RigidBodyType::Fixed, usize::MAX) + (RigidBodyType::Fixed, u32::MAX) }; - let is_fixed1 = !status1.is_dynamic(); - let is_fixed2 = !status2.is_dynamic(); + let is_fixed1 = !status1.is_dynamic_or_kinematic(); + let is_fixed2 = !status2.is_dynamic_or_kinematic(); // TODO: don't generate interactions between fixed bodies in the first place. if is_fixed1 && is_fixed2 { @@ -439,8 +445,16 @@ impl InteractionGroups { let i1 = active_set_offset1; let i2 = active_set_offset2; - let mask1 = if !is_fixed1 { self.body_masks[i1] } else { 0 }; - let mask2 = if !is_fixed2 { self.body_masks[i2] } else { 0 }; + let mask1 = if !is_fixed1 { + self.body_masks[i1 as usize] + } else { + 0 + }; + let mask2 = if !is_fixed2 { + self.body_masks[i2 as usize] + } else { + 0 + }; let conflicts = mask1 | mask2; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; @@ -482,11 +496,11 @@ impl InteractionGroups { // NOTE: fixed bodies don't transmit forces. Therefore they don't // imply any interaction conflicts. if !is_fixed1 { - self.body_masks[i1] |= target_mask_bit; + self.body_masks[i1 as usize] |= target_mask_bit; } if !is_fixed2 { - self.body_masks[i2] |= target_mask_bit; + self.body_masks[i2 as usize] |= target_mask_bit; } } diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 62b22e0..80f2d4b 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -43,7 +43,7 @@ impl IslandSolver { multibodies: &mut MultibodyJointSet, ) { counters.solver.velocity_assembly_time.resume(); - let num_solver_iterations = base_params.num_solver_iterations.get() + let num_solver_iterations = base_params.num_solver_iterations + islands.active_island_additional_solver_iterations(island_id); let mut params = *base_params; @@ -55,14 +55,18 @@ impl IslandSolver { * */ // INIT + // let t0 = std::time::Instant::now(); self.velocity_solver .init_solver_velocities_and_solver_bodies( + base_params.dt, ¶ms, island_id, islands, bodies, multibodies, ); + // let t_solver_body_init = t0.elapsed().as_secs_f32(); + // let t0 = std::time::Instant::now(); self.velocity_solver.init_constraints( island_id, islands, @@ -74,8 +78,16 @@ impl IslandSolver { joint_indices, &mut self.contact_constraints, &mut self.joint_constraints, + #[cfg(feature = "dim3")] + params.friction_model, ); + // let t_init_constraints = t0.elapsed().as_secs_f32(); counters.solver.velocity_assembly_time.pause(); + // println!( + // "Solver body init: {}, init constraints: {}", + // t_solver_body_init * 1000.0, + // t_init_constraints * 1000.0 + // ); // SOLVE counters.solver.velocity_resolution_time.resume(); @@ -93,14 +105,8 @@ impl IslandSolver { counters.solver.velocity_writeback_time.resume(); self.joint_constraints.writeback_impulses(impulse_joints); self.contact_constraints.writeback_impulses(manifolds); - self.velocity_solver.writeback_bodies( - base_params, - num_solver_iterations, - islands, - island_id, - bodies, - multibodies, - ); + self.velocity_solver + .writeback_bodies(base_params, islands, island_id, bodies, multibodies); counters.solver.velocity_writeback_time.pause(); } } diff --git a/src/dynamics/solver/joint_constraint/any_joint_constraint.rs b/src/dynamics/solver/joint_constraint/any_joint_constraint.rs index 654e8a4..883c273 100644 --- a/src/dynamics/solver/joint_constraint/any_joint_constraint.rs +++ b/src/dynamics/solver/joint_constraint/any_joint_constraint.rs @@ -1,97 +1,52 @@ use crate::dynamics::JointGraphEdge; -use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{ - JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint, -}; -use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ - JointOneBodyConstraint, JointTwoBodyConstraint, -}; -use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes}; +use crate::dynamics::solver::joint_constraint::generic_joint_constraint::GenericJointConstraint; +use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::JointConstraint; use crate::math::Real; use na::DVector; -use crate::dynamics::solver::joint_constraint::joint_generic_constraint_builder::{ - JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder, -}; -use crate::dynamics::solver::solver_vel::SolverVel; #[cfg(feature = "simd-is-enabled")] -use crate::{ - dynamics::solver::joint_constraint::joint_constraint_builder::{ - JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd, - }, - math::{SIMD_WIDTH, SimdReal}, -}; +use crate::math::{SIMD_WIDTH, SimdReal}; -use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{ - JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder, -}; +use crate::dynamics::solver::solver_body::SolverBodies; -pub struct JointConstraintTypes; +#[derive(Debug)] +pub enum AnyJointConstraintMut<'a> { + Generic(&'a mut GenericJointConstraint), + Rigid(&'a mut JointConstraint), + #[cfg(feature = "simd-is-enabled")] + SimdRigid(&'a mut JointConstraint), +} -impl AnyConstraintMut<'_, JointConstraintTypes> { +impl AnyJointConstraintMut<'_> { pub fn remove_bias(&mut self) { match self { - Self::OneBody(c) => c.remove_bias_from_rhs(), - Self::TwoBodies(c) => c.remove_bias_from_rhs(), - Self::GenericOneBody(c) => c.remove_bias_from_rhs(), - Self::GenericTwoBodies(c) => c.remove_bias_from_rhs(), + Self::Rigid(c) => c.remove_bias_from_rhs(), + Self::Generic(c) => c.remove_bias_from_rhs(), #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.remove_bias_from_rhs(), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.remove_bias_from_rhs(), + Self::SimdRigid(c) => c.remove_bias_from_rhs(), } } pub fn solve( &mut self, generic_jacobians: &DVector, - solver_vels: &mut [SolverVel], + solver_vels: &mut SolverBodies, generic_solver_vels: &mut DVector, ) { match self { - Self::OneBody(c) => c.solve(solver_vels), - Self::TwoBodies(c) => c.solve(solver_vels), - Self::GenericOneBody(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels), - Self::GenericTwoBodies(c) => { - c.solve(generic_jacobians, solver_vels, generic_solver_vels) - } + Self::Rigid(c) => c.solve(solver_vels), + Self::Generic(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels), #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.solve(solver_vels), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.solve(solver_vels), + Self::SimdRigid(c) => c.solve(solver_vels), } } pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) { match self { - Self::OneBody(c) => c.writeback_impulses(joints_all), - Self::TwoBodies(c) => c.writeback_impulses(joints_all), - Self::GenericOneBody(c) => c.writeback_impulses(joints_all), - Self::GenericTwoBodies(c) => c.writeback_impulses(joints_all), + Self::Rigid(c) => c.writeback_impulses(joints_all), + Self::Generic(c) => c.writeback_impulses(joints_all), #[cfg(feature = "simd-is-enabled")] - Self::SimdOneBody(c) => c.writeback_impulses(joints_all), - #[cfg(feature = "simd-is-enabled")] - Self::SimdTwoBodies(c) => c.writeback_impulses(joints_all), + Self::SimdRigid(c) => c.writeback_impulses(joints_all), } } } - -impl ConstraintTypes for JointConstraintTypes { - type OneBody = JointOneBodyConstraint; - type TwoBodies = JointTwoBodyConstraint; - type GenericOneBody = JointGenericOneBodyConstraint; - type GenericTwoBodies = JointGenericTwoBodyConstraint; - - #[cfg(feature = "simd-is-enabled")] - type SimdOneBody = JointOneBodyConstraint; - #[cfg(feature = "simd-is-enabled")] - type SimdTwoBodies = JointTwoBodyConstraint; - - type BuilderOneBody = JointOneBodyConstraintBuilder; - type BuilderTwoBodies = JointTwoBodyConstraintBuilder; - type GenericBuilderOneBody = JointGenericOneBodyConstraintBuilder; - type GenericBuilderTwoBodies = JointGenericTwoBodyConstraintBuilder; - #[cfg(feature = "simd-is-enabled")] - type SimdBuilderOneBody = JointOneBodyConstraintBuilderSimd; - #[cfg(feature = "simd-is-enabled")] - type SimdBuilderTwoBodies = JointTwoBodyConstraintBuilderSimd; -} diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs new file mode 100644 index 0000000..ee48b27 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint.rs @@ -0,0 +1,320 @@ +use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId; +use crate::dynamics::solver::joint_constraint::{JointConstraintHelper, JointSolverBody}; +use crate::dynamics::solver::solver_body::SolverBodies; +use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex}; +use crate::math::{DIM, Isometry, Real}; +use crate::prelude::SPATIAL_DIM; +use na::{DVector, DVectorView, DVectorViewMut}; + +use super::LinkOrBodyRef; + +#[derive(Debug, Copy, Clone)] +pub struct GenericJointConstraint { + pub is_rigid_body1: bool, + pub is_rigid_body2: bool, + pub solver_vel1: u32, + pub solver_vel2: u32, + + pub ndofs1: usize, + pub j_id1: usize, + pub ndofs2: usize, + pub j_id2: usize, + + pub joint_id: JointIndex, + + pub impulse: Real, + pub impulse_bounds: [Real; 2], + pub inv_lhs: Real, + pub rhs: Real, + pub rhs_wo_bias: Real, + pub cfm_coeff: Real, + pub cfm_gain: Real, + + pub writeback_id: WritebackId, +} + +impl Default for GenericJointConstraint { + fn default() -> Self { + GenericJointConstraint::invalid() + } +} + +impl GenericJointConstraint { + pub fn invalid() -> Self { + Self { + is_rigid_body1: false, + is_rigid_body2: false, + solver_vel1: u32::MAX, + solver_vel2: u32::MAX, + ndofs1: usize::MAX, + j_id1: usize::MAX, + ndofs2: usize::MAX, + j_id2: usize::MAX, + joint_id: usize::MAX, + impulse: 0.0, + impulse_bounds: [-Real::MAX, Real::MAX], + inv_lhs: Real::MAX, + rhs: Real::MAX, + rhs_wo_bias: Real::MAX, + cfm_coeff: Real::MAX, + cfm_gain: Real::MAX, + writeback_id: WritebackId::Dof(0), + } + } + + pub fn lock_axes( + params: &IntegrationParameters, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + frame1: &Isometry, + frame2: &Isometry, + joint: &GenericJoint, + jacobians: &mut DVector, + j_id: &mut usize, + out: &mut [Self], + ) -> usize { + let mut len = 0; + let locked_axes = joint.locked_axes.bits(); + let motor_axes = joint.motor_axes.bits(); + let limit_axes = joint.limit_axes.bits(); + + let builder = JointConstraintHelper::new( + frame1, + frame2, + &body1.world_com, + &body2.world_com, + locked_axes, + ); + + let start = len; + for i in DIM..SPATIAL_DIM { + if motor_axes & (1 << i) != 0 { + out[len] = builder.motor_angular_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + i - DIM, + &joint.motors[i].motor_params(params.dt), + WritebackId::Motor(i), + ); + len += 1; + } + } + for i in 0..DIM { + if motor_axes & (1 << i) != 0 { + out[len] = builder.motor_linear_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + // locked_ang_axes, + i, + &joint.motors[i].motor_params(params.dt), + WritebackId::Motor(i), + ); + len += 1; + } + } + JointConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]); + + let start = len; + for i in DIM..SPATIAL_DIM { + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_angular_generic( + params, + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + i - DIM, + WritebackId::Dof(i), + ); + len += 1; + } + } + for i in 0..DIM { + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_linear_generic( + params, + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + i, + WritebackId::Dof(i), + ); + len += 1; + } + } + + for i in DIM..SPATIAL_DIM { + if limit_axes & (1 << i) != 0 { + out[len] = builder.limit_angular_generic( + params, + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + i - DIM, + [joint.limits[i].min, joint.limits[i].max], + WritebackId::Limit(i), + ); + len += 1; + } + } + for i in 0..DIM { + if limit_axes & (1 << i) != 0 { + out[len] = builder.limit_linear_generic( + params, + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + i, + [joint.limits[i].min, joint.limits[i].max], + WritebackId::Limit(i), + ); + len += 1; + } + } + + JointConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]); + len + } + + fn wj_id1(&self) -> usize { + self.j_id1 + self.ndofs1 + } + + fn wj_id2(&self) -> usize { + self.j_id2 + self.ndofs2 + } + + fn solver_vel1<'a>( + &self, + solver_vels: &'a SolverBodies, + generic_solver_vels: &'a DVector, + ) -> DVectorView<'a, Real> { + if self.solver_vel1 == u32::MAX { + generic_solver_vels.rows(0, 0) // empty + } else if self.is_rigid_body1 { + solver_vels.vels[self.solver_vel1 as usize].as_vector_slice() + } else { + generic_solver_vels.rows(self.solver_vel1 as usize, self.ndofs1) + } + } + + fn solver_vel1_mut<'a>( + &self, + solver_vels: &'a mut SolverBodies, + generic_solver_vels: &'a mut DVector, + ) -> DVectorViewMut<'a, Real> { + if self.solver_vel1 == u32::MAX { + generic_solver_vels.rows_mut(0, 0) // empty + } else if self.is_rigid_body1 { + solver_vels.vels[self.solver_vel1 as usize].as_vector_slice_mut() + } else { + generic_solver_vels.rows_mut(self.solver_vel1 as usize, self.ndofs1) + } + } + + fn solver_vel2<'a>( + &self, + solver_vels: &'a SolverBodies, + generic_solver_vels: &'a DVector, + ) -> DVectorView<'a, Real> { + if self.solver_vel2 == u32::MAX { + generic_solver_vels.rows(0, 0) // empty + } else if self.is_rigid_body2 { + solver_vels.vels[self.solver_vel2 as usize].as_vector_slice() + } else { + generic_solver_vels.rows(self.solver_vel2 as usize, self.ndofs2) + } + } + + fn solver_vel2_mut<'a>( + &self, + solver_vels: &'a mut SolverBodies, + generic_solver_vels: &'a mut DVector, + ) -> DVectorViewMut<'a, Real> { + if self.solver_vel2 == u32::MAX { + generic_solver_vels.rows_mut(0, 0) // empty + } else if self.is_rigid_body2 { + solver_vels.vels[self.solver_vel2 as usize].as_vector_slice_mut() + } else { + generic_solver_vels.rows_mut(self.solver_vel2 as usize, self.ndofs2) + } + } + + pub fn solve( + &mut self, + jacobians: &DVector, + solver_vels: &mut SolverBodies, + generic_solver_vels: &mut DVector, + ) { + let jacobians = jacobians.as_slice(); + + let solver_vel1 = self.solver_vel1(solver_vels, generic_solver_vels); + let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1); + let vel1 = j1.dot(&solver_vel1); + + let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels); + let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2); + let vel2 = j2.dot(&solver_vel2); + + let dvel = self.rhs + (vel2 - vel1); + let total_impulse = na::clamp( + self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse), + self.impulse_bounds[0], + self.impulse_bounds[1], + ); + let delta_impulse = total_impulse - self.impulse; + self.impulse = total_impulse; + + let mut solver_vel1 = self.solver_vel1_mut(solver_vels, generic_solver_vels); + let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1); + solver_vel1.axpy(delta_impulse, &wj1, 1.0); + + let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels); + let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); + solver_vel2.axpy(-delta_impulse, &wj2, 1.0); + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + // TODO: we don’t support impulse writeback for internal constraints yet. + if self.joint_id != JointIndex::MAX { + let joint = &mut joints_all[self.joint_id].weight; + match self.writeback_id { + WritebackId::Dof(i) => joint.impulses[i] = self.impulse, + WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse, + WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse, + } + } + } + + pub fn remove_bias_from_rhs(&mut self) { + self.rhs = self.rhs_wo_bias; + } +} diff --git a/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs new file mode 100644 index 0000000..d9bbf26 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs @@ -0,0 +1,749 @@ +use crate::dynamics::solver::MotorParameters; +use crate::dynamics::solver::joint_constraint::generic_joint_constraint::GenericJointConstraint; +use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId; +use crate::dynamics::solver::joint_constraint::{JointConstraintHelper, JointSolverBody}; +use crate::dynamics::{ + GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet, + MultibodyLinkId, RigidBodySet, +}; +use crate::math::{ANG_DIM, DIM, Real, SPATIAL_DIM, Vector}; +use crate::utils; +use crate::utils::IndexMut2; +use na::{DVector, SVector}; + +use crate::dynamics::solver::ConstraintsCounts; +use crate::dynamics::solver::solver_body::SolverBodies; +#[cfg(feature = "dim3")] +use crate::utils::SimdAngularInertia; +#[cfg(feature = "dim2")] +use na::Vector1; +use parry::math::Isometry; + +#[derive(Copy, Clone)] +enum LinkOrBody { + Link(MultibodyLinkId), + Body(u32), + Fixed, +} + +#[derive(Copy, Clone)] +pub enum LinkOrBodyRef<'a> { + Link(&'a Multibody, usize), + Body(u32), + Fixed, +} + +#[allow(clippy::large_enum_variant)] +#[derive(Copy, Clone)] +pub enum GenericJointConstraintBuilder { + Internal(JointGenericInternalConstraintBuilder), + External(JointGenericExternalConstraintBuilder), + Empty, // No constraint +} + +#[derive(Copy, Clone)] +pub struct JointGenericExternalConstraintBuilder { + link1: LinkOrBody, + link2: LinkOrBody, + joint_id: JointIndex, + joint: GenericJoint, + j_id: usize, + // These are solver body for both joints, except that + // the world_com is actually in local-space. + local_body1: JointSolverBody, + local_body2: JointSolverBody, + multibodies_ndof: usize, + constraint_id: usize, +} + +impl JointGenericExternalConstraintBuilder { + pub fn generate( + joint_id: JointIndex, + joint: &ImpulseJoint, + bodies: &RigidBodySet, + multibodies: &MultibodyJointSet, + out_builder: &mut GenericJointConstraintBuilder, + j_id: &mut usize, + jacobians: &mut DVector, + out_constraint_id: &mut usize, + ) { + let starting_j_id = *j_id; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + let solver_vel1 = rb1.effective_active_set_offset(); + let solver_vel2 = rb2.effective_active_set_offset(); + let local_body1 = JointSolverBody { + im: rb1.mprops.effective_inv_mass, + ii: rb1.mprops.effective_world_inv_inertia, + world_com: rb1.mprops.local_mprops.local_com, + solver_vel: [solver_vel1], + }; + let local_body2 = JointSolverBody { + im: rb2.mprops.effective_inv_mass, + ii: rb2.mprops.effective_world_inv_inertia, + world_com: rb2.mprops.local_mprops.local_com, + solver_vel: [solver_vel2], + }; + + let mut multibodies_ndof = 0; + let link1 = if solver_vel1 == u32::MAX { + LinkOrBody::Fixed + } else if let Some(link) = multibodies.rigid_body_link(joint.body1) { + multibodies_ndof += multibodies[link.multibody].ndofs(); + LinkOrBody::Link(*link) + } else { + // Dynamic rigid-body. + multibodies_ndof += SPATIAL_DIM; + LinkOrBody::Body(solver_vel1) + }; + + let link2 = if solver_vel2 == u32::MAX { + LinkOrBody::Fixed + } else if let Some(link) = multibodies.rigid_body_link(joint.body2) { + multibodies_ndof += multibodies[link.multibody].ndofs(); + LinkOrBody::Link(*link) + } else { + // Dynamic rigid-body. + multibodies_ndof += SPATIAL_DIM; + LinkOrBody::Body(solver_vel2) + }; + + if multibodies_ndof == 0 { + return; + } + + // For each solver contact we generate up to SPATIAL_DIM constraints, and each + // constraints appends the multibodies jacobian and weighted jacobians. + // Also note that for impulse_joints, the rigid-bodies will also add their jacobians + // to the generic DVector. + // TODO: is this count correct when we take both motors and limits into account? + let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; + + // TODO: use a more precise increment. + *j_id += multibodies_ndof * 2 * SPATIAL_DIM; + + if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { + jacobians.resize_vertically_mut(required_jacobian_len, 0.0); + } + + let mut joint_data = joint.data; + joint_data.transform_to_solver_body_space(rb1, rb2); + *out_builder = GenericJointConstraintBuilder::External(Self { + link1, + link2, + joint_id, + joint: joint_data, + j_id: starting_j_id, + local_body1, + local_body2, + multibodies_ndof, + constraint_id: *out_constraint_id, + }); + + *out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + multibodies: &MultibodyJointSet, + bodies: &SolverBodies, + jacobians: &mut DVector, + out: &mut [GenericJointConstraint], + ) { + if self.multibodies_ndof == 0 { + // The joint is between two static bodies, no constraint needed. + return; + } + + // NOTE: right now, the "update", is basically reconstructing all the + // constraints. Could we make this more incremental? + let pos1; + let pos2; + let mb1; + let mb2; + + match self.link1 { + LinkOrBody::Link(link) => { + let mb = &multibodies[link.multibody]; + pos1 = mb.link(link.id).unwrap().local_to_world; + mb1 = LinkOrBodyRef::Link(mb, link.id); + } + LinkOrBody::Body(body1) => { + pos1 = bodies.get_pose(body1).pose; + mb1 = LinkOrBodyRef::Body(body1); + } + LinkOrBody::Fixed => { + pos1 = Isometry::identity(); + mb1 = LinkOrBodyRef::Fixed; + } + }; + match self.link2 { + LinkOrBody::Link(link) => { + let mb = &multibodies[link.multibody]; + pos2 = mb.link(link.id).unwrap().local_to_world; + mb2 = LinkOrBodyRef::Link(mb, link.id); + } + LinkOrBody::Body(body2) => { + pos2 = bodies.get_pose(body2).pose; + mb2 = LinkOrBodyRef::Body(body2); + } + LinkOrBody::Fixed => { + pos2 = Isometry::identity(); + mb2 = LinkOrBodyRef::Fixed; + } + }; + + let frame1 = pos1 * self.joint.local_frame1; + let frame2 = pos2 * self.joint.local_frame2; + + let joint_body1 = JointSolverBody { + world_com: pos1.translation.vector.into(), // the solver body pose is at the center of mass. + ..self.local_body1 + }; + let joint_body2 = JointSolverBody { + world_com: pos2.translation.vector.into(), // the solver body pose is at the center of mass. + ..self.local_body2 + }; + + let mut j_id = self.j_id; + + GenericJointConstraint::lock_axes( + params, + self.joint_id, + &joint_body1, + &joint_body2, + mb1, + mb2, + &frame1, + &frame2, + &self.joint, + jacobians, + &mut j_id, + &mut out[self.constraint_id..], + ); + } +} + +#[derive(Copy, Clone)] +pub struct JointGenericInternalConstraintBuilder { + link: MultibodyLinkId, + j_id: usize, + constraint_id: usize, +} + +impl JointGenericInternalConstraintBuilder { + pub fn num_constraints(multibodies: &MultibodyJointSet, link_id: &MultibodyLinkId) -> usize { + let multibody = &multibodies[link_id.multibody]; + let link = multibody.link(link_id.id).unwrap(); + link.joint().num_velocity_constraints() + } + + pub fn generate( + multibodies: &MultibodyJointSet, + link_id: &MultibodyLinkId, + out_builder: &mut GenericJointConstraintBuilder, + j_id: &mut usize, + jacobians: &mut DVector, + out_constraint_id: &mut usize, + ) { + let multibody = &multibodies[link_id.multibody]; + let link = multibody.link(link_id.id).unwrap(); + let num_constraints = link.joint().num_velocity_constraints(); + + if num_constraints == 0 { + return; + } + + *out_builder = GenericJointConstraintBuilder::Internal(Self { + link: *link_id, + j_id: *j_id, + constraint_id: *out_constraint_id, + }); + + *j_id += num_constraints * multibody.ndofs() * 2; + if jacobians.nrows() < *j_id { + jacobians.resize_vertically_mut(*j_id, 0.0); + } + + *out_constraint_id += num_constraints; + } + + pub fn update( + &self, + params: &IntegrationParameters, + multibodies: &MultibodyJointSet, + jacobians: &mut DVector, + out: &mut [GenericJointConstraint], + ) { + let mb = &multibodies[self.link.multibody]; + let link = mb.link(self.link.id).unwrap(); + link.joint().velocity_constraints( + params, + mb, + link, + self.j_id, + jacobians, + &mut out[self.constraint_id..], + ); + } +} + +impl JointSolverBody { + pub fn fill_jacobians( + &self, + unit_force: Vector, + unit_torque: SVector, + j_id: &mut usize, + jacobians: &mut DVector, + ) { + let wj_id = *j_id + SPATIAL_DIM; + jacobians + .fixed_rows_mut::(*j_id) + .copy_from(&unit_force); + jacobians + .fixed_rows_mut::(*j_id + DIM) + .copy_from(&unit_torque); + + { + let mut out_invm_j = jacobians.fixed_rows_mut::(wj_id); + out_invm_j + .fixed_rows_mut::(0) + .copy_from(&self.im.component_mul(&unit_force)); + + #[cfg(feature = "dim2")] + { + out_invm_j[DIM] *= self.ii; + } + #[cfg(feature = "dim3")] + { + out_invm_j.fixed_rows_mut::(DIM).gemv( + 1.0, + &self.ii.into_matrix(), + &unit_torque, + 0.0, + ); + } + } + + *j_id += SPATIAL_DIM * 2; + } +} + +impl JointConstraintHelper { + pub fn lock_jacobians_generic( + &self, + jacobians: &mut DVector, + j_id: &mut usize, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + writeback_id: WritebackId, + lin_jac: Vector, + ang_jac1: SVector, + ang_jac2: SVector, + ) -> GenericJointConstraint { + let j_id1 = *j_id; + let (ndofs1, solver_vel1, is_rigid_body1) = match mb1 { + LinkOrBodyRef::Link(mb1, link_id1) => { + mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians); + (mb1.ndofs(), mb1.solver_id, false) + } + LinkOrBodyRef::Body(_) => { + body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians); + (SPATIAL_DIM, body1.solver_vel[0], true) + } + LinkOrBodyRef::Fixed => (0, u32::MAX, true), + }; + + let j_id2 = *j_id; + let (ndofs2, solver_vel2, is_rigid_body2) = match mb2 { + LinkOrBodyRef::Link(mb2, link_id2) => { + mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians); + (mb2.ndofs(), mb2.solver_id, false) + } + LinkOrBodyRef::Body(_) => { + body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians); + (SPATIAL_DIM, body2.solver_vel[0], true) + } + LinkOrBodyRef::Fixed => (0, u32::MAX, true), + }; + + let rhs_wo_bias = 0.0; + + GenericJointConstraint { + is_rigid_body1, + is_rigid_body2, + solver_vel1, + solver_vel2, + ndofs1, + j_id1, + ndofs2, + j_id2, + joint_id, + impulse: 0.0, + impulse_bounds: [-Real::MAX, Real::MAX], + inv_lhs: 0.0, + rhs: rhs_wo_bias, + rhs_wo_bias, + cfm_coeff: 0.0, + cfm_gain: 0.0, + writeback_id, + } + } + + pub fn lock_linear_generic( + &self, + params: &IntegrationParameters, + jacobians: &mut DVector, + j_id: &mut usize, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + locked_axis: usize, + writeback_id: WritebackId, + ) -> GenericJointConstraint { + let lin_jac = self.basis.column(locked_axis).into_owned(); + let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); + let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); + + let mut c = self.lock_jacobians_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + writeback_id, + lin_jac, + ang_jac1, + ang_jac2, + ); + + let erp_inv_dt = params.joint_erp_inv_dt(); + let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; + c.rhs += rhs_bias; + c + } + + pub fn limit_linear_generic( + &self, + params: &IntegrationParameters, + jacobians: &mut DVector, + j_id: &mut usize, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + limited_axis: usize, + limits: [Real; 2], + writeback_id: WritebackId, + ) -> GenericJointConstraint { + let lin_jac = self.basis.column(limited_axis).into_owned(); + let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); + let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); + + let mut constraint = self.lock_jacobians_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + writeback_id, + lin_jac, + ang_jac1, + ang_jac2, + ); + + let dist = self.lin_err.dot(&lin_jac); + let min_enabled = dist <= limits[0]; + let max_enabled = limits[1] <= dist; + + let erp_inv_dt = params.joint_erp_inv_dt(); + let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt; + constraint.rhs += rhs_bias; + constraint.impulse_bounds = [ + min_enabled as u32 as Real * -Real::MAX, + max_enabled as u32 as Real * Real::MAX, + ]; + + constraint + } + + pub fn motor_linear_generic( + &self, + jacobians: &mut DVector, + j_id: &mut usize, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + motor_axis: usize, + motor_params: &MotorParameters, + writeback_id: WritebackId, + ) -> GenericJointConstraint { + let lin_jac = self.basis.column(motor_axis).into_owned(); + let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); + let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); + + // TODO: do we need the same trick as for the non-generic constraint? + // if locked_ang_axes & (1 << motor_axis) != 0 { + // // FIXME: check that this also works for cases + // // whene not all the angular axes are locked. + // constraint.ang_jac1.fill(0.0); + // constraint.ang_jac2.fill(0.0); + // } + + let mut constraint = self.lock_jacobians_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + writeback_id, + lin_jac, + ang_jac1, + ang_jac2, + ); + + let mut rhs_wo_bias = 0.0; + if motor_params.erp_inv_dt != 0.0 { + let dist = self.lin_err.dot(&lin_jac); + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; + } + + rhs_wo_bias += -motor_params.target_vel; + + constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; + constraint.rhs = rhs_wo_bias; + constraint.rhs_wo_bias = rhs_wo_bias; + constraint.cfm_coeff = motor_params.cfm_coeff; + constraint.cfm_gain = motor_params.cfm_gain; + constraint + } + + pub fn lock_angular_generic( + &self, + params: &IntegrationParameters, + jacobians: &mut DVector, + j_id: &mut usize, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + _locked_axis: usize, + writeback_id: WritebackId, + ) -> GenericJointConstraint { + #[cfg(feature = "dim2")] + let ang_jac = Vector1::new(1.0); + #[cfg(feature = "dim3")] + let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); + + let mut constraint = self.lock_jacobians_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + writeback_id, + na::zero(), + ang_jac, + ang_jac, + ); + + let erp_inv_dt = params.joint_erp_inv_dt(); + #[cfg(feature = "dim2")] + let rhs_bias = self.ang_err.im * erp_inv_dt; + #[cfg(feature = "dim3")] + let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; + constraint.rhs += rhs_bias; + constraint + } + + pub fn limit_angular_generic( + &self, + params: &IntegrationParameters, + jacobians: &mut DVector, + j_id: &mut usize, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + _limited_axis: usize, + limits: [Real; 2], + writeback_id: WritebackId, + ) -> GenericJointConstraint { + #[cfg(feature = "dim2")] + let ang_jac = Vector1::new(1.0); + #[cfg(feature = "dim3")] + let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); + + let mut constraint = self.lock_jacobians_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + writeback_id, + na::zero(), + ang_jac, + ang_jac, + ); + + let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; + #[cfg(feature = "dim2")] + let s_ang = (self.ang_err.angle() / 2.0).sin(); + #[cfg(feature = "dim3")] + let s_ang = self.ang_err.imag()[_limited_axis]; + let min_enabled = s_ang <= s_limits[0]; + let max_enabled = s_limits[1] <= s_ang; + let impulse_bounds = [ + min_enabled as u32 as Real * -Real::MAX, + max_enabled as u32 as Real * Real::MAX, + ]; + + let erp_inv_dt = params.joint_erp_inv_dt(); + let rhs_bias = + ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; + + constraint.rhs += rhs_bias; + constraint.impulse_bounds = impulse_bounds; + constraint + } + + pub fn motor_angular_generic( + &self, + jacobians: &mut DVector, + j_id: &mut usize, + joint_id: JointIndex, + body1: &JointSolverBody, + body2: &JointSolverBody, + mb1: LinkOrBodyRef, + mb2: LinkOrBodyRef, + _motor_axis: usize, + motor_params: &MotorParameters, + writeback_id: WritebackId, + ) -> GenericJointConstraint { + #[cfg(feature = "dim2")] + let ang_jac = na::Vector1::new(1.0); + #[cfg(feature = "dim3")] + let ang_jac = self.basis.column(_motor_axis).into_owned(); + + let mut constraint = self.lock_jacobians_generic( + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + writeback_id, + na::zero(), + ang_jac, + ang_jac, + ); + + let mut rhs_wo_bias = 0.0; + if motor_params.erp_inv_dt != 0.0 { + #[cfg(feature = "dim2")] + let s_ang_dist = (self.ang_err.angle() / 2.0).sin(); + #[cfg(feature = "dim3")] + let s_ang_dist = self.ang_err.imag()[_motor_axis]; + let s_target_ang = (motor_params.target_pos / 2.0).sin(); + rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) + * motor_params.erp_inv_dt; + } + + rhs_wo_bias += -motor_params.target_vel; + + constraint.rhs_wo_bias = rhs_wo_bias; + constraint.rhs = rhs_wo_bias; + constraint.cfm_coeff = motor_params.cfm_coeff; + constraint.cfm_gain = motor_params.cfm_gain; + constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; + constraint + } + + pub fn finalize_generic_constraints( + jacobians: &mut DVector, + constraints: &mut [GenericJointConstraint], + ) { + // TODO: orthogonalization doesn’t seem to give good results for multibodies? + const ORTHOGONALIZE: bool = false; + let len = constraints.len(); + + if len == 0 { + return; + } + + let ndofs1 = constraints[0].ndofs1; + let ndofs2 = constraints[0].ndofs2; + + // Use the modified Gramm-Schmidt orthogonalization. + for j in 0..len { + let c_j = &mut constraints[j]; + + let jac_j1 = jacobians.rows(c_j.j_id1, ndofs1); + let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2); + let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1); + let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); + + let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2); + let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; + let inv_dot_jj = crate::utils::simd_inv(dot_jj); + c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. + c_j.cfm_gain = cfm_gain; + + if c_j.impulse_bounds != [-Real::MAX, Real::MAX] { + // Don't remove constraints with limited forces from the others + // because they may not deliver the necessary forces to fulfill + // the removed parts of other constraints. + continue; + } + + if !ORTHOGONALIZE { + continue; + } + + for i in (j + 1)..len { + let (c_i, c_j) = constraints.index_mut_const(i, j); + + let jac_i1 = jacobians.rows(c_i.j_id1, ndofs1); + let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2); + let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1); + let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); + + let dot_ij = jac_i1.dot(&w_jac_j1) + jac_i2.dot(&w_jac_j2); + let coeff = dot_ij * inv_dot_jj; + + let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut( + c_i.j_id1..c_i.j_id1 + 2 * (ndofs1 + ndofs2), + c_j.j_id1..c_j.j_id1 + 2 * (ndofs1 + ndofs2), + ); + + jac_i.axpy(-coeff, &jac_j, 1.0); + + c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff; + c_i.rhs -= c_j.rhs * coeff; + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index cd8d336..0558782 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -2,31 +2,30 @@ use crate::dynamics::solver::ConstraintsCounts; use crate::dynamics::solver::MotorParameters; use crate::dynamics::solver::joint_constraint::JointSolverBody; use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ - JointFixedSolverBody, JointOneBodyConstraint, JointTwoBodyConstraint, WritebackId, + JointConstraint, WritebackId, }; -use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::solver_body::SolverBodies; use crate::dynamics::{GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex}; use crate::math::{ANG_DIM, AngVector, DIM, Isometry, Matrix, Point, Real, Rotation, Vector}; use crate::prelude::RigidBodySet; use crate::utils; use crate::utils::{IndexMut2, SimdCrossMatrix, SimdDot, SimdRealCopy}; -use na::SMatrix; - #[cfg(feature = "dim3")] use crate::utils::{SimdBasis, SimdQuat}; +use na::SMatrix; #[cfg(feature = "simd-is-enabled")] use crate::math::{SIMD_WIDTH, SimdReal}; -pub struct JointTwoBodyConstraintBuilder { - body1: usize, - body2: usize, +pub struct JointConstraintBuilder { + body1: u32, + body2: u32, joint_id: JointIndex, joint: GenericJoint, constraint_id: usize, } -impl JointTwoBodyConstraintBuilder { +impl JointConstraintBuilder { pub fn generate( joint: &ImpulseJoint, bodies: &RigidBodySet, @@ -36,13 +35,19 @@ impl JointTwoBodyConstraintBuilder { ) { let rb1 = &bodies[joint.body1]; let rb2 = &bodies[joint.body2]; + let solver_body1 = rb1.effective_active_set_offset(); + let solver_body2 = rb2.effective_active_set_offset(); + *out_builder = Self { - body1: rb1.ids.active_set_offset, - body2: rb2.ids.active_set_offset, + body1: solver_body1, + body2: solver_body2, joint_id, joint: joint.data, constraint_id: *out_constraint_id, }; + // Since solver body poses are given in center-of-mass space, + // we need to transform the anchors to that space. + out_builder.joint.transform_to_solver_body_space(rb1, rb2); let count = ConstraintsCounts::from_joint(joint); *out_constraint_id += count.num_constraints; @@ -51,31 +56,33 @@ impl JointTwoBodyConstraintBuilder { pub fn update( &self, params: &IntegrationParameters, - bodies: &[SolverBody], - out: &mut [JointTwoBodyConstraint], + bodies: &SolverBodies, + out: &mut [JointConstraint], ) { // NOTE: right now, the "update", is basically reconstructing all the // constraints. Could we make this more incremental? - let rb1 = &bodies[self.body1]; - let rb2 = &bodies[self.body2]; - let frame1 = rb1.position * self.joint.local_frame1; - let frame2 = rb2.position * self.joint.local_frame2; + let rb1 = bodies.get_pose(self.body1); + let rb2 = bodies.get_pose(self.body2); + let frame1 = rb1.pose * self.joint.local_frame1; + let frame2 = rb2.pose * self.joint.local_frame2; + let world_com1 = Point::from(rb1.pose.translation.vector); + let world_com2 = Point::from(rb2.pose.translation.vector); let joint_body1 = JointSolverBody { im: rb1.im, - sqrt_ii: rb1.sqrt_ii, - world_com: rb1.world_com, + ii: rb1.ii, + world_com: world_com1, solver_vel: [self.body1], }; let joint_body2 = JointSolverBody { im: rb2.im, - sqrt_ii: rb2.sqrt_ii, - world_com: rb2.world_com, + ii: rb2.ii, + world_com: world_com2, solver_vel: [self.body2], }; - JointTwoBodyConstraint::::lock_axes( + JointConstraint::::update( params, self.joint_id, &joint_body1, @@ -89,11 +96,9 @@ impl JointTwoBodyConstraintBuilder { } #[cfg(feature = "simd-is-enabled")] -pub struct JointTwoBodyConstraintBuilderSimd { - body1: [usize; SIMD_WIDTH], - body2: [usize; SIMD_WIDTH], - joint_body1: JointSolverBody, - joint_body2: JointSolverBody, +pub struct JointConstraintBuilderSimd { + body1: [u32; SIMD_WIDTH], + body2: [u32; SIMD_WIDTH], joint_id: [JointIndex; SIMD_WIDTH], local_frame1: Isometry, local_frame2: Isometry, @@ -102,7 +107,7 @@ pub struct JointTwoBodyConstraintBuilderSimd { } #[cfg(feature = "simd-is-enabled")] -impl JointTwoBodyConstraintBuilderSimd { +impl JointConstraintBuilderSimd { pub fn generate( joint: [&ImpulseJoint; SIMD_WIDTH], bodies: &RigidBodySet, @@ -110,33 +115,39 @@ impl JointTwoBodyConstraintBuilderSimd { out_builder: &mut Self, out_constraint_id: &mut usize, ) { - let rb1 = gather![|ii| &bodies[joint[ii].body1]]; - let rb2 = gather![|ii| &bodies[joint[ii].body2]]; + let rb1 = array![|ii| &bodies[joint[ii].body1]]; + let rb2 = array![|ii| &bodies[joint[ii].body2]]; - let body1 = gather![|ii| rb1[ii].ids.active_set_offset]; - let body2 = gather![|ii| rb2[ii].ids.active_set_offset]; + let body1 = array![|ii| if rb1[ii].is_dynamic_or_kinematic() { + rb1[ii].ids.active_set_offset + } else { + u32::MAX + }]; + let body2 = array![|ii| if rb2[ii].is_dynamic_or_kinematic() { + rb2[ii].ids.active_set_offset + } else { + u32::MAX + }]; - let joint_body1 = JointSolverBody { - im: gather![|ii| rb1[ii].mprops.effective_inv_mass].into(), - sqrt_ii: gather![|ii| rb1[ii].mprops.effective_world_inv_inertia_sqrt].into(), - world_com: Point::origin(), - solver_vel: body1, - }; - let joint_body2 = JointSolverBody { - im: gather![|ii| rb2[ii].mprops.effective_inv_mass].into(), - sqrt_ii: gather![|ii| rb2[ii].mprops.effective_world_inv_inertia_sqrt].into(), - world_com: Point::origin(), - solver_vel: body2, - }; + let local_frame1 = array![|ii| if body1[ii] != u32::MAX { + joint[ii].data.local_frame1 + } else { + rb1[ii].pos.position * joint[ii].data.local_frame1 + }] + .into(); + let local_frame2 = array![|ii| if body2[ii] != u32::MAX { + joint[ii].data.local_frame2 + } else { + rb2[ii].pos.position * joint[ii].data.local_frame2 + }] + .into(); *out_builder = Self { body1, body2, - joint_body1, - joint_body2, joint_id, - local_frame1: gather![|ii| joint[ii].data.local_frame1].into(), - local_frame2: gather![|ii| joint[ii].data.local_frame2].into(), + local_frame1, + local_frame2, locked_axes: joint[0].data.locked_axes.bits(), constraint_id: *out_constraint_id, }; @@ -148,24 +159,35 @@ impl JointTwoBodyConstraintBuilderSimd { pub fn update( &mut self, params: &IntegrationParameters, - bodies: &[SolverBody], - out: &mut [JointTwoBodyConstraint], + bodies: &SolverBodies, + out: &mut [JointConstraint], ) { // NOTE: right now, the "update", is basically reconstructing all the // constraints. Could we make this more incremental? - let rb1 = gather![|ii| &bodies[self.body1[ii]]]; - let rb2 = gather![|ii| &bodies[self.body2[ii]]]; - let frame1 = Isometry::from(gather![|ii| rb1[ii].position]) * self.local_frame1; - let frame2 = Isometry::from(gather![|ii| rb2[ii].position]) * self.local_frame2; - self.joint_body1.world_com = gather![|ii| rb1[ii].world_com].into(); - self.joint_body2.world_com = gather![|ii| rb2[ii].world_com].into(); + let rb1 = bodies.gather_poses(self.body1); + let rb2 = bodies.gather_poses(self.body2); + let frame1 = rb1.pose * self.local_frame1; + let frame2 = rb2.pose * self.local_frame2; - JointTwoBodyConstraint::::lock_axes( + let joint_body1 = JointSolverBody { + im: rb1.im, + ii: rb1.ii, + world_com: rb1.pose.translation.vector.into(), + solver_vel: self.body1, + }; + let joint_body2 = JointSolverBody { + im: rb2.im, + ii: rb2.ii, + world_com: rb2.pose.translation.vector.into(), + solver_vel: self.body2, + }; + + JointConstraint::::update( params, self.joint_id, - &self.joint_body1, - &self.joint_body2, + &joint_body1, + &joint_body2, &frame1, &frame2, self.locked_axes, @@ -174,176 +196,8 @@ impl JointTwoBodyConstraintBuilderSimd { } } -pub struct JointOneBodyConstraintBuilder { - body1: JointFixedSolverBody, - frame1: Isometry, - body2: usize, - joint_id: JointIndex, - joint: GenericJoint, - constraint_id: usize, -} - -impl JointOneBodyConstraintBuilder { - pub fn generate( - joint: &ImpulseJoint, - bodies: &RigidBodySet, - joint_id: JointIndex, - out_builder: &mut Self, - out_constraint_id: &mut usize, - ) { - let mut joint_data = joint.data; - let mut handle1 = joint.body1; - let mut handle2 = joint.body2; - let flipped = !bodies[handle2].is_dynamic(); - - if flipped { - std::mem::swap(&mut handle1, &mut handle2); - joint_data.flip(); - }; - - let rb1 = &bodies[handle1]; - let rb2 = &bodies[handle2]; - - let frame1 = rb1.pos.position * joint_data.local_frame1; - let joint_body1 = JointFixedSolverBody { - linvel: rb1.vels.linvel, - angvel: rb1.vels.angvel, - world_com: rb1.mprops.world_com, - }; - - *out_builder = Self { - body1: joint_body1, - frame1, - body2: rb2.ids.active_set_offset, - joint_id, - joint: joint_data, - constraint_id: *out_constraint_id, - }; - - let count = ConstraintsCounts::from_joint(joint); - *out_constraint_id += count.num_constraints; - } - - pub fn update( - &self, - params: &IntegrationParameters, - bodies: &[SolverBody], - out: &mut [JointOneBodyConstraint], - ) { - // NOTE: right now, the "update", is basically reconstructing all the - // constraints. Could we make this more incremental? - - let rb2 = &bodies[self.body2]; - let frame2 = rb2.position * self.joint.local_frame2; - - let joint_body2 = JointSolverBody { - im: rb2.im, - sqrt_ii: rb2.sqrt_ii, - world_com: rb2.world_com, - solver_vel: [self.body2], - }; - - JointOneBodyConstraint::::lock_axes( - params, - self.joint_id, - &self.body1, - &joint_body2, - &self.frame1, - &frame2, - &self.joint, - &mut out[self.constraint_id..], - ); - } -} - -#[cfg(feature = "simd-is-enabled")] -pub struct JointOneBodyConstraintBuilderSimd { - body1: JointFixedSolverBody, - frame1: Isometry, - body2: [usize; SIMD_WIDTH], - joint_id: [JointIndex; SIMD_WIDTH], - local_frame2: Isometry, - locked_axes: u8, - constraint_id: usize, -} - -#[cfg(feature = "simd-is-enabled")] -impl JointOneBodyConstraintBuilderSimd { - pub fn generate( - joint: [&ImpulseJoint; SIMD_WIDTH], - bodies: &RigidBodySet, - joint_id: [JointIndex; SIMD_WIDTH], - out_builder: &mut Self, - out_constraint_id: &mut usize, - ) { - let mut rb1 = gather![|ii| &bodies[joint[ii].body1]]; - let mut rb2 = gather![|ii| &bodies[joint[ii].body2]]; - let mut local_frame1 = gather![|ii| joint[ii].data.local_frame1]; - let mut local_frame2 = gather![|ii| joint[ii].data.local_frame2]; - - for ii in 0..SIMD_WIDTH { - if !rb2[ii].is_dynamic() { - std::mem::swap(&mut rb1[ii], &mut rb2[ii]); - std::mem::swap(&mut local_frame1[ii], &mut local_frame2[ii]); - } - } - - let poss1 = Isometry::from(gather![|ii| rb1[ii].pos.position]); - - let joint_body1 = JointFixedSolverBody { - linvel: gather![|ii| rb1[ii].vels.linvel].into(), - angvel: gather![|ii| rb1[ii].vels.angvel].into(), - world_com: gather![|ii| rb1[ii].mprops.world_com].into(), - }; - - *out_builder = Self { - body1: joint_body1, - body2: gather![|ii| rb2[ii].ids.active_set_offset], - joint_id, - frame1: poss1 * Isometry::from(local_frame1), - local_frame2: local_frame2.into(), - locked_axes: joint[0].data.locked_axes.bits(), - constraint_id: *out_constraint_id, - }; - - let count = ConstraintsCounts::from_joint(joint[0]); - *out_constraint_id += count.num_constraints; - } - - pub fn update( - &self, - params: &IntegrationParameters, - bodies: &[SolverBody], - out: &mut [JointOneBodyConstraint], - ) { - // NOTE: right now, the "update", is basically reconstructing all the - // constraints. Could we make this more incremental? - - let rb2 = gather![|ii| &bodies[self.body2[ii]]]; - let frame2 = Isometry::from(gather![|ii| rb2[ii].position]) * self.local_frame2; - - let joint_body2 = JointSolverBody { - im: gather![|ii| rb2[ii].im].into(), - sqrt_ii: gather![|ii| rb2[ii].sqrt_ii].into(), - world_com: gather![|ii| rb2[ii].world_com].into(), - solver_vel: self.body2, - }; - - JointOneBodyConstraint::::lock_axes( - params, - self.joint_id, - &self.body1, - &joint_body2, - &self.frame1, - &frame2, - self.locked_axes, - &mut out[self.constraint_id..], - ); - } -} - #[derive(Debug, Copy, Clone)] -pub struct JointTwoBodyConstraintHelper { +pub struct JointConstraintHelper { pub basis: Matrix, #[cfg(feature = "dim3")] pub basis2: Matrix, // TODO: used for angular coupling. Can we avoid storing this? @@ -355,7 +209,7 @@ pub struct JointTwoBodyConstraintHelper { pub ang_err: Rotation, } -impl JointTwoBodyConstraintHelper { +impl JointConstraintHelper { pub fn new( frame1: &Isometry, frame2: &Isometry, @@ -423,7 +277,7 @@ impl JointTwoBodyConstraintHelper { limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { let zero = N::zero(); let mut constraint = self.lock_linear(params, joint_id, body1, body2, limited_axis, writeback_id); @@ -455,7 +309,7 @@ impl JointTwoBodyConstraintHelper { coupled_axes: u8, limits: [N; 2], writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { let zero = N::zero(); let mut lin_jac = Vector::zeros(); let mut ang_jac1: AngVector = na::zero(); @@ -488,8 +342,8 @@ impl JointTwoBodyConstraintHelper { let rhs_wo_bias = (dist - limits[1]).simd_min(zero) * N::splat(params.inv_dt()); - ang_jac1 = body1.sqrt_ii * ang_jac1; - ang_jac2 = body2.sqrt_ii * ang_jac2; + let ii_ang_jac1 = body1.ii * ang_jac1; + let ii_ang_jac2 = body2.ii * ang_jac2; let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); @@ -497,7 +351,7 @@ impl JointTwoBodyConstraintHelper { let rhs = rhs_wo_bias + rhs_bias; let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; - JointTwoBodyConstraint { + JointConstraint { joint_id, solver_vel1: body1.solver_vel, solver_vel2: body2.solver_vel, @@ -508,6 +362,8 @@ impl JointTwoBodyConstraintHelper { lin_jac, ang_jac1, ang_jac2, + ii_ang_jac1, + ii_ang_jac2, inv_lhs: N::zero(), // Will be set during orthogonalization. cfm_coeff, cfm_gain: N::zero(), @@ -527,7 +383,7 @@ impl JointTwoBodyConstraintHelper { motor_params: &MotorParameters, limits: Option<[N; 2]>, writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { let inv_dt = N::splat(params.inv_dt()); let mut constraint = self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id); @@ -565,7 +421,7 @@ impl JointTwoBodyConstraintHelper { motor_params: &MotorParameters, limits: Option<[N; 2]>, writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { let inv_dt = N::splat(params.inv_dt()); let mut lin_jac = Vector::zeros(); @@ -608,10 +464,10 @@ impl JointTwoBodyConstraintHelper { rhs_wo_bias += -target_vel; - ang_jac1 = body1.sqrt_ii * ang_jac1; - ang_jac2 = body2.sqrt_ii * ang_jac2; + let ii_ang_jac1 = body1.ii * ang_jac1; + let ii_ang_jac2 = body2.ii * ang_jac2; - JointTwoBodyConstraint { + JointConstraint { joint_id, solver_vel1: body1.solver_vel, solver_vel2: body2.solver_vel, @@ -622,6 +478,8 @@ impl JointTwoBodyConstraintHelper { lin_jac, ang_jac1, ang_jac2, + ii_ang_jac1, + ii_ang_jac2, inv_lhs: N::zero(), // Will be set during orthogonalization. cfm_coeff: motor_params.cfm_coeff, cfm_gain: motor_params.cfm_gain, @@ -639,26 +497,26 @@ impl JointTwoBodyConstraintHelper { body2: &JointSolverBody, locked_axis: usize, writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { let lin_jac = self.basis.column(locked_axis).into_owned(); #[cfg(feature = "dim2")] - let mut ang_jac1 = self.cmat1_basis[locked_axis]; + let ang_jac1 = self.cmat1_basis[locked_axis]; #[cfg(feature = "dim2")] - let mut ang_jac2 = self.cmat2_basis[locked_axis]; + let ang_jac2 = self.cmat2_basis[locked_axis]; #[cfg(feature = "dim3")] - let mut ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); + let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); #[cfg(feature = "dim3")] - let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); + let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); let rhs_wo_bias = N::zero(); let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; - ang_jac1 = body1.sqrt_ii * ang_jac1; - ang_jac2 = body2.sqrt_ii * ang_jac2; + let ii_ang_jac1 = body1.ii * ang_jac1; + let ii_ang_jac2 = body2.ii * ang_jac2; - JointTwoBodyConstraint { + JointConstraint { joint_id, solver_vel1: body1.solver_vel, solver_vel2: body2.solver_vel, @@ -669,6 +527,8 @@ impl JointTwoBodyConstraintHelper { lin_jac, ang_jac1, ang_jac2, + ii_ang_jac1, + ii_ang_jac2, inv_lhs: N::zero(), // Will be set during orthogonalization. cfm_coeff, cfm_gain: N::zero(), @@ -687,7 +547,7 @@ impl JointTwoBodyConstraintHelper { _limited_axis: usize, limits: [N; 2], writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { let zero = N::zero(); let half = N::splat(0.5); let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()]; @@ -714,10 +574,10 @@ impl JointTwoBodyConstraintHelper { - (s_limits[0] - s_ang).simd_max(zero)) * erp_inv_dt; - let ang_jac1 = body1.sqrt_ii * ang_jac; - let ang_jac2 = body2.sqrt_ii * ang_jac; + let ii_ang_jac1 = body1.ii * ang_jac; + let ii_ang_jac2 = body2.ii * ang_jac; - JointTwoBodyConstraint { + JointConstraint { joint_id, solver_vel1: body1.solver_vel, solver_vel2: body2.solver_vel, @@ -726,8 +586,10 @@ impl JointTwoBodyConstraintHelper { impulse: N::zero(), impulse_bounds, lin_jac: na::zero(), - ang_jac1, - ang_jac2, + ang_jac1: ang_jac, + ang_jac2: ang_jac, + ii_ang_jac1, + ii_ang_jac2, inv_lhs: N::zero(), // Will be set during orthogonalization. cfm_coeff, cfm_gain: N::zero(), @@ -745,7 +607,7 @@ impl JointTwoBodyConstraintHelper { _motor_axis: usize, motor_params: &MotorParameters, writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] @@ -774,10 +636,10 @@ impl JointTwoBodyConstraintHelper { rhs_wo_bias += -motor_params.target_vel; - let ang_jac1 = body1.sqrt_ii * ang_jac; - let ang_jac2 = body2.sqrt_ii * ang_jac; + let ii_ang_jac1 = body1.ii * ang_jac; + let ii_ang_jac2 = body2.ii * ang_jac; - JointTwoBodyConstraint { + JointConstraint { joint_id, solver_vel1: body1.solver_vel, solver_vel2: body2.solver_vel, @@ -786,8 +648,10 @@ impl JointTwoBodyConstraintHelper { impulse: N::zero(), impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], lin_jac: na::zero(), - ang_jac1, - ang_jac2, + ang_jac1: ang_jac, + ang_jac2: ang_jac, + ii_ang_jac1, + ii_ang_jac2, inv_lhs: N::zero(), // Will be set during orthogonalization. cfm_coeff: motor_params.cfm_coeff, cfm_gain: motor_params.cfm_gain, @@ -805,7 +669,7 @@ impl JointTwoBodyConstraintHelper { body2: &JointSolverBody, _locked_axis: usize, writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { #[cfg(feature = "dim2")] let ang_jac = N::one(); #[cfg(feature = "dim3")] @@ -819,10 +683,10 @@ impl JointTwoBodyConstraintHelper { #[cfg(feature = "dim3")] let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; - let ang_jac1 = body1.sqrt_ii * ang_jac; - let ang_jac2 = body2.sqrt_ii * ang_jac; + let ii_ang_jac1 = body1.ii * ang_jac; + let ii_ang_jac2 = body2.ii * ang_jac; - JointTwoBodyConstraint { + JointConstraint { joint_id, solver_vel1: body1.solver_vel, solver_vel2: body2.solver_vel, @@ -831,8 +695,10 @@ impl JointTwoBodyConstraintHelper { impulse: N::zero(), impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)], lin_jac: na::zero(), - ang_jac1, - ang_jac2, + ang_jac1: ang_jac, + ang_jac2: ang_jac, + ii_ang_jac1, + ii_ang_jac2, inv_lhs: N::zero(), // Will be set during orthogonalization. cfm_coeff, cfm_gain: N::zero(), @@ -843,9 +709,7 @@ impl JointTwoBodyConstraintHelper { } /// Orthogonalize the constraints and set their inv_lhs field. - pub fn finalize_constraints( - constraints: &mut [JointTwoBodyConstraint], - ) { + pub fn finalize_constraints(constraints: &mut [JointConstraint]) { let len = constraints.len(); if len == 0 { @@ -858,8 +722,8 @@ impl JointTwoBodyConstraintHelper { for j in 0..len { let c_j = &mut constraints[j]; let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) - + c_j.ang_jac1.gdot(c_j.ang_jac1) - + c_j.ang_jac2.gdot(c_j.ang_jac2); + + c_j.ii_ang_jac1.gdot(c_j.ang_jac1) + + c_j.ii_ang_jac2.gdot(c_j.ang_jac2); let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; let inv_dot_jj = crate::utils::simd_inv(dot_jj); c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. @@ -876,491 +740,15 @@ impl JointTwoBodyConstraintHelper { let (c_i, c_j) = constraints.index_mut_const(i, j); let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) - + c_i.ang_jac1.gdot(c_j.ang_jac1) - + c_i.ang_jac2.gdot(c_j.ang_jac2); + + c_i.ii_ang_jac1.gdot(c_j.ang_jac1) + + c_i.ii_ang_jac2.gdot(c_j.ang_jac2); let coeff = dot_ij * inv_dot_jj; c_i.lin_jac -= c_j.lin_jac * coeff; c_i.ang_jac1 -= c_j.ang_jac1 * coeff; c_i.ang_jac2 -= c_j.ang_jac2 * coeff; - c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff; - c_i.rhs -= c_j.rhs * coeff; - } - } - } - - pub fn limit_linear_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - limited_axis: usize, - limits: [N; 2], - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - let zero = N::zero(); - let lin_jac = self.basis.column(limited_axis).into_owned(); - let dist = self.lin_err.dot(&lin_jac); - - let min_enabled = dist.simd_le(limits[0]); - let max_enabled = limits[1].simd_le(dist); - - let impulse_bounds = [ - N::splat(-Real::INFINITY).select(min_enabled, zero), - N::splat(Real::INFINITY).select(max_enabled, zero), - ]; - - let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); - #[cfg(feature = "dim2")] - let mut ang_jac2 = self.cmat2_basis[limited_axis]; - #[cfg(feature = "dim3")] - let mut ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); - - let rhs_wo_bias = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); - let rhs_bias = - ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt; - - ang_jac2 = body2.sqrt_ii * ang_jac2; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: zero, - impulse_bounds, - lin_jac, - ang_jac2, - inv_lhs: zero, // Will be set during orthogonalization. - cfm_coeff, - cfm_gain: N::zero(), - rhs: rhs_wo_bias + rhs_bias, - rhs_wo_bias, - writeback_id, - } - } - - pub fn limit_linear_coupled_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - coupled_axes: u8, - limits: [N; 2], - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - let zero = N::zero(); - let mut lin_jac = Vector::zeros(); - let mut ang_jac1: AngVector = na::zero(); - let mut ang_jac2: AngVector = na::zero(); - - for i in 0..DIM { - if coupled_axes & (1 << i) != 0 { - let coeff = self.basis.column(i).dot(&self.lin_err); - lin_jac += self.basis.column(i) * coeff; - #[cfg(feature = "dim2")] - { - ang_jac1 += self.cmat1_basis[i] * coeff; - ang_jac2 += self.cmat2_basis[i] * coeff; - } - #[cfg(feature = "dim3")] - { - ang_jac1 += self.cmat1_basis.column(i) * coeff; - ang_jac2 += self.cmat2_basis.column(i) * coeff; - } - } - } - - let dist = lin_jac.norm(); - let inv_dist = crate::utils::simd_inv(dist); - lin_jac *= inv_dist; - ang_jac1 *= inv_dist; - ang_jac2 *= inv_dist; - - // FIXME: handle min limit too. - let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - let rhs_wo_bias = proj_vel1 + (dist - limits[1]).simd_min(zero) * N::splat(params.inv_dt()); - - ang_jac2 = body2.sqrt_ii * ang_jac2; - - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); - let rhs_bias = (dist - limits[1]).simd_max(zero) * erp_inv_dt; - let rhs = rhs_wo_bias + rhs_bias; - let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: N::zero(), - impulse_bounds, - lin_jac, - ang_jac2, - inv_lhs: N::zero(), // Will be set during orthogonalization. - cfm_coeff, - cfm_gain: N::zero(), - rhs, - rhs_wo_bias, - writeback_id, - } - } - - pub fn motor_linear_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - motor_axis: usize, - motor_params: &MotorParameters, - limits: Option<[N; 2]>, - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - let inv_dt = N::splat(params.inv_dt()); - - let lin_jac = self.basis.column(motor_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); - #[cfg(feature = "dim2")] - let mut ang_jac2 = self.cmat2_basis[motor_axis]; - #[cfg(feature = "dim3")] - let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); - - let mut rhs_wo_bias = N::zero(); - if motor_params.erp_inv_dt != N::zero() { - let dist = self.lin_err.dot(&lin_jac); - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; - } - - let mut target_vel = motor_params.target_vel; - if let Some(limits) = limits { - let dist = self.lin_err.dot(&lin_jac); - target_vel = - target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); - }; - - let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - rhs_wo_bias += proj_vel1 - target_vel; - - ang_jac2 = body2.sqrt_ii * ang_jac2; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: N::zero(), - impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], - lin_jac, - ang_jac2, - inv_lhs: N::zero(), // Will be set during orthogonalization. - cfm_coeff: motor_params.cfm_coeff, - cfm_gain: motor_params.cfm_gain, - rhs: rhs_wo_bias, - rhs_wo_bias, - writeback_id, - } - } - - pub fn motor_linear_coupled_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - coupled_axes: u8, - motor_params: &MotorParameters, - limits: Option<[N; 2]>, - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - let inv_dt = N::splat(params.inv_dt()); - - let mut lin_jac = Vector::zeros(); - let mut ang_jac1: AngVector = na::zero(); - let mut ang_jac2: AngVector = na::zero(); - - for i in 0..DIM { - if coupled_axes & (1 << i) != 0 { - let coeff = self.basis.column(i).dot(&self.lin_err); - lin_jac += self.basis.column(i) * coeff; - #[cfg(feature = "dim2")] - { - ang_jac1 += self.cmat1_basis[i] * coeff; - ang_jac2 += self.cmat2_basis[i] * coeff; - } - #[cfg(feature = "dim3")] - { - ang_jac1 += self.cmat1_basis.column(i) * coeff; - ang_jac2 += self.cmat2_basis.column(i) * coeff; - } - } - } - - let dist = lin_jac.norm(); - let inv_dist = crate::utils::simd_inv(dist); - lin_jac *= inv_dist; - ang_jac1 *= inv_dist; - ang_jac2 *= inv_dist; - - let mut rhs_wo_bias = N::zero(); - if motor_params.erp_inv_dt != N::zero() { - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; - } - - let mut target_vel = motor_params.target_vel; - if let Some(limits) = limits { - target_vel = - target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); - }; - - let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - rhs_wo_bias += proj_vel1 - target_vel; - - ang_jac2 = body2.sqrt_ii * ang_jac2; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: N::zero(), - impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], - lin_jac, - ang_jac2, - inv_lhs: N::zero(), // Will be set during orthogonalization. - cfm_coeff: motor_params.cfm_coeff, - cfm_gain: motor_params.cfm_gain, - rhs: rhs_wo_bias, - rhs_wo_bias, - writeback_id, - } - } - - pub fn lock_linear_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - locked_axis: usize, - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - let lin_jac = self.basis.column(locked_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); - #[cfg(feature = "dim2")] - let mut ang_jac2 = self.cmat2_basis[locked_axis]; - #[cfg(feature = "dim3")] - let mut ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); - - let rhs_wo_bias = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); - let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; - - ang_jac2 = body2.sqrt_ii * ang_jac2; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: N::zero(), - impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)], - lin_jac, - ang_jac2, - inv_lhs: N::zero(), // Will be set during orthogonalization. - cfm_coeff, - cfm_gain: N::zero(), - rhs: rhs_wo_bias + rhs_bias, - rhs_wo_bias, - writeback_id, - } - } - - pub fn motor_angular_one_body( - &self, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - _motor_axis: usize, - motor_params: &MotorParameters, - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = N::one(); - #[cfg(feature = "dim3")] - let ang_jac = self.basis.column(_motor_axis).into_owned(); - - let mut rhs_wo_bias = N::zero(); - if motor_params.erp_inv_dt != N::zero() { - #[cfg(feature = "dim2")] - let ang_dist = self.ang_err.angle(); - #[cfg(feature = "dim3")] - let ang_dist = self.ang_err.imag()[_motor_axis].simd_asin() * N::splat(2.0); - let target_ang = motor_params.target_pos; - rhs_wo_bias += utils::smallest_abs_diff_between_angles(ang_dist, target_ang) - * motor_params.erp_inv_dt; - } - - let proj_vel1 = -ang_jac.gdot(body1.angvel); - rhs_wo_bias += proj_vel1 - motor_params.target_vel; - - let ang_jac2 = body2.sqrt_ii * ang_jac; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: N::zero(), - impulse_bounds: [-motor_params.max_impulse, motor_params.max_impulse], - lin_jac: na::zero(), - ang_jac2, - inv_lhs: N::zero(), // Will be set during orthogonalization. - cfm_coeff: motor_params.cfm_coeff, - cfm_gain: motor_params.cfm_gain, - rhs: rhs_wo_bias, - rhs_wo_bias, - writeback_id, - } - } - - pub fn limit_angular_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - _limited_axis: usize, - limits: [N; 2], - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - let zero = N::zero(); - let half = N::splat(0.5); - let s_limits = [(limits[0] * half).simd_sin(), (limits[1] * half).simd_sin()]; - #[cfg(feature = "dim2")] - let s_ang = (self.ang_err.angle() * half).simd_sin(); - #[cfg(feature = "dim3")] - let s_ang = self.ang_err.imag()[_limited_axis]; - let min_enabled = s_ang.simd_le(s_limits[0]); - let max_enabled = s_limits[1].simd_le(s_ang); - - let impulse_bounds = [ - N::splat(-Real::INFINITY).select(min_enabled, zero), - N::splat(Real::INFINITY).select(max_enabled, zero), - ]; - - #[cfg(feature = "dim2")] - let ang_jac = N::one(); - #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); - let rhs_wo_bias = -ang_jac.gdot(body1.angvel); - - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); - let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) - - (s_limits[0] - s_ang).simd_max(zero)) - * erp_inv_dt; - - let ang_jac2 = body2.sqrt_ii * ang_jac; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: zero, - impulse_bounds, - lin_jac: na::zero(), - ang_jac2, - inv_lhs: zero, // Will be set during orthogonalization. - cfm_coeff, - cfm_gain: N::zero(), - rhs: rhs_wo_bias + rhs_bias, - rhs_wo_bias, - writeback_id, - } - } - - pub fn lock_angular_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; LANES], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - _locked_axis: usize, - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = N::one(); - #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); - - let rhs_wo_bias = -ang_jac.gdot(body1.angvel); - - let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); - let cfm_coeff = N::splat(params.joint_cfm_coeff()); - #[cfg(feature = "dim2")] - let rhs_bias = self.ang_err.im * erp_inv_dt; - #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; - - let ang_jac2 = body2.sqrt_ii * ang_jac; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: N::zero(), - impulse_bounds: [-N::splat(Real::MAX), N::splat(Real::MAX)], - lin_jac: na::zero(), - ang_jac2, - inv_lhs: N::zero(), // Will be set during orthogonalization. - cfm_coeff, - cfm_gain: N::zero(), - rhs: rhs_wo_bias + rhs_bias, - rhs_wo_bias, - writeback_id, - } - } - - /// Orthogonalize the constraints and set their inv_lhs field. - pub fn finalize_one_body_constraints( - constraints: &mut [JointOneBodyConstraint], - ) { - let len = constraints.len(); - - if len == 0 { - return; - } - - let imsum = constraints[0].im2; - - // Use the modified Gram-Schmidt orthogonalization. - for j in 0..len { - let c_j = &mut constraints[j]; - let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) - + c_j.ang_jac2.gdot(c_j.ang_jac2); - let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; - let inv_dot_jj = crate::utils::simd_inv(dot_jj); - c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. - c_j.cfm_gain = cfm_gain; - - if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] { - // Don't remove constraints with limited forces from the others - // because they may not deliver the necessary forces to fulfill - // the removed parts of other constraints. - continue; - } - - for i in j + 1..len { - let (c_i, c_j) = constraints.index_mut_const(i, j); - - let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) - + c_i.ang_jac2.gdot(c_j.ang_jac2); - let coeff = dot_ij * inv_dot_jj; - - c_i.lin_jac -= c_j.lin_jac * coeff; - c_i.ang_jac2 -= c_j.ang_jac2 * coeff; + c_i.ii_ang_jac1 -= c_j.ii_ang_jac1 * coeff; + c_i.ii_ang_jac2 -= c_j.ii_ang_jac2 * coeff; c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff; c_i.rhs -= c_j.rhs * coeff; } @@ -1368,9 +756,7 @@ impl JointTwoBodyConstraintHelper { } } -impl JointTwoBodyConstraintHelper { - // TODO: this method is almost identical to the one_body version, except for the - // return type. Could they share their implementation somehow? +impl JointConstraintHelper { #[cfg(feature = "dim3")] pub fn limit_angular_coupled( &self, @@ -1381,7 +767,7 @@ impl JointTwoBodyConstraintHelper { coupled_axes: u8, limits: [Real; 2], writeback_id: WritebackId, - ) -> JointTwoBodyConstraint { + ) -> JointConstraint { // NOTE: right now, this only supports exactly 2 coupled axes. let ang_coupled_axes = coupled_axes >> DIM; assert_eq!(ang_coupled_axes.count_ones(), 2); @@ -1409,10 +795,10 @@ impl JointTwoBodyConstraintHelper { let cfm_coeff = params.joint_cfm_coeff(); let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; - let ang_jac1 = body1.sqrt_ii * ang_jac; - let ang_jac2 = body2.sqrt_ii * ang_jac; + let ii_ang_jac1 = body1.ii * ang_jac; + let ii_ang_jac2 = body2.ii * ang_jac; - JointTwoBodyConstraint { + JointConstraint { joint_id, solver_vel1: body1.solver_vel, solver_vel2: body2.solver_vel, @@ -1421,65 +807,10 @@ impl JointTwoBodyConstraintHelper { impulse: 0.0, impulse_bounds, lin_jac: na::zero(), - ang_jac1, - ang_jac2, - inv_lhs: 0.0, // Will be set during orthogonalization. - cfm_coeff, - cfm_gain: 0.0, - rhs: rhs_wo_bias + rhs_bias, - rhs_wo_bias, - writeback_id, - } - } - - #[cfg(feature = "dim3")] - pub fn limit_angular_coupled_one_body( - &self, - params: &IntegrationParameters, - joint_id: [JointIndex; 1], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - coupled_axes: u8, - limits: [Real; 2], - writeback_id: WritebackId, - ) -> JointOneBodyConstraint { - // NOTE: right now, this only supports exactly 2 coupled axes. - let ang_coupled_axes = coupled_axes >> DIM; - assert_eq!(ang_coupled_axes.count_ones(), 2); - let not_coupled_index = ang_coupled_axes.trailing_ones() as usize; - let axis1 = self.basis.column(not_coupled_index).into_owned(); - let axis2 = self.basis2.column(not_coupled_index).into_owned(); - - let rot = Rotation::rotation_between(&axis1, &axis2).unwrap_or_else(Rotation::identity); - let (ang_jac, angle) = rot - .axis_angle() - .map(|(axis, angle)| (axis.into_inner(), angle)) - .unwrap_or_else(|| (axis1.orthonormal_basis()[0], 0.0)); - - let min_enabled = angle <= limits[0]; - let max_enabled = limits[1] <= angle; - - let impulse_bounds = [ - if min_enabled { -Real::INFINITY } else { 0.0 }, - if max_enabled { Real::INFINITY } else { 0.0 }, - ]; - - let rhs_wo_bias = -ang_jac.gdot(body1.angvel); - - let erp_inv_dt = params.joint_erp_inv_dt(); - let cfm_coeff = params.joint_cfm_coeff(); - let rhs_bias = ((angle - limits[1]).max(0.0) - (limits[0] - angle).max(0.0)) * erp_inv_dt; - - let ang_jac2 = body2.sqrt_ii * ang_jac; - - JointOneBodyConstraint { - joint_id, - solver_vel2: body2.solver_vel, - im2: body2.im, - impulse: 0.0, - impulse_bounds, - lin_jac: na::zero(), - ang_jac2, + ang_jac1: ang_jac, + ang_jac2: ang_jac, + ii_ang_jac1, + ii_ang_jac2, inv_lhs: 0.0, // Will be set during orthogonalization. cfm_coeff, cfm_gain: 0.0, diff --git a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs index 87906f8..c7abe9a 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs @@ -1,11 +1,7 @@ use crate::dynamics::solver::categorization::categorize_joints; -use crate::dynamics::solver::solver_body::SolverBody; -use crate::dynamics::solver::solver_vel::SolverVel; use crate::dynamics::solver::{ - JointConstraintTypes, JointGenericOneBodyConstraint, JointGenericOneBodyConstraintBuilder, - JointGenericTwoBodyConstraint, JointGenericTwoBodyConstraintBuilder, - JointGenericVelocityOneBodyExternalConstraintBuilder, - JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet, reset_buffer, + AnyJointConstraintMut, GenericJointConstraint, JointGenericExternalConstraintBuilder, + JointGenericInternalConstraintBuilder, reset_buffer, }; use crate::dynamics::{ IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, @@ -14,18 +10,92 @@ use crate::dynamics::{ use na::DVector; use parry::math::Real; -use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{ - JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder, -}; +use crate::dynamics::solver::interaction_groups::InteractionGroups; +use crate::dynamics::solver::joint_constraint::generic_joint_constraint_builder::GenericJointConstraintBuilder; +use crate::dynamics::solver::joint_constraint::joint_constraint_builder::JointConstraintBuilder; +use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::JointConstraint; +use crate::dynamics::solver::solver_body::SolverBodies; #[cfg(feature = "simd-is-enabled")] use { - crate::dynamics::solver::joint_constraint::joint_constraint_builder::{ - JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd, - }, - crate::math::SIMD_WIDTH, + crate::dynamics::solver::joint_constraint::joint_constraint_builder::JointConstraintBuilderSimd, + crate::math::{SIMD_WIDTH, SimdReal}, }; -pub type JointConstraintsSet = SolverConstraintsSet; +pub struct JointConstraintsSet { + pub generic_jacobians: DVector, + pub two_body_interactions: Vec, + pub generic_two_body_interactions: Vec, + pub interaction_groups: InteractionGroups, + + pub generic_velocity_constraints: Vec, + pub velocity_constraints: Vec>, + #[cfg(feature = "simd-is-enabled")] + pub simd_velocity_constraints: Vec>, + + pub generic_velocity_constraints_builder: Vec, + pub velocity_constraints_builder: Vec, + #[cfg(feature = "simd-is-enabled")] + pub simd_velocity_constraints_builder: Vec, +} + +impl JointConstraintsSet { + pub fn new() -> Self { + Self { + generic_jacobians: DVector::zeros(0), + two_body_interactions: vec![], + generic_two_body_interactions: vec![], + interaction_groups: InteractionGroups::new(), + velocity_constraints: vec![], + generic_velocity_constraints: vec![], + #[cfg(feature = "simd-is-enabled")] + simd_velocity_constraints: vec![], + velocity_constraints_builder: vec![], + generic_velocity_constraints_builder: vec![], + #[cfg(feature = "simd-is-enabled")] + simd_velocity_constraints_builder: vec![], + } + } + + pub fn clear_constraints(&mut self) { + self.generic_jacobians.fill(0.0); + self.generic_velocity_constraints.clear(); + #[cfg(feature = "simd-is-enabled")] + self.simd_velocity_constraints.clear(); + } + + pub fn clear_builders(&mut self) { + self.generic_velocity_constraints_builder.clear(); + #[cfg(feature = "simd-is-enabled")] + self.simd_velocity_constraints_builder.clear(); + } + + // Returns the generic jacobians and a mutable iterator through all the constraints. + pub fn iter_constraints_mut( + &mut self, + ) -> ( + &DVector, + impl Iterator>, + ) { + let jac = &self.generic_jacobians; + let a = self + .generic_velocity_constraints + .iter_mut() + .map(AnyJointConstraintMut::Generic); + let b = self + .velocity_constraints + .iter_mut() + .map(AnyJointConstraintMut::Rigid); + #[cfg(feature = "simd-is-enabled")] + let c = self + .simd_velocity_constraints + .iter_mut() + .map(AnyJointConstraintMut::SimdRigid); + #[cfg(not(feature = "simd-is-enabled"))] + return (jac, a.chain(b)); + #[cfg(feature = "simd-is-enabled")] + return (jac, a.chain(b).chain(c)); + } +} impl JointConstraintsSet { pub fn init( @@ -39,18 +109,13 @@ impl JointConstraintsSet { ) { // Generate constraints for impulse_joints. self.two_body_interactions.clear(); - self.one_body_interactions.clear(); self.generic_two_body_interactions.clear(); - self.generic_one_body_interactions.clear(); categorize_joints( - bodies, multibody_joints, impulse_joints, joint_constraint_indices, - &mut self.one_body_interactions, &mut self.two_body_interactions, - &mut self.generic_one_body_interactions, &mut self.generic_two_body_interactions, ); @@ -66,21 +131,10 @@ impl JointConstraintsSet { &self.two_body_interactions, ); - self.one_body_interaction_groups.clear_groups(); - self.one_body_interaction_groups.group_joints( - island_id, - islands, - bodies, - impulse_joints, - &self.one_body_interactions, - ); // NOTE: uncomment this do disable SIMD joint resolution. // self.interaction_groups // .nongrouped_interactions // .append(&mut self.interaction_groups.simd_interactions); - // self.one_body_interaction_groups - // .nongrouped_interactions - // .append(&mut self.one_body_interaction_groups.simd_interactions); let mut j_id = 0; self.compute_joint_constraints(bodies, impulse_joints); @@ -88,14 +142,7 @@ impl JointConstraintsSet { { self.simd_compute_joint_constraints(bodies, impulse_joints); } - self.compute_generic_joint_constraints(bodies, multibody_joints, impulse_joints, &mut j_id); - - self.compute_joint_one_body_constraints(bodies, impulse_joints); - #[cfg(feature = "simd-is-enabled")] - { - self.simd_compute_joint_one_body_constraints(bodies, impulse_joints); - } - self.compute_generic_one_body_joint_constraints( + self.compute_generic_joint_constraints( island_id, islands, bodies, @@ -105,87 +152,6 @@ impl JointConstraintsSet { ); } - fn compute_joint_one_body_constraints( - &mut self, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - ) { - let total_num_builders = self - .one_body_interaction_groups - .nongrouped_interactions - .len(); - - unsafe { - reset_buffer( - &mut self.velocity_one_body_constraints_builder, - total_num_builders, - ); - } - - let mut num_constraints = 0; - for (joint_i, builder) in self - .one_body_interaction_groups - .nongrouped_interactions - .iter() - .zip(self.velocity_one_body_constraints_builder.iter_mut()) - { - let joint = &joints_all[*joint_i].weight; - JointOneBodyConstraintBuilder::generate( - joint, - bodies, - *joint_i, - builder, - &mut num_constraints, - ); - } - - unsafe { - reset_buffer(&mut self.velocity_one_body_constraints, num_constraints); - } - } - - #[cfg(feature = "simd-is-enabled")] - fn simd_compute_joint_one_body_constraints( - &mut self, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - ) { - let total_num_builders = - self.one_body_interaction_groups.simd_interactions.len() / SIMD_WIDTH; - - unsafe { - reset_buffer( - &mut self.simd_velocity_one_body_constraints_builder, - total_num_builders, - ); - } - - let mut num_constraints = 0; - for (joints_i, builder) in self - .one_body_interaction_groups - .simd_interactions - .chunks_exact(SIMD_WIDTH) - .zip(self.simd_velocity_one_body_constraints_builder.iter_mut()) - { - let joints_id = gather![|ii| joints_i[ii]]; - let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight]; - JointOneBodyConstraintBuilderSimd::generate( - impulse_joints, - bodies, - joints_id, - builder, - &mut num_constraints, - ); - } - - unsafe { - reset_buffer( - &mut self.simd_velocity_one_body_constraints, - num_constraints, - ); - } - } - fn compute_joint_constraints(&mut self, bodies: &RigidBodySet, joints_all: &[JointGraphEdge]) { let total_num_builders = self.interaction_groups.nongrouped_interactions.len(); @@ -201,7 +167,7 @@ impl JointConstraintsSet { .zip(self.velocity_constraints_builder.iter_mut()) { let joint = &joints_all[*joint_i].weight; - JointTwoBodyConstraintBuilder::generate( + JointConstraintBuilder::generate( joint, bodies, *joint_i, @@ -216,42 +182,6 @@ impl JointConstraintsSet { } fn compute_generic_joint_constraints( - &mut self, - bodies: &RigidBodySet, - multibodies: &MultibodyJointSet, - joints_all: &[JointGraphEdge], - j_id: &mut usize, - ) { - let total_num_builders = self.generic_two_body_interactions.len(); - self.generic_velocity_constraints_builder.resize( - total_num_builders, - JointGenericTwoBodyConstraintBuilder::invalid(), - ); - - let mut num_constraints = 0; - for (joint_i, builder) in self - .generic_two_body_interactions - .iter() - .zip(self.generic_velocity_constraints_builder.iter_mut()) - { - let joint = &joints_all[*joint_i].weight; - JointGenericTwoBodyConstraintBuilder::generate( - *joint_i, - joint, - bodies, - multibodies, - builder, - j_id, - &mut self.generic_jacobians, - &mut num_constraints, - ); - } - - self.generic_velocity_constraints - .resize(num_constraints, JointGenericTwoBodyConstraint::invalid()); - } - - fn compute_generic_one_body_joint_constraints( &mut self, island_id: usize, islands: &IslandManager, @@ -260,32 +190,33 @@ impl JointConstraintsSet { joints_all: &[JointGraphEdge], j_id: &mut usize, ) { - let mut total_num_builders = self.generic_one_body_interactions.len(); - + // Count the internal and external constraints builder. + let num_external_constraint_builders = self.generic_two_body_interactions.len(); + let mut num_internal_constraint_builders = 0; for handle in islands.active_island(island_id) { if let Some(link_id) = multibodies.rigid_body_link(*handle) { - if JointGenericVelocityOneBodyInternalConstraintBuilder::num_constraints( - multibodies, - link_id, - ) > 0 + if JointGenericInternalConstraintBuilder::num_constraints(multibodies, link_id) > 0 { - total_num_builders += 1; + num_internal_constraint_builders += 1; } } } + let total_num_builders = + num_external_constraint_builders + num_internal_constraint_builders; - self.generic_velocity_one_body_constraints_builder.resize( - total_num_builders, - JointGenericOneBodyConstraintBuilder::invalid(), - ); + // Preallocate builders buffer. + self.generic_velocity_constraints_builder + .resize(total_num_builders, GenericJointConstraintBuilder::Empty); + // Generate external constraints builders. let mut num_constraints = 0; - for (joint_i, builder) in self.generic_one_body_interactions.iter().zip( - self.generic_velocity_one_body_constraints_builder - .iter_mut(), - ) { + for (joint_i, builder) in self + .generic_two_body_interactions + .iter() + .zip(self.generic_velocity_constraints_builder.iter_mut()) + { let joint = &joints_all[*joint_i].weight; - JointGenericVelocityOneBodyExternalConstraintBuilder::generate( + JointGenericExternalConstraintBuilder::generate( *joint_i, joint, bodies, @@ -297,18 +228,19 @@ impl JointConstraintsSet { ); } - let mut curr_builder = self.generic_one_body_interactions.len(); + // Generate internal constraints builder. They are indexed after the + let mut curr_builder = self.generic_two_body_interactions.len(); for handle in islands.active_island(island_id) { - if curr_builder >= self.generic_velocity_one_body_constraints_builder.len() { + if curr_builder >= self.generic_velocity_constraints_builder.len() { break; // No more builder need to be generated. } if let Some(link_id) = multibodies.rigid_body_link(*handle) { let prev_num_constraints = num_constraints; - JointGenericVelocityOneBodyInternalConstraintBuilder::generate( + JointGenericInternalConstraintBuilder::generate( multibodies, link_id, - &mut self.generic_velocity_one_body_constraints_builder[curr_builder], + &mut self.generic_velocity_constraints_builder[curr_builder], j_id, &mut self.generic_jacobians, &mut num_constraints, @@ -319,8 +251,9 @@ impl JointConstraintsSet { } } - self.generic_velocity_one_body_constraints - .resize(num_constraints, JointGenericOneBodyConstraint::invalid()); + // Resize constraints buffer now that we know the total count. + self.generic_velocity_constraints + .resize(num_constraints, GenericJointConstraint::invalid()); } #[cfg(feature = "simd-is-enabled")] @@ -345,9 +278,9 @@ impl JointConstraintsSet { .chunks_exact(SIMD_WIDTH) .zip(self.simd_velocity_constraints_builder.iter_mut()) { - let joints_id = gather![|ii| joints_i[ii]]; - let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight]; - JointTwoBodyConstraintBuilderSimd::generate( + let joints_id = array![|ii| joints_i[ii]]; + let impulse_joints = array![|ii| &joints_all[joints_i[ii]].weight]; + JointConstraintBuilderSimd::generate( impulse_joints, bodies, joints_id, @@ -364,7 +297,7 @@ impl JointConstraintsSet { #[profiling::function] pub fn solve( &mut self, - solver_vels: &mut [SolverVel], + solver_vels: &mut SolverBodies, generic_solver_vels: &mut DVector, ) { let (jac, constraints) = self.iter_constraints_mut(); @@ -375,7 +308,7 @@ impl JointConstraintsSet { pub fn solve_wo_bias( &mut self, - solver_vels: &mut [SolverVel], + solver_vels: &mut SolverBodies, generic_solver_vels: &mut DVector, ) { let (jac, constraints) = self.iter_constraints_mut(); @@ -397,26 +330,29 @@ impl JointConstraintsSet { &mut self, params: &IntegrationParameters, multibodies: &MultibodyJointSet, - solver_bodies: &[SolverBody], + solver_bodies: &SolverBodies, ) { for builder in &mut self.generic_velocity_constraints_builder { - builder.update( - params, - multibodies, - solver_bodies, - &mut self.generic_jacobians, - &mut self.generic_velocity_constraints, - ); - } - - for builder in &mut self.generic_velocity_one_body_constraints_builder { - builder.update( - params, - multibodies, - solver_bodies, - &mut self.generic_jacobians, - &mut self.generic_velocity_one_body_constraints, - ); + match builder { + GenericJointConstraintBuilder::External(builder) => { + builder.update( + params, + multibodies, + solver_bodies, + &mut self.generic_jacobians, + &mut self.generic_velocity_constraints, + ); + } + GenericJointConstraintBuilder::Internal(builder) => { + builder.update( + params, + multibodies, + &mut self.generic_jacobians, + &mut self.generic_velocity_constraints, + ); + } + GenericJointConstraintBuilder::Empty => {} + } } for builder in &mut self.velocity_constraints_builder { @@ -427,22 +363,5 @@ impl JointConstraintsSet { for builder in &mut self.simd_velocity_constraints_builder { builder.update(params, solver_bodies, &mut self.simd_velocity_constraints); } - - for builder in &mut self.velocity_one_body_constraints_builder { - builder.update( - params, - solver_bodies, - &mut self.velocity_one_body_constraints, - ); - } - - #[cfg(feature = "simd-is-enabled")] - for builder in &mut self.simd_velocity_one_body_constraints_builder { - builder.update( - params, - solver_bodies, - &mut self.simd_velocity_one_body_constraints, - ); - } } } diff --git a/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs deleted file mode 100644 index 41ed398..0000000 --- a/src/dynamics/solver/joint_constraint/joint_generic_constraint.rs +++ /dev/null @@ -1,552 +0,0 @@ -use crate::dynamics::solver::SolverVel; -use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ - JointFixedSolverBody, WritebackId, -}; -use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper}; -use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody}; -use crate::math::{DIM, Isometry, Real}; -use crate::prelude::SPATIAL_DIM; -use na::{DVector, DVectorView, DVectorViewMut}; - -#[derive(Debug, Copy, Clone)] -pub struct JointGenericTwoBodyConstraint { - pub is_rigid_body1: bool, - pub is_rigid_body2: bool, - pub solver_vel1: usize, - pub solver_vel2: usize, - - pub ndofs1: usize, - pub j_id1: usize, - pub ndofs2: usize, - pub j_id2: usize, - - pub joint_id: JointIndex, - - pub impulse: Real, - pub impulse_bounds: [Real; 2], - pub inv_lhs: Real, - pub rhs: Real, - pub rhs_wo_bias: Real, - pub cfm_coeff: Real, - pub cfm_gain: Real, - - pub writeback_id: WritebackId, -} - -impl Default for JointGenericTwoBodyConstraint { - fn default() -> Self { - JointGenericTwoBodyConstraint::invalid() - } -} - -impl JointGenericTwoBodyConstraint { - pub fn invalid() -> Self { - Self { - is_rigid_body1: false, - is_rigid_body2: false, - solver_vel1: usize::MAX, - solver_vel2: usize::MAX, - ndofs1: usize::MAX, - j_id1: usize::MAX, - ndofs2: usize::MAX, - j_id2: usize::MAX, - joint_id: usize::MAX, - impulse: 0.0, - impulse_bounds: [-Real::MAX, Real::MAX], - inv_lhs: Real::MAX, - rhs: Real::MAX, - rhs_wo_bias: Real::MAX, - cfm_coeff: Real::MAX, - cfm_gain: Real::MAX, - writeback_id: WritebackId::Dof(0), - } - } - - pub fn lock_axes( - params: &IntegrationParameters, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - frame1: &Isometry, - frame2: &Isometry, - joint: &GenericJoint, - jacobians: &mut DVector, - j_id: &mut usize, - out: &mut [Self], - ) -> usize { - let mut len = 0; - let locked_axes = joint.locked_axes.bits(); - let motor_axes = joint.motor_axes.bits(); - let limit_axes = joint.limit_axes.bits(); - - let builder = JointTwoBodyConstraintHelper::new( - frame1, - frame2, - &body1.world_com, - &body2.world_com, - locked_axes, - ); - - let start = len; - for i in DIM..SPATIAL_DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_angular_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - for i in 0..DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_linear_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - // locked_ang_axes, - i, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]); - - let start = len; - for i in DIM..SPATIAL_DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_generic( - params, - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - i - DIM, - WritebackId::Dof(i), - ); - len += 1; - } - } - for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_generic( - params, - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - i, - WritebackId::Dof(i), - ); - len += 1; - } - } - - for i in DIM..SPATIAL_DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_angular_generic( - params, - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - i - DIM, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } - } - for i in 0..DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_linear_generic( - params, - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - i, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } - } - - JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]); - len - } - - fn wj_id1(&self) -> usize { - self.j_id1 + self.ndofs1 - } - - fn wj_id2(&self) -> usize { - self.j_id2 + self.ndofs2 - } - - fn solver_vel1<'a>( - &self, - solver_vels: &'a [SolverVel], - generic_solver_vels: &'a DVector, - ) -> DVectorView<'a, Real> { - if self.is_rigid_body1 { - solver_vels[self.solver_vel1].as_vector_slice() - } else { - generic_solver_vels.rows(self.solver_vel1, self.ndofs1) - } - } - - fn solver_vel1_mut<'a>( - &self, - solver_vels: &'a mut [SolverVel], - generic_solver_vels: &'a mut DVector, - ) -> DVectorViewMut<'a, Real> { - if self.is_rigid_body1 { - solver_vels[self.solver_vel1].as_vector_slice_mut() - } else { - generic_solver_vels.rows_mut(self.solver_vel1, self.ndofs1) - } - } - - fn solver_vel2<'a>( - &self, - solver_vels: &'a [SolverVel], - generic_solver_vels: &'a DVector, - ) -> DVectorView<'a, Real> { - if self.is_rigid_body2 { - solver_vels[self.solver_vel2].as_vector_slice() - } else { - generic_solver_vels.rows(self.solver_vel2, self.ndofs2) - } - } - - fn solver_vel2_mut<'a>( - &self, - solver_vels: &'a mut [SolverVel], - generic_solver_vels: &'a mut DVector, - ) -> DVectorViewMut<'a, Real> { - if self.is_rigid_body2 { - solver_vels[self.solver_vel2].as_vector_slice_mut() - } else { - generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2) - } - } - - pub fn solve( - &mut self, - jacobians: &DVector, - solver_vels: &mut [SolverVel], - generic_solver_vels: &mut DVector, - ) { - let jacobians = jacobians.as_slice(); - - let solver_vel1 = self.solver_vel1(solver_vels, generic_solver_vels); - let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1); - let vel1 = j1.dot(&solver_vel1); - - let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels); - let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2); - let vel2 = j2.dot(&solver_vel2); - - let dvel = self.rhs + (vel2 - vel1); - let total_impulse = na::clamp( - self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse), - self.impulse_bounds[0], - self.impulse_bounds[1], - ); - let delta_impulse = total_impulse - self.impulse; - self.impulse = total_impulse; - - let mut solver_vel1 = self.solver_vel1_mut(solver_vels, generic_solver_vels); - let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1); - solver_vel1.axpy(delta_impulse, &wj1, 1.0); - - let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels); - let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); - solver_vel2.axpy(-delta_impulse, &wj2, 1.0); - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - let joint = &mut joints_all[self.joint_id].weight; - match self.writeback_id { - WritebackId::Dof(i) => joint.impulses[i] = self.impulse, - WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse, - WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse, - } - } - - pub fn remove_bias_from_rhs(&mut self) { - self.rhs = self.rhs_wo_bias; - } -} - -#[derive(Debug, Copy, Clone)] -pub struct JointGenericOneBodyConstraint { - pub solver_vel2: usize, - pub ndofs2: usize, - pub j_id2: usize, - - pub joint_id: JointIndex, - - pub impulse: Real, - pub impulse_bounds: [Real; 2], - pub inv_lhs: Real, - pub rhs: Real, - pub rhs_wo_bias: Real, - pub cfm_coeff: Real, - pub cfm_gain: Real, - - pub writeback_id: WritebackId, -} - -impl Default for JointGenericOneBodyConstraint { - fn default() -> Self { - JointGenericOneBodyConstraint::invalid() - } -} - -impl JointGenericOneBodyConstraint { - pub fn invalid() -> Self { - Self { - solver_vel2: crate::INVALID_USIZE, - ndofs2: 0, - j_id2: crate::INVALID_USIZE, - joint_id: 0, - impulse: 0.0, - impulse_bounds: [-Real::MAX, Real::MAX], - inv_lhs: 0.0, - rhs: 0.0, - rhs_wo_bias: 0.0, - cfm_coeff: 0.0, - cfm_gain: 0.0, - writeback_id: WritebackId::Dof(0), - } - } - - pub fn lock_axes( - params: &IntegrationParameters, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - mb2: (&Multibody, usize), - frame1: &Isometry, - frame2: &Isometry, - joint: &GenericJoint, - jacobians: &mut DVector, - j_id: &mut usize, - out: &mut [Self], - ) -> usize { - let mut len = 0; - let locked_axes = joint.locked_axes.bits(); - let motor_axes = joint.motor_axes.bits(); - let limit_axes = joint.limit_axes.bits(); - - let builder = JointTwoBodyConstraintHelper::new( - frame1, - frame2, - &body1.world_com, - &body2.world_com, - locked_axes, - ); - - let start = len; - for i in DIM..SPATIAL_DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_angular_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - - for i in 0..DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_linear_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - // locked_ang_axes, - i, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - - JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body( - jacobians, - &mut out[start..len], - ); - - let start = len; - for i in DIM..SPATIAL_DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_generic_one_body( - params, - jacobians, - j_id, - joint_id, - body1, - mb2, - i - DIM, - WritebackId::Dof(i), - ); - len += 1; - } - } - for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_generic_one_body( - params, - jacobians, - j_id, - joint_id, - body1, - mb2, - i, - WritebackId::Dof(i), - ); - len += 1; - } - } - - for i in DIM..SPATIAL_DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_angular_generic_one_body( - params, - jacobians, - j_id, - joint_id, - body1, - mb2, - i - DIM, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } - } - for i in 0..DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_linear_generic_one_body( - params, - jacobians, - j_id, - joint_id, - body1, - mb2, - i, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } - } - - JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body( - jacobians, - &mut out[start..len], - ); - len - } - - fn wj_id2(&self) -> usize { - self.j_id2 + self.ndofs2 - } - - fn solver_vel2<'a>( - &self, - _solver_vels: &'a [SolverVel], - generic_solver_vels: &'a DVector, - ) -> DVectorView<'a, Real> { - generic_solver_vels.rows(self.solver_vel2, self.ndofs2) - } - - fn solver_vel2_mut<'a>( - &self, - _solver_vels: &'a mut [SolverVel], - generic_solver_vels: &'a mut DVector, - ) -> DVectorViewMut<'a, Real> { - generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2) - } - - pub fn solve( - &mut self, - jacobians: &DVector, - solver_vels: &mut [SolverVel], - generic_solver_vels: &mut DVector, - ) { - let jacobians = jacobians.as_slice(); - - let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels); - let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2); - let vel2 = j2.dot(&solver_vel2); - - let dvel = self.rhs + vel2; - let total_impulse = na::clamp( - self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse), - self.impulse_bounds[0], - self.impulse_bounds[1], - ); - let delta_impulse = total_impulse - self.impulse; - self.impulse = total_impulse; - - let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels); - let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2); - solver_vel2.axpy(-delta_impulse, &wj2, 1.0); - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - // FIXME: impulse writeback isn’t supported yet for internal multibody_joint constraints. - if self.joint_id != usize::MAX { - let joint = &mut joints_all[self.joint_id].weight; - match self.writeback_id { - WritebackId::Dof(i) => joint.impulses[i] = self.impulse, - WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse, - WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse, - } - } - } - - pub fn remove_bias_from_rhs(&mut self) { - self.rhs = self.rhs_wo_bias; - } -} diff --git a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs deleted file mode 100644 index 8aaeef5..0000000 --- a/src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs +++ /dev/null @@ -1,1279 +0,0 @@ -use crate::dynamics::solver::MotorParameters; -use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{ - JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint, -}; -use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ - JointFixedSolverBody, WritebackId, -}; -use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper}; -use crate::dynamics::{ - GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet, - MultibodyLinkId, RigidBodySet, -}; -use crate::math::{ANG_DIM, DIM, Real, SPATIAL_DIM, Vector}; -use crate::utils; -use crate::utils::IndexMut2; -use crate::utils::SimdDot; -use na::{DVector, SVector}; - -use crate::dynamics::solver::ConstraintsCounts; -use crate::dynamics::solver::solver_body::SolverBody; -#[cfg(feature = "dim3")] -use crate::utils::SimdAngularInertia; -#[cfg(feature = "dim2")] -use na::Vector1; -use parry::math::Isometry; - -#[derive(Copy, Clone)] -enum LinkOrBody { - Link(MultibodyLinkId), - Body(usize), -} - -#[derive(Copy, Clone)] -pub struct JointGenericTwoBodyConstraintBuilder { - link1: LinkOrBody, - link2: LinkOrBody, - joint_id: JointIndex, - joint: GenericJoint, - j_id: usize, - // These are solver body for both joints, except that - // the world_com is actually in local-space. - local_body1: JointSolverBody, - local_body2: JointSolverBody, - multibodies_ndof: usize, - constraint_id: usize, -} - -impl JointGenericTwoBodyConstraintBuilder { - pub fn invalid() -> Self { - Self { - link1: LinkOrBody::Body(usize::MAX), - link2: LinkOrBody::Body(usize::MAX), - joint_id: JointIndex::MAX, - joint: GenericJoint::default(), - j_id: usize::MAX, - local_body1: JointSolverBody::invalid(), - local_body2: JointSolverBody::invalid(), - multibodies_ndof: usize::MAX, - constraint_id: usize::MAX, - } - } - - pub fn generate( - joint_id: JointIndex, - joint: &ImpulseJoint, - bodies: &RigidBodySet, - multibodies: &MultibodyJointSet, - out_builder: &mut Self, - j_id: &mut usize, - jacobians: &mut DVector, - out_constraint_id: &mut usize, - ) { - let starting_j_id = *j_id; - let rb1 = &bodies[joint.body1]; - let rb2 = &bodies[joint.body2]; - - let local_body1 = JointSolverBody { - im: rb1.mprops.effective_inv_mass, - sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt, - world_com: rb1.mprops.local_mprops.local_com, - solver_vel: [rb1.ids.active_set_offset], - }; - let local_body2 = JointSolverBody { - im: rb2.mprops.effective_inv_mass, - sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, - world_com: rb2.mprops.local_mprops.local_com, - solver_vel: [rb2.ids.active_set_offset], - }; - - let mut multibodies_ndof = 0; - let link1 = match multibodies.rigid_body_link(joint.body1) { - Some(link) => { - multibodies_ndof += multibodies[link.multibody].ndofs(); - LinkOrBody::Link(*link) - } - None => { - multibodies_ndof += SPATIAL_DIM; - LinkOrBody::Body(rb2.ids.active_set_offset) - } - }; - let link2 = match multibodies.rigid_body_link(joint.body2) { - Some(link) => { - multibodies_ndof += multibodies[link.multibody].ndofs(); - LinkOrBody::Link(*link) - } - None => { - multibodies_ndof += SPATIAL_DIM; - LinkOrBody::Body(rb2.ids.active_set_offset) - } - }; - - if multibodies_ndof == 0 { - // Both multibodies are fixed, don’t generate any constraint. - out_builder.multibodies_ndof = multibodies_ndof; - return; - } - - // For each solver contact we generate up to SPATIAL_DIM constraints, and each - // constraints appends the multibodies jacobian and weighted jacobians. - // Also note that for impulse_joints, the rigid-bodies will also add their jacobians - // to the generic DVector. - // TODO: is this count correct when we take both motors and limits into account? - let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; - - // TODO: use a more precise increment. - *j_id += multibodies_ndof * 2 * SPATIAL_DIM; - - if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - - *out_builder = Self { - link1, - link2, - joint_id, - joint: joint.data, - j_id: starting_j_id, - local_body1, - local_body2, - multibodies_ndof, - constraint_id: *out_constraint_id, - }; - - *out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints; - } - - pub fn update( - &self, - params: &IntegrationParameters, - multibodies: &MultibodyJointSet, - bodies: &[SolverBody], - jacobians: &mut DVector, - out: &mut [JointGenericTwoBodyConstraint], - ) { - if self.multibodies_ndof == 0 { - // The joint is between two static bodies, no constraint needed. - return; - } - - // NOTE: right now, the "update", is basically reconstructing all the - // constraints. Could we make this more incremental? - let pos1; - let pos2; - let mb1; - let mb2; - - match self.link1 { - LinkOrBody::Link(link) => { - let mb = &multibodies[link.multibody]; - pos1 = &mb.link(link.id).unwrap().local_to_world; - mb1 = Some((mb, link.id)); - } - LinkOrBody::Body(body1) => { - pos1 = &bodies[body1].position; - mb1 = None; - } - }; - match self.link2 { - LinkOrBody::Link(link) => { - let mb = &multibodies[link.multibody]; - pos2 = &mb.link(link.id).unwrap().local_to_world; - mb2 = Some((mb, link.id)); - } - LinkOrBody::Body(body2) => { - pos2 = &bodies[body2].position; - mb2 = None; - } - }; - - let frame1 = pos1 * self.joint.local_frame1; - let frame2 = pos2 * self.joint.local_frame2; - - let joint_body1 = JointSolverBody { - world_com: pos1 * self.local_body1.world_com, // the world_com was stored in local-space. - ..self.local_body1 - }; - let joint_body2 = JointSolverBody { - world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space. - ..self.local_body2 - }; - - let mut j_id = self.j_id; - - JointGenericTwoBodyConstraint::lock_axes( - params, - self.joint_id, - &joint_body1, - &joint_body2, - mb1, - mb2, - &frame1, - &frame2, - &self.joint, - jacobians, - &mut j_id, - &mut out[self.constraint_id..], - ); - } -} - -#[derive(Copy, Clone)] -#[allow(clippy::large_enum_variant)] -pub enum JointGenericOneBodyConstraintBuilder { - Internal(JointGenericVelocityOneBodyInternalConstraintBuilder), - External(JointGenericVelocityOneBodyExternalConstraintBuilder), - Empty, -} - -#[derive(Copy, Clone)] -pub struct JointGenericVelocityOneBodyInternalConstraintBuilder { - link: MultibodyLinkId, - j_id: usize, - constraint_id: usize, -} - -impl JointGenericOneBodyConstraintBuilder { - pub fn invalid() -> Self { - Self::Empty - } - - pub fn update( - &self, - params: &IntegrationParameters, - multibodies: &MultibodyJointSet, - bodies: &[SolverBody], - jacobians: &mut DVector, - out: &mut [JointGenericOneBodyConstraint], - ) { - match self { - Self::Empty => {} - Self::Internal(builder) => builder.update(params, multibodies, jacobians, out), - Self::External(builder) => builder.update(params, multibodies, bodies, jacobians, out), - } - } -} - -impl JointGenericVelocityOneBodyInternalConstraintBuilder { - pub fn num_constraints(multibodies: &MultibodyJointSet, link_id: &MultibodyLinkId) -> usize { - let multibody = &multibodies[link_id.multibody]; - let link = multibody.link(link_id.id).unwrap(); - link.joint().num_velocity_constraints() - } - - pub fn generate( - multibodies: &MultibodyJointSet, - link_id: &MultibodyLinkId, - out_builder: &mut JointGenericOneBodyConstraintBuilder, - j_id: &mut usize, - jacobians: &mut DVector, - out_constraint_id: &mut usize, - ) { - let multibody = &multibodies[link_id.multibody]; - let link = multibody.link(link_id.id).unwrap(); - let num_constraints = link.joint().num_velocity_constraints(); - - if num_constraints == 0 { - return; - } - - *out_builder = JointGenericOneBodyConstraintBuilder::Internal(Self { - link: *link_id, - j_id: *j_id, - constraint_id: *out_constraint_id, - }); - - *j_id += num_constraints * multibody.ndofs() * 2; - if jacobians.nrows() < *j_id { - jacobians.resize_vertically_mut(*j_id, 0.0); - } - - *out_constraint_id += num_constraints; - } - - pub fn update( - &self, - params: &IntegrationParameters, - multibodies: &MultibodyJointSet, - jacobians: &mut DVector, - out: &mut [JointGenericOneBodyConstraint], - ) { - let mb = &multibodies[self.link.multibody]; - let link = mb.link(self.link.id).unwrap(); - link.joint().velocity_constraints( - params, - mb, - link, - self.j_id, - jacobians, - &mut out[self.constraint_id..], - ); - } -} - -#[derive(Copy, Clone)] -pub struct JointGenericVelocityOneBodyExternalConstraintBuilder { - body1: JointFixedSolverBody, - frame1: Isometry, - link2: MultibodyLinkId, - joint_id: JointIndex, - joint: GenericJoint, - j_id: usize, - constraint_id: usize, - // These are solver body for both joints, except that - // the world_com is actually in local-space. - local_body2: JointSolverBody, -} - -impl JointGenericVelocityOneBodyExternalConstraintBuilder { - pub fn generate( - joint_id: JointIndex, - joint: &ImpulseJoint, - bodies: &RigidBodySet, - multibodies: &MultibodyJointSet, - out_builder: &mut JointGenericOneBodyConstraintBuilder, - j_id: &mut usize, - jacobians: &mut DVector, - out_constraint_id: &mut usize, - ) { - let mut joint_data = joint.data; - let mut handle1 = joint.body1; - let mut handle2 = joint.body2; - let flipped = !bodies[handle2].is_dynamic(); - - if flipped { - std::mem::swap(&mut handle1, &mut handle2); - joint_data.flip(); - } - - let rb1 = &bodies[handle1]; - let rb2 = &bodies[handle2]; - - let frame1 = rb1.pos.position * joint_data.local_frame1; - - let starting_j_id = *j_id; - - let body1 = JointFixedSolverBody { - linvel: rb1.vels.linvel, - angvel: rb1.vels.angvel, - world_com: rb1.mprops.world_com, - }; - let local_body2 = JointSolverBody { - im: rb2.mprops.effective_inv_mass, - sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt, - world_com: rb2.mprops.local_mprops.local_com, - solver_vel: [rb2.ids.active_set_offset], - }; - - let link2 = *multibodies.rigid_body_link(handle2).unwrap(); - let mb2 = &multibodies[link2.multibody]; - let multibodies_ndof = mb2.ndofs(); - - if multibodies_ndof == 0 { - // The multibody is fixed, don’t generate any constraint. - *out_builder = JointGenericOneBodyConstraintBuilder::Empty; - return; - } - - // For each solver contact we generate up to SPATIAL_DIM constraints, and each - // constraints appends the multibodies jacobian and weighted jacobians. - // Also note that for impulse_joints, the rigid-bodies will also add their jacobians - // to the generic DVector. - // TODO: is this count correct when we take both motors and limits into account? - let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM; - // TODO: use a more precise increment. - *j_id += multibodies_ndof * 2 * SPATIAL_DIM; - - if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") { - jacobians.resize_vertically_mut(required_jacobian_len, 0.0); - } - - *out_builder = JointGenericOneBodyConstraintBuilder::External(Self { - body1, - link2, - joint_id, - joint: joint_data, - j_id: starting_j_id, - frame1, - local_body2, - constraint_id: *out_constraint_id, - }); - - *out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints; - } - - pub fn update( - &self, - params: &IntegrationParameters, - multibodies: &MultibodyJointSet, - _bodies: &[SolverBody], - jacobians: &mut DVector, - out: &mut [JointGenericOneBodyConstraint], - ) { - // NOTE: right now, the "update", is basically reconstructing all the - // constraints. Could we make this more incremental? - let mb2 = &multibodies[self.link2.multibody]; - let pos2 = &mb2.link(self.link2.id).unwrap().local_to_world; - let frame2 = pos2 * self.joint.local_frame2; - - let joint_body2 = JointSolverBody { - world_com: pos2 * self.local_body2.world_com, // the world_com was stored in local-space. - ..self.local_body2 - }; - - let mut j_id = self.j_id; - - JointGenericOneBodyConstraint::lock_axes( - params, - self.joint_id, - &self.body1, - &joint_body2, - (mb2, self.link2.id), - &self.frame1, - &frame2, - &self.joint, - jacobians, - &mut j_id, - &mut out[self.constraint_id..], - ); - } -} - -impl JointSolverBody { - pub fn fill_jacobians( - &self, - unit_force: Vector, - unit_torque: SVector, - j_id: &mut usize, - jacobians: &mut DVector, - ) { - let wj_id = *j_id + SPATIAL_DIM; - jacobians - .fixed_rows_mut::(*j_id) - .copy_from(&unit_force); - jacobians - .fixed_rows_mut::(*j_id + DIM) - .copy_from(&unit_torque); - - { - let mut out_invm_j = jacobians.fixed_rows_mut::(wj_id); - out_invm_j - .fixed_rows_mut::(0) - .copy_from(&self.im.component_mul(&unit_force)); - - #[cfg(feature = "dim2")] - { - out_invm_j[DIM] *= self.sqrt_ii; - } - #[cfg(feature = "dim3")] - { - out_invm_j.fixed_rows_mut::(DIM).gemv( - 1.0, - &self.sqrt_ii.into_matrix(), - &unit_torque, - 0.0, - ); - } - } - - *j_id += SPATIAL_DIM * 2; - } -} - -impl JointTwoBodyConstraintHelper { - pub fn lock_jacobians_generic( - &self, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - writeback_id: WritebackId, - lin_jac: Vector, - ang_jac1: SVector, - ang_jac2: SVector, - ) -> JointGenericTwoBodyConstraint { - let is_rigid_body1 = mb1.is_none(); - let is_rigid_body2 = mb2.is_none(); - - let ndofs1 = mb1.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM); - let ndofs2 = mb2.map(|(m, _)| m.ndofs()).unwrap_or(SPATIAL_DIM); - - let j_id1 = *j_id; - if let Some((mb1, link_id1)) = mb1 { - mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians); - } else { - body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians); - }; - - let j_id2 = *j_id; - if let Some((mb2, link_id2)) = mb2 { - mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians); - } else { - body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians); - }; - - if is_rigid_body1 { - let ang_j_id1 = j_id1 + DIM; - let ang_wj_id1 = j_id1 + DIM + ndofs1; - let (mut j, wj) = jacobians.rows_range_pair_mut( - ang_j_id1..ang_j_id1 + ANG_DIM, - ang_wj_id1..ang_wj_id1 + ANG_DIM, - ); - j.copy_from(&wj); - } - - if is_rigid_body2 { - let ang_j_id2 = j_id2 + DIM; - let ang_wj_id2 = j_id2 + DIM + ndofs2; - let (mut j, wj) = jacobians.rows_range_pair_mut( - ang_j_id2..ang_j_id2 + ANG_DIM, - ang_wj_id2..ang_wj_id2 + ANG_DIM, - ); - j.copy_from(&wj); - } - - let rhs_wo_bias = 0.0; - - let solver_vel1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.solver_vel[0]); - let solver_vel2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.solver_vel[0]); - - JointGenericTwoBodyConstraint { - is_rigid_body1, - is_rigid_body2, - solver_vel1, - solver_vel2, - ndofs1, - j_id1, - ndofs2, - j_id2, - joint_id, - impulse: 0.0, - impulse_bounds: [-Real::MAX, Real::MAX], - inv_lhs: 0.0, - rhs: rhs_wo_bias, - rhs_wo_bias, - cfm_coeff: 0.0, - cfm_gain: 0.0, - writeback_id, - } - } - - pub fn lock_linear_generic( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - locked_axis: usize, - writeback_id: WritebackId, - ) -> JointGenericTwoBodyConstraint { - let lin_jac = self.basis.column(locked_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); - let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); - - let mut c = self.lock_jacobians_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - writeback_id, - lin_jac, - ang_jac1, - ang_jac2, - ); - - let erp_inv_dt = params.joint_erp_inv_dt(); - let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; - c.rhs += rhs_bias; - c - } - - pub fn limit_linear_generic( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - limited_axis: usize, - limits: [Real; 2], - writeback_id: WritebackId, - ) -> JointGenericTwoBodyConstraint { - let lin_jac = self.basis.column(limited_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); - let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - writeback_id, - lin_jac, - ang_jac1, - ang_jac2, - ); - - let dist = self.lin_err.dot(&lin_jac); - let min_enabled = dist <= limits[0]; - let max_enabled = limits[1] <= dist; - - let erp_inv_dt = params.joint_erp_inv_dt(); - let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt; - constraint.rhs += rhs_bias; - constraint.impulse_bounds = [ - min_enabled as u32 as Real * -Real::MAX, - max_enabled as u32 as Real * Real::MAX, - ]; - - constraint - } - - pub fn motor_linear_generic( - &self, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - motor_axis: usize, - motor_params: &MotorParameters, - writeback_id: WritebackId, - ) -> JointGenericTwoBodyConstraint { - let lin_jac = self.basis.column(motor_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); - let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); - - // TODO: do we need the same trick as for the non-generic constraint? - // if locked_ang_axes & (1 << motor_axis) != 0 { - // // FIXME: check that this also works for cases - // // whene not all the angular axes are locked. - // constraint.ang_jac1.fill(0.0); - // constraint.ang_jac2.fill(0.0); - // } - - let mut constraint = self.lock_jacobians_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - writeback_id, - lin_jac, - ang_jac1, - ang_jac2, - ); - - let mut rhs_wo_bias = 0.0; - if motor_params.erp_inv_dt != 0.0 { - let dist = self.lin_err.dot(&lin_jac); - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; - } - - rhs_wo_bias += -motor_params.target_vel; - - constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; - constraint.rhs = rhs_wo_bias; - constraint.rhs_wo_bias = rhs_wo_bias; - constraint.cfm_coeff = motor_params.cfm_coeff; - constraint.cfm_gain = motor_params.cfm_gain; - constraint - } - - pub fn lock_angular_generic( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - _locked_axis: usize, - writeback_id: WritebackId, - ) -> JointGenericTwoBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = Vector1::new(1.0); - #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - writeback_id, - na::zero(), - ang_jac, - ang_jac, - ); - - let erp_inv_dt = params.joint_erp_inv_dt(); - #[cfg(feature = "dim2")] - let rhs_bias = self.ang_err.im * erp_inv_dt; - #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; - constraint.rhs += rhs_bias; - constraint - } - - pub fn limit_angular_generic( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - _limited_axis: usize, - limits: [Real; 2], - writeback_id: WritebackId, - ) -> JointGenericTwoBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = Vector1::new(1.0); - #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - writeback_id, - na::zero(), - ang_jac, - ang_jac, - ); - - let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; - #[cfg(feature = "dim2")] - let s_ang = (self.ang_err.angle() / 2.0).sin(); - #[cfg(feature = "dim3")] - let s_ang = self.ang_err.imag()[_limited_axis]; - let min_enabled = s_ang <= s_limits[0]; - let max_enabled = s_limits[1] <= s_ang; - let impulse_bounds = [ - min_enabled as u32 as Real * -Real::MAX, - max_enabled as u32 as Real * Real::MAX, - ]; - - let erp_inv_dt = params.joint_erp_inv_dt(); - let rhs_bias = - ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; - - constraint.rhs += rhs_bias; - constraint.impulse_bounds = impulse_bounds; - constraint - } - - pub fn motor_angular_generic( - &self, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointSolverBody, - body2: &JointSolverBody, - mb1: Option<(&Multibody, usize)>, - mb2: Option<(&Multibody, usize)>, - _motor_axis: usize, - motor_params: &MotorParameters, - writeback_id: WritebackId, - ) -> JointGenericTwoBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = na::Vector1::new(1.0); - #[cfg(feature = "dim3")] - let ang_jac = self.basis.column(_motor_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic( - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - writeback_id, - na::zero(), - ang_jac, - ang_jac, - ); - - let mut rhs_wo_bias = 0.0; - if motor_params.erp_inv_dt != 0.0 { - #[cfg(feature = "dim2")] - let s_ang_dist = (self.ang_err.angle() / 2.0).sin(); - #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = (motor_params.target_pos / 2.0).sin(); - rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) - * motor_params.erp_inv_dt; - } - - rhs_wo_bias += -motor_params.target_vel; - - constraint.rhs_wo_bias = rhs_wo_bias; - constraint.rhs = rhs_wo_bias; - constraint.cfm_coeff = motor_params.cfm_coeff; - constraint.cfm_gain = motor_params.cfm_gain; - constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; - constraint - } - - pub fn finalize_generic_constraints( - jacobians: &mut DVector, - constraints: &mut [JointGenericTwoBodyConstraint], - ) { - // TODO: orthogonalization doesn’t seem to give good results for multibodies? - const ORTHOGONALIZE: bool = false; - let len = constraints.len(); - - if len == 0 { - return; - } - - let ndofs1 = constraints[0].ndofs1; - let ndofs2 = constraints[0].ndofs2; - - // Use the modified Gramm-Schmidt orthogonalization. - for j in 0..len { - let c_j = &mut constraints[j]; - - let jac_j1 = jacobians.rows(c_j.j_id1, ndofs1); - let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2); - let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1); - let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); - - let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2); - let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; - let inv_dot_jj = crate::utils::simd_inv(dot_jj); - c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. - c_j.cfm_gain = cfm_gain; - - if c_j.impulse_bounds != [-Real::MAX, Real::MAX] { - // Don't remove constraints with limited forces from the others - // because they may not deliver the necessary forces to fulfill - // the removed parts of other constraints. - continue; - } - - if !ORTHOGONALIZE { - continue; - } - - for i in (j + 1)..len { - let (c_i, c_j) = constraints.index_mut_const(i, j); - - let jac_i1 = jacobians.rows(c_i.j_id1, ndofs1); - let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2); - let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1); - let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); - - let dot_ij = jac_i1.dot(&w_jac_j1) + jac_i2.dot(&w_jac_j2); - let coeff = dot_ij * inv_dot_jj; - - let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut( - c_i.j_id1..c_i.j_id1 + 2 * (ndofs1 + ndofs2), - c_j.j_id1..c_j.j_id1 + 2 * (ndofs1 + ndofs2), - ); - - jac_i.axpy(-coeff, &jac_j, 1.0); - - c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff; - c_i.rhs -= c_j.rhs * coeff; - } - } - } -} - -impl JointTwoBodyConstraintHelper { - pub fn lock_jacobians_generic_one_body( - &self, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - (mb2, link_id2): (&Multibody, usize), - writeback_id: WritebackId, - lin_jac: Vector, - ang_jac1: SVector, - ang_jac2: SVector, - ) -> JointGenericOneBodyConstraint { - let ndofs2 = mb2.ndofs(); - - let proj_vel1 = lin_jac.dot(&body1.linvel) + ang_jac1.gdot(body1.angvel); - let j_id2 = *j_id; - mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians); - let rhs_wo_bias = -proj_vel1; - - let solver_vel2 = mb2.solver_id; - - JointGenericOneBodyConstraint { - solver_vel2, - ndofs2, - j_id2, - joint_id, - impulse: 0.0, - impulse_bounds: [-Real::MAX, Real::MAX], - inv_lhs: 0.0, - rhs: rhs_wo_bias, - rhs_wo_bias, - cfm_coeff: 0.0, - cfm_gain: 0.0, - writeback_id, - } - } - - pub fn lock_linear_generic_one_body( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - mb2: (&Multibody, usize), - locked_axis: usize, - writeback_id: WritebackId, - ) -> JointGenericOneBodyConstraint { - let lin_jac = self.basis.column(locked_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned(); - let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned(); - - let mut c = self.lock_jacobians_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - writeback_id, - lin_jac, - ang_jac1, - ang_jac2, - ); - - let erp_inv_dt = params.joint_erp_inv_dt(); - let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; - c.rhs += rhs_bias; - c - } - - pub fn limit_linear_generic_one_body( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - mb2: (&Multibody, usize), - limited_axis: usize, - limits: [Real; 2], - writeback_id: WritebackId, - ) -> JointGenericOneBodyConstraint { - let lin_jac = self.basis.column(limited_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned(); - let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - writeback_id, - lin_jac, - ang_jac1, - ang_jac2, - ); - - let dist = self.lin_err.dot(&lin_jac); - let min_enabled = dist <= limits[0]; - let max_enabled = limits[1] <= dist; - - let erp_inv_dt = params.joint_erp_inv_dt(); - let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt; - constraint.rhs += rhs_bias; - constraint.impulse_bounds = [ - min_enabled as u32 as Real * -Real::MAX, - max_enabled as u32 as Real * Real::MAX, - ]; - - constraint - } - - pub fn motor_linear_generic_one_body( - &self, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - mb2: (&Multibody, usize), - motor_axis: usize, - motor_params: &MotorParameters, - writeback_id: WritebackId, - ) -> JointGenericOneBodyConstraint { - let lin_jac = self.basis.column(motor_axis).into_owned(); - let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); - let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); - - // TODO: do we need the same trick as for the non-generic constraint? - // if locked_ang_axes & (1 << motor_axis) != 0 { - // // FIXME: check that this also works for cases - // // whene not all the angular axes are locked. - // constraint.ang_jac1.fill(0.0); - // constraint.ang_jac2.fill(0.0); - // } - - let mut constraint = self.lock_jacobians_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - writeback_id, - lin_jac, - ang_jac1, - ang_jac2, - ); - - let mut rhs_wo_bias = 0.0; - if motor_params.erp_inv_dt != 0.0 { - let dist = self.lin_err.dot(&lin_jac); - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; - } - - let proj_vel1 = -lin_jac.dot(&body1.linvel) - ang_jac1.gdot(body1.angvel); - rhs_wo_bias += proj_vel1 - motor_params.target_vel; - - constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; - constraint.rhs = rhs_wo_bias; - constraint.rhs_wo_bias = rhs_wo_bias; - constraint.cfm_coeff = motor_params.cfm_coeff; - constraint.cfm_gain = motor_params.cfm_gain; - constraint - } - - pub fn lock_angular_generic_one_body( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - mb2: (&Multibody, usize), - _locked_axis: usize, - writeback_id: WritebackId, - ) -> JointGenericOneBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = Vector1::new(1.0); - #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_locked_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - writeback_id, - na::zero(), - ang_jac, - ang_jac, - ); - - let erp_inv_dt = params.joint_erp_inv_dt(); - #[cfg(feature = "dim2")] - let rhs_bias = self.ang_err.im * erp_inv_dt; - #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt; - constraint.rhs += rhs_bias; - constraint - } - - pub fn limit_angular_generic_one_body( - &self, - params: &IntegrationParameters, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - mb2: (&Multibody, usize), - _limited_axis: usize, - limits: [Real; 2], - writeback_id: WritebackId, - ) -> JointGenericOneBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = Vector1::new(1.0); - #[cfg(feature = "dim3")] - let ang_jac = self.ang_basis.column(_limited_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - writeback_id, - na::zero(), - ang_jac, - ang_jac, - ); - - let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()]; - #[cfg(feature = "dim2")] - let s_ang = (self.ang_err.angle() / 2.0).sin(); - #[cfg(feature = "dim3")] - let s_ang = self.ang_err.imag()[_limited_axis]; - let min_enabled = s_ang <= s_limits[0]; - let max_enabled = s_limits[1] <= s_ang; - let impulse_bounds = [ - min_enabled as u32 as Real * -Real::MAX, - max_enabled as u32 as Real * Real::MAX, - ]; - - let erp_inv_dt = params.joint_erp_inv_dt(); - let rhs_bias = - ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; - - constraint.rhs += rhs_bias; - constraint.impulse_bounds = impulse_bounds; - constraint - } - - pub fn motor_angular_generic_one_body( - &self, - jacobians: &mut DVector, - j_id: &mut usize, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - mb2: (&Multibody, usize), - _motor_axis: usize, - motor_params: &MotorParameters, - writeback_id: WritebackId, - ) -> JointGenericOneBodyConstraint { - #[cfg(feature = "dim2")] - let ang_jac = na::Vector1::new(1.0); - #[cfg(feature = "dim3")] - let ang_jac = self.basis.column(_motor_axis).into_owned(); - - let mut constraint = self.lock_jacobians_generic_one_body( - jacobians, - j_id, - joint_id, - body1, - mb2, - writeback_id, - na::zero(), - ang_jac, - ang_jac, - ); - - let mut rhs = 0.0; - if motor_params.erp_inv_dt != 0.0 { - #[cfg(feature = "dim2")] - let s_ang_dist = (self.ang_err.angle() / 2.0).sin(); - #[cfg(feature = "dim3")] - let s_ang_dist = self.ang_err.imag()[_motor_axis]; - let s_target_ang = (motor_params.target_pos / 2.0).sin(); - rhs += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang) - * motor_params.erp_inv_dt; - } - - let proj_vel1 = -ang_jac.gdot(body1.angvel); - rhs += proj_vel1 - motor_params.target_vel; - - constraint.rhs_wo_bias = rhs; - constraint.rhs = rhs; - constraint.cfm_coeff = motor_params.cfm_coeff; - constraint.cfm_gain = motor_params.cfm_gain; - constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; - constraint - } - - pub fn finalize_generic_constraints_one_body( - jacobians: &mut DVector, - constraints: &mut [JointGenericOneBodyConstraint], - ) { - // TODO: orthogonalization doesn’t seem to give good results for multibodies? - const ORTHOGONALIZE: bool = false; - let len = constraints.len(); - - if len == 0 { - return; - } - - let ndofs2 = constraints[0].ndofs2; - - // Use the modified Gramm-Schmidt orthogonalization. - for j in 0..len { - let c_j = &mut constraints[j]; - - let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2); - let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); - - let dot_jj = jac_j2.dot(&w_jac_j2); - let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; - let inv_dot_jj = crate::utils::simd_inv(dot_jj); - c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. - c_j.cfm_gain = cfm_gain; - - if c_j.impulse_bounds != [-Real::MAX, Real::MAX] { - // Don't remove constraints with limited forces from the others - // because they may not deliver the necessary forces to fulfill - // the removed parts of other constraints. - continue; - } - - if !ORTHOGONALIZE { - continue; - } - - for i in (j + 1)..len { - let (c_i, c_j) = constraints.index_mut_const(i, j); - - let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2); - let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); - - let dot_ij = jac_i2.dot(&w_jac_j2); - let coeff = dot_ij * inv_dot_jj; - - let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut( - c_i.j_id2..c_i.j_id2 + 2 * ndofs2, - c_j.j_id2..c_j.j_id2 + 2 * ndofs2, - ); - - jac_i.axpy(-coeff, &jac_j, 1.0); - - c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff; - c_i.rhs -= c_j.rhs * coeff; - } - } - } -} diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index 6245ec6..e33c131 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -1,17 +1,16 @@ use crate::dynamics::solver::SolverVel; -use crate::dynamics::solver::joint_constraint::JointTwoBodyConstraintHelper; +use crate::dynamics::solver::joint_constraint::JointConstraintHelper; use crate::dynamics::{ GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, }; use crate::math::{AngVector, AngularInertia, DIM, Isometry, Point, Real, SPATIAL_DIM, Vector}; -use crate::num::Zero; use crate::utils::{SimdDot, SimdRealCopy}; +use crate::dynamics::solver::solver_body::SolverBodies; #[cfg(feature = "simd-is-enabled")] -use { - crate::math::{SIMD_WIDTH, SimdReal}, - na::SimdValue, -}; +use crate::math::{SIMD_WIDTH, SimdReal}; +#[cfg(feature = "dim2")] +use crate::num::Zero; #[derive(Copy, Clone, PartialEq, Debug)] pub struct MotorParameters { @@ -50,44 +49,26 @@ pub enum WritebackId { #[derive(Copy, Clone)] pub struct JointSolverBody { pub im: Vector, - pub sqrt_ii: AngularInertia, - pub world_com: Point, - pub solver_vel: [usize; LANES], + pub ii: AngularInertia, + pub world_com: Point, // TODO: is this still needed now that the solver body poses are expressed at the center of mass? + pub solver_vel: [u32; LANES], } impl JointSolverBody { pub fn invalid() -> Self { Self { im: Vector::zeros(), - sqrt_ii: AngularInertia::zero(), - world_com: Point::origin(), - solver_vel: [usize::MAX; LANES], - } - } -} - -#[derive(Copy, Clone)] -pub struct JointFixedSolverBody { - pub linvel: Vector, - pub angvel: AngVector, - // TODO: is this really needed? - pub world_com: Point, -} - -impl JointFixedSolverBody { - pub fn invalid() -> Self { - Self { - linvel: Vector::zeros(), - angvel: AngVector::zero(), + ii: AngularInertia::zero(), world_com: Point::origin(), + solver_vel: [u32::MAX; LANES], } } } #[derive(Debug, Copy, Clone)] -pub struct JointTwoBodyConstraint { - pub solver_vel1: [usize; LANES], - pub solver_vel2: [usize; LANES], +pub struct JointConstraint { + pub solver_vel1: [u32; LANES], + pub solver_vel2: [u32; LANES], pub joint_id: [JointIndex; LANES], @@ -97,6 +78,9 @@ pub struct JointTwoBodyConstraint { pub ang_jac1: AngVector, pub ang_jac2: AngVector, + pub ii_ang_jac1: AngVector, + pub ii_ang_jac2: AngVector, + pub inv_lhs: N, pub rhs: N, pub rhs_wo_bias: N, @@ -109,7 +93,7 @@ pub struct JointTwoBodyConstraint { pub writeback_id: WritebackId, } -impl JointTwoBodyConstraint { +impl JointConstraint { #[profiling::function] pub fn solve_generic( &mut self, @@ -127,13 +111,13 @@ impl JointTwoBodyConstraint { self.impulse = total_impulse; let lin_impulse = self.lin_jac * delta_impulse; - let ang_impulse1 = self.ang_jac1 * delta_impulse; - let ang_impulse2 = self.ang_jac2 * delta_impulse; + let ii_ang_impulse1 = self.ii_ang_jac1 * delta_impulse; + let ii_ang_impulse2 = self.ii_ang_jac2 * delta_impulse; solver_vel1.linear += lin_impulse.component_mul(&self.im1); - solver_vel1.angular += ang_impulse1; + solver_vel1.angular += ii_ang_impulse1; solver_vel2.linear -= lin_impulse.component_mul(&self.im2); - solver_vel2.angular -= ang_impulse2; + solver_vel2.angular -= ii_ang_impulse2; } pub fn remove_bias_from_rhs(&mut self) { @@ -141,8 +125,8 @@ impl JointTwoBodyConstraint { } } -impl JointTwoBodyConstraint { - pub fn lock_axes( +impl JointConstraint { + pub fn update( params: &IntegrationParameters, joint_id: JointIndex, body1: &JointSolverBody, @@ -169,7 +153,7 @@ impl JointTwoBodyConstraint { let first_coupled_ang_axis_id = (coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize; - let builder = JointTwoBodyConstraintHelper::new( + let builder = JointConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -240,7 +224,7 @@ impl JointTwoBodyConstraint { len += 1; } - JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]); + JointConstraintHelper::finalize_constraints(&mut out[start..len]); let start = len; for i in DIM..SPATIAL_DIM { @@ -325,19 +309,19 @@ impl JointTwoBodyConstraint { ); len += 1; } - JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]); + JointConstraintHelper::finalize_constraints(&mut out[start..len]); len } - pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel1 = solver_vels[self.solver_vel1[0]]; - let mut solver_vel2 = solver_vels[self.solver_vel2[0]]; + pub fn solve(&mut self, solver_vels: &mut SolverBodies) { + let mut solver_vel1 = solver_vels.get_vel(self.solver_vel1[0]); + let mut solver_vel2 = solver_vels.get_vel(self.solver_vel2[0]); self.solve_generic(&mut solver_vel1, &mut solver_vel2); - solver_vels[self.solver_vel1[0]] = solver_vel1; - solver_vels[self.solver_vel2[0]] = solver_vel2; + solver_vels.set_vel(self.solver_vel1[0], solver_vel1); + solver_vels.set_vel(self.solver_vel2[0], solver_vel2); } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { @@ -351,8 +335,8 @@ impl JointTwoBodyConstraint { } #[cfg(feature = "simd-is-enabled")] -impl JointTwoBodyConstraint { - pub fn lock_axes( +impl JointConstraint { + pub fn update( params: &IntegrationParameters, joint_id: [JointIndex; SIMD_WIDTH], body1: &JointSolverBody, @@ -362,7 +346,7 @@ impl JointTwoBodyConstraint { locked_axes: u8, out: &mut [Self], ) -> usize { - let builder = JointTwoBodyConstraintHelper::new( + let builder = JointConstraintHelper::new( frame1, frame2, &body1.world_com, @@ -393,372 +377,24 @@ impl JointTwoBodyConstraint { } } - JointTwoBodyConstraintHelper::finalize_constraints(&mut out[..len]); + JointConstraintHelper::finalize_constraints(&mut out[..len]); len } - pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel1 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]), - }; - let mut solver_vel2 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), - }; + pub fn solve(&mut self, solver_vels: &mut SolverBodies) { + let mut solver_vel1 = solver_vels.gather_vels(self.solver_vel1); + let mut solver_vel2 = solver_vels.gather_vels(self.solver_vel2); self.solve_generic(&mut solver_vel1, &mut solver_vel2); - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii); - solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii); - solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); - solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); - } + solver_vels.scatter_vels(self.solver_vel1, solver_vel1); + solver_vels.scatter_vels(self.solver_vel2, solver_vel2); } pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { let impulses: [_; SIMD_WIDTH] = self.impulse.into(); - // TODO: should we move the iteration on ii deeper in the mested match? - for ii in 0..SIMD_WIDTH { - let joint = &mut joints_all[self.joint_id[ii]].weight; - match self.writeback_id { - WritebackId::Dof(i) => joint.impulses[i] = impulses[ii], - WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii], - WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii], - } - } - } -} - -#[derive(Debug, Copy, Clone)] -pub struct JointOneBodyConstraint { - pub solver_vel2: [usize; LANES], - - pub joint_id: [JointIndex; LANES], - - pub impulse: N, - pub impulse_bounds: [N; 2], - pub lin_jac: Vector, - pub ang_jac2: AngVector, - - pub inv_lhs: N, - pub cfm_coeff: N, - pub cfm_gain: N, - pub rhs: N, - pub rhs_wo_bias: N, - - pub im2: Vector, - - pub writeback_id: WritebackId, -} - -impl JointOneBodyConstraint { - pub fn solve_generic(&mut self, solver_vel2: &mut SolverVel) { - let dlinvel = solver_vel2.linear; - let dangvel = solver_vel2.angular; - - let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs; - let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse)) - .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]); - let delta_impulse = total_impulse - self.impulse; - self.impulse = total_impulse; - - let lin_impulse = self.lin_jac * delta_impulse; - let ang_impulse = self.ang_jac2 * delta_impulse; - - solver_vel2.linear -= lin_impulse.component_mul(&self.im2); - solver_vel2.angular -= ang_impulse; - } - - pub fn remove_bias_from_rhs(&mut self) { - self.rhs = self.rhs_wo_bias; - } -} - -impl JointOneBodyConstraint { - pub fn lock_axes( - params: &IntegrationParameters, - joint_id: JointIndex, - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - frame1: &Isometry, - frame2: &Isometry, - joint: &GenericJoint, - out: &mut [Self], - ) -> usize { - let mut len = 0; - let locked_axes = joint.locked_axes.bits(); - let motor_axes = joint.motor_axes.bits() & !locked_axes; - let limit_axes = joint.limit_axes.bits() & !locked_axes; - let coupled_axes = joint.coupled_axes.bits(); - - // The has_lin/ang_coupling test is needed to avoid shl overflow later. - let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0; - let first_coupled_lin_axis_id = - (coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize; - - #[cfg(feature = "dim3")] - let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0; - #[cfg(feature = "dim3")] - let first_coupled_ang_axis_id = - (coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize; - - let builder = JointTwoBodyConstraintHelper::new( - frame1, - frame2, - &body1.world_com, - &body2.world_com, - locked_axes, - ); - - let start = len; - for i in DIM..SPATIAL_DIM { - if (motor_axes & !coupled_axes) & (1 << i) != 0 { - out[len] = builder.motor_angular_one_body( - [joint_id], - body1, - body2, - i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - for i in 0..DIM { - if (motor_axes & !coupled_axes) & (1 << i) != 0 { - let limits = if limit_axes & (1 << i) != 0 { - Some([joint.limits[i].min, joint.limits[i].max]) - } else { - None - }; - - out[len] = builder.motor_linear_one_body( - params, - [joint_id], - body1, - body2, - i, - &joint.motors[i].motor_params(params.dt), - limits, - WritebackId::Motor(i), - ); - len += 1; - } - } - - #[cfg(feature = "dim3")] - if has_ang_coupling && (motor_axes & (1 << first_coupled_ang_axis_id)) != 0 { - // TODO: coupled angular motor constraint. - } - - if has_lin_coupling && (motor_axes & (1 << first_coupled_lin_axis_id)) != 0 { - let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { - Some([ - joint.limits[first_coupled_lin_axis_id].min, - joint.limits[first_coupled_lin_axis_id].max, - ]) - } else { - None - }; - - out[len] = builder.motor_linear_coupled_one_body( - params, - [joint_id], - body1, - body2, - coupled_axes, - &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), - limits, - WritebackId::Motor(first_coupled_lin_axis_id), - ); - len += 1; - } - - JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]); - - let start = len; - for i in DIM..SPATIAL_DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_one_body( - params, - [joint_id], - body1, - body2, - i - DIM, - WritebackId::Dof(i), - ); - len += 1; - } - } - for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_one_body( - params, - [joint_id], - body1, - body2, - i, - WritebackId::Dof(i), - ); - len += 1; - } - } - - for i in DIM..SPATIAL_DIM { - if (limit_axes & !coupled_axes) & (1 << i) != 0 { - out[len] = builder.limit_angular_one_body( - params, - [joint_id], - body1, - body2, - i - DIM, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } - } - for i in 0..DIM { - if (limit_axes & !coupled_axes) & (1 << i) != 0 { - out[len] = builder.limit_linear_one_body( - params, - [joint_id], - body1, - body2, - i, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } - } - - #[cfg(feature = "dim3")] - if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 { - out[len] = builder.limit_angular_coupled_one_body( - params, - [joint_id], - body1, - body2, - coupled_axes, - [ - joint.limits[first_coupled_ang_axis_id].min, - joint.limits[first_coupled_ang_axis_id].max, - ], - WritebackId::Limit(first_coupled_ang_axis_id), - ); - len += 1; - } - - if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { - out[len] = builder.limit_linear_coupled_one_body( - params, - [joint_id], - body1, - body2, - coupled_axes, - [ - joint.limits[first_coupled_lin_axis_id].min, - joint.limits[first_coupled_lin_axis_id].max, - ], - WritebackId::Limit(first_coupled_lin_axis_id), - ); - len += 1; - } - JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]); - - len - } - - pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel2 = solver_vels[self.solver_vel2[0]]; - self.solve_generic(&mut solver_vel2); - solver_vels[self.solver_vel2[0]] = solver_vel2; - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - let joint = &mut joints_all[self.joint_id[0]].weight; - match self.writeback_id { - WritebackId::Dof(i) => joint.impulses[i] = self.impulse, - WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse, - WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse, - } - } -} - -#[cfg(feature = "simd-is-enabled")] -impl JointOneBodyConstraint { - pub fn lock_axes( - params: &IntegrationParameters, - joint_id: [JointIndex; SIMD_WIDTH], - body1: &JointFixedSolverBody, - body2: &JointSolverBody, - frame1: &Isometry, - frame2: &Isometry, - locked_axes: u8, - out: &mut [Self], - ) -> usize { - let mut len = 0; - let builder = JointTwoBodyConstraintHelper::new( - frame1, - frame2, - &body1.world_com, - &body2.world_com, - locked_axes, - ); - - for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_one_body( - params, - joint_id, - body1, - body2, - i, - WritebackId::Dof(i), - ); - len += 1; - } - } - for i in DIM..SPATIAL_DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_one_body( - params, - joint_id, - body1, - body2, - i - DIM, - WritebackId::Dof(i), - ); - len += 1; - } - } - - JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[..len]); - len - } - - pub fn solve(&mut self, solver_vels: &mut [SolverVel]) { - let mut solver_vel2 = SolverVel { - linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]), - angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]), - }; - - self.solve_generic(&mut solver_vel2); - - for ii in 0..SIMD_WIDTH { - solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii); - solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii); - } - } - - pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { - let impulses: [_; SIMD_WIDTH] = self.impulse.into(); - - // TODO: should we move the iteration on ii deeper in the mested match? + // TODO: should we move the iteration on ii deeper in the nested match? for ii in 0..SIMD_WIDTH { let joint = &mut joints_all[self.joint_id[ii]].weight; match self.writeback_id { diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index 7d9f0c5..e4df2df 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -1,18 +1,16 @@ pub use joint_velocity_constraint::{JointSolverBody, MotorParameters, WritebackId}; -pub use any_joint_constraint::JointConstraintTypes; -pub use joint_constraint_builder::JointTwoBodyConstraintHelper; -pub use joint_constraints_set::JointConstraintsSet; -pub use joint_generic_constraint::{JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint}; -pub use joint_generic_constraint_builder::{ - JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder, - JointGenericVelocityOneBodyExternalConstraintBuilder, - JointGenericVelocityOneBodyInternalConstraintBuilder, +pub use any_joint_constraint::AnyJointConstraintMut; +pub use generic_joint_constraint::GenericJointConstraint; +pub use generic_joint_constraint_builder::{ + JointGenericExternalConstraintBuilder, JointGenericInternalConstraintBuilder, LinkOrBodyRef, }; +pub use joint_constraint_builder::JointConstraintHelper; +pub use joint_constraints_set::JointConstraintsSet; mod any_joint_constraint; +mod generic_joint_constraint; +mod generic_joint_constraint_builder; mod joint_constraint_builder; mod joint_constraints_set; -mod joint_generic_constraint; -mod joint_generic_constraint_builder; mod joint_velocity_constraint; diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index 044d658..261a2ac 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -7,17 +7,12 @@ pub(crate) use self::island_solver::IslandSolver; // #[cfg(feature = "parallel")] // pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver; // #[cfg(not(feature = "parallel"))] -use self::solver_constraints_set::SolverConstraintsSet; -// #[cfg(not(feature = "parallel"))] use self::velocity_solver::VelocitySolver; use contact_constraint::*; -use interaction_groups::*; pub(crate) use joint_constraint::MotorParameters; pub use joint_constraint::*; -use solver_body::SolverBody; -use solver_constraints_set::{AnyConstraintMut, ConstraintTypes}; -use solver_vel::SolverVel; +use solver_body::SolverVel; mod categorization; mod contact_constraint; @@ -33,18 +28,17 @@ mod joint_constraint; // mod parallel_velocity_solver; mod solver_body; // #[cfg(not(feature = "parallel"))] -mod solver_constraints_set; -mod solver_vel; // #[cfg(not(feature = "parallel"))] mod velocity_solver; -// TODO: SAFETY: restrict with bytemuck::AnyBitPattern to make this safe. +// TODO: SAFETY: restrict with bytemuck::Zeroable to make this safe. pub unsafe fn reset_buffer(buffer: &mut Vec, len: usize) { buffer.clear(); buffer.reserve(len); unsafe { - buffer.as_mut_ptr().write_bytes(u8::MAX, len); + // NOTE: writing zeros is faster than u8::MAX. + buffer.as_mut_ptr().write_bytes(0, len); buffer.set_len(len); } } diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 4cfa2d2..1bec778 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -235,7 +235,7 @@ impl ParallelIslandSolver { { let mut solver_id = 0; let island_range = islands.active_island_range(island_id); - let active_bodies = &islands.active_dynamic_set[island_range]; + let active_bodies = &islands.active_set[island_range]; for handle in active_bodies { if let Some(link) = multibodies.rigid_body_link(*handle).copied() { let multibody = multibodies @@ -299,7 +299,7 @@ impl ParallelIslandSolver { // Initialize `solver_vels` (per-body velocity deltas) with external accelerations (gravity etc): { let island_range = islands.active_island_range(island_id); - let active_bodies = &islands.active_dynamic_set[island_range]; + let active_bodies = &islands.active_set[island_range]; concurrent_loop! { let batch_size = thread.batch_size; @@ -321,7 +321,7 @@ impl ParallelIslandSolver { // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied // by the square root of the inertia tensor: - dvel.angular += rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt; + dvel.angular += rb.mprops.effective_world_inv_inertia * rb.forces.torque * params.dt; dvel.linear += rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt; } } diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs index 358a992..d3f02f0 100644 --- a/src/dynamics/solver/parallel_solver_constraints.rs +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -164,7 +164,7 @@ macro_rules! impl_init_constraints_group { self.constraint_descs.push(( total_num_constraints, ConstraintDesc::TwoBodyGrouped( - gather![|ii| interaction_i[ii]], + array![|ii| interaction_i[ii]], ), )); total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; @@ -190,7 +190,7 @@ macro_rules! impl_init_constraints_group { self.constraint_descs.push(( total_num_constraints, ConstraintDesc::OneBodyGrouped( - gather![|ii| interaction_i[ii]], + array![|ii| interaction_i[ii]], ), )); total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0; @@ -337,12 +337,12 @@ impl ParallelSolverConstraints { } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::TwoBodyGrouped(manifold_id) => { - let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]]; TwoBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::OneBodyGrouped(manifold_id) => { - let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]]; + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]]; OneBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GenericTwoBodyNongrouped(manifold_id, j_id) => { @@ -387,12 +387,12 @@ impl ParallelSolverConstraints { } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::TwoBodyGrouped(joint_id) => { - let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight]; + let impulse_joints = array![|ii| &joints_all[joint_id[ii]].weight]; JointConstraintTypes::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); } #[cfg(feature = "simd-is-enabled")] ConstraintDesc::OneBodyGrouped(joint_id) => { - let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight]; + let impulse_joints = array![|ii| &joints_all[joint_id[ii]].weight]; JointConstraintTypes::from_wide_joint_one_body(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0)); } ConstraintDesc::GenericTwoBodyNongrouped(joint_id, j_id) => { diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 8870c98..eedc3f2 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -1,8 +1,8 @@ use super::{ContactConstraintTypes, JointConstraintTypes, SolverVel, ThreadContext}; use crate::concurrent_loop; use crate::dynamics::{ - solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge, - MultibodyJointSet, RigidBodySet, + IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet, + solver::ParallelSolverConstraints, }; use crate::geometry::ContactManifold; use crate::math::Real; @@ -182,7 +182,7 @@ impl ParallelVelocitySolver { // Integrate positions. { let island_range = islands.active_island_range(island_id); - let active_bodies = &islands.active_dynamic_set[island_range]; + let active_bodies = &islands.active_set[island_range]; concurrent_loop! { let batch_size = thread.batch_size; @@ -206,7 +206,7 @@ impl ParallelVelocitySolver { let rb = bodies.index_mut_internal(*handle); let dvel = self.solver_vels[rb.ids.active_set_offset]; let dangvel = rb.mprops - .effective_world_inv_inertia_sqrt + .effective_world_inv_inertia .transform_vector(dvel.angular); // Update positions. @@ -284,7 +284,7 @@ impl ParallelVelocitySolver { // Update velocities. { let island_range = islands.active_island_range(island_id); - let active_bodies = &islands.active_dynamic_set[island_range]; + let active_bodies = &islands.active_set[island_range]; concurrent_loop! { let batch_size = thread.batch_size; @@ -304,7 +304,7 @@ impl ParallelVelocitySolver { let rb = bodies.index_mut_internal(*handle); let dvel = self.solver_vels[rb.ids.active_set_offset]; let dangvel = rb.mprops - .effective_world_inv_inertia_sqrt + .effective_world_inv_inertia .transform_vector(dvel.angular); rb.vels.linvel += dvel.linear; rb.vels.angvel += dangvel; diff --git a/src/dynamics/solver/solver_body.rs b/src/dynamics/solver/solver_body.rs index 297f28a..f4890fa 100644 --- a/src/dynamics/solver/solver_body.rs +++ b/src/dynamics/solver/solver_body.rs @@ -1,59 +1,555 @@ -use crate::dynamics::{RigidBody, RigidBodyVelocity}; -use crate::math::{AngularInertia, Isometry, Point, Real, Vector}; -use crate::prelude::RigidBodyDamping; +use crate::dynamics::RigidBody; +use crate::math::{AngularInertia, Isometry, Real, SPATIAL_DIM, Vector}; +use crate::utils::SimdRealCopy; +use na::{DVectorView, DVectorViewMut}; +use parry::math::{AngVector, SIMD_WIDTH, SimdReal, Translation}; +use std::ops::{AddAssign, Sub, SubAssign}; -#[cfg(feature = "dim2")] -use crate::num::Zero; +#[cfg(feature = "simd-is-enabled")] +use crate::utils::transmute_to_wide; -#[derive(Copy, Clone, Debug)] -pub(crate) struct SolverBody { - pub position: Isometry, - pub integrated_vels: RigidBodyVelocity, - pub im: Vector, - pub sqrt_ii: AngularInertia, - pub world_com: Point, - pub ccd_thickness: Real, - pub damping: RigidBodyDamping, - pub local_com: Point, +#[cfg(feature = "simd-is-enabled")] +macro_rules! aos( + ($data_repr: ident [ $idx: ident ] . $data_n: ident, $fallback: ident) => { + [ + if ($idx[0] as usize) < $data_repr.len() { + $data_repr[$idx[0] as usize].$data_n.0 + } else { + $fallback.$data_n.0 + }, + if ($idx[1] as usize) < $data_repr.len() { + $data_repr[$idx[1] as usize].$data_n.0 + } else { + $fallback.$data_n.0 + }, + if ($idx[2] as usize) < $data_repr.len() { + $data_repr[$idx[2] as usize].$data_n.0 + } else { + $fallback.$data_n.0 + }, + if ($idx[3] as usize) < $data_repr.len() { + $data_repr[$idx[3] as usize].$data_n.0 + } else { + $fallback.$data_n.0 + }, + ] + } +); + +#[cfg(feature = "simd-is-enabled")] +macro_rules! aos_unchecked( + ($data_repr: ident [ $idx: ident ] . $data_n: ident) => { + [ + unsafe { $data_repr.get_unchecked($idx[0] as usize).$data_n.0 }, + unsafe { $data_repr.get_unchecked($idx[1] as usize).$data_n.0 }, + unsafe { $data_repr.get_unchecked($idx[2] as usize).$data_n.0 }, + unsafe { $data_repr.get_unchecked($idx[3] as usize).$data_n.0 }, + ] + } +); + +#[cfg(feature = "simd-is-enabled")] +macro_rules! scatter( + ($data: ident [ $idx: ident [ $i: expr ] ] = [$($aos: ident),*]) => { + unsafe { + #[allow(clippy::missing_transmute_annotations)] // Different macro calls transmute to different types + if ($idx[$i] as usize) < $data.len() { + $data[$idx[$i] as usize] = std::mem::transmute([$($aos[$i]),*]); + } + } + } +); + +#[cfg(feature = "simd-is-enabled")] +macro_rules! scatter_unchecked( + ($data: ident [ $idx: ident [ $i: expr ] ] = [$($aos: ident),*]) => { + #[allow(clippy::missing_transmute_annotations)] // Different macro calls transmute to different types + unsafe { + *$data.get_unchecked_mut($idx[$i] as usize) = std::mem::transmute([$($aos[$i]),*]); + } + } +); + +#[derive(Default)] +pub struct SolverBodies { + pub vels: Vec>, + pub poses: Vec>, } -impl Default for SolverBody { +impl SolverBodies { + pub fn clear(&mut self) { + self.vels.clear(); + self.poses.clear(); + } + + pub fn resize(&mut self, sz: usize) { + self.vels.resize(sz, Default::default()); + self.poses.resize(sz, Default::default()); + } + + pub fn len(&self) -> usize { + self.vels.len() + } + + // TODO: add a SIMD version? + pub fn copy_from(&mut self, _dt: Real, i: usize, rb: &RigidBody) { + let poses = &mut self.poses[i]; + let vels = &mut self.vels[i]; + + #[cfg(feature = "dim2")] + { + vels.angular = rb.vels.angvel; + } + + #[cfg(feature = "dim3")] + { + if rb.forces.gyroscopic_forces_enabled { + vels.angular = rb.angvel_with_gyroscopic_forces(_dt); + } else { + vels.angular = *rb.angvel(); + } + } + vels.linear = rb.vels.linvel; + poses.pose = rb.pos.position * Translation::from(rb.mprops.local_mprops.local_com); + + if rb.is_dynamic_or_kinematic() { + poses.ii = rb.mprops.effective_world_inv_inertia; + poses.im = rb.mprops.effective_inv_mass; + } else { + poses.ii = Default::default(); + poses.im = Default::default(); + } + } + + #[inline] + pub unsafe fn gather_vels_unchecked(&self, idx: [u32; SIMD_WIDTH]) -> SolverVel { + #[cfg(not(feature = "simd-is-enabled"))] + unsafe { + *self.vels.get_unchecked(idx[0] as usize) + } + #[cfg(feature = "simd-is-enabled")] + unsafe { + SolverVel::gather_unchecked(&self.vels, idx) + } + } + + #[inline] + pub fn gather_vels(&self, idx: [u32; SIMD_WIDTH]) -> SolverVel { + #[cfg(not(feature = "simd-is-enabled"))] + return self.vels.get(idx[0] as usize).copied().unwrap_or_default(); + #[cfg(feature = "simd-is-enabled")] + return SolverVel::gather(&self.vels, idx); + } + + #[inline] + pub fn get_vel(&self, i: u32) -> SolverVel { + self.vels.get(i as usize).copied().unwrap_or_default() + } + + #[inline] + pub fn scatter_vels(&mut self, idx: [u32; SIMD_WIDTH], vels: SolverVel) { + #[cfg(not(feature = "simd-is-enabled"))] + if (idx[0] as usize) < self.vels.len() { + self.vels[idx[0] as usize] = vels + } + + #[cfg(feature = "simd-is-enabled")] + vels.scatter(&mut self.vels, idx); + } + + #[inline] + pub fn set_vel(&mut self, i: u32, vel: SolverVel) { + if (i as usize) < self.vels.len() { + self.vels[i as usize] = vel; + } + } + + #[inline] + pub fn get_pose(&self, i: u32) -> SolverPose { + self.poses.get(i as usize).copied().unwrap_or_default() + } + + #[inline] + pub unsafe fn gather_poses_unchecked(&self, idx: [u32; SIMD_WIDTH]) -> SolverPose { + #[cfg(not(feature = "simd-is-enabled"))] + unsafe { + *self.poses.get_unchecked(idx[0] as usize) + } + + #[cfg(feature = "simd-is-enabled")] + unsafe { + SolverPose::gather_unchecked(&self.poses, idx) + } + } + + #[inline] + pub fn gather_poses(&self, idx: [u32; SIMD_WIDTH]) -> SolverPose { + #[cfg(not(feature = "simd-is-enabled"))] + return self.poses.get(idx[0] as usize).copied().unwrap_or_default(); + + #[cfg(feature = "simd-is-enabled")] + return SolverPose::gather(&self.poses, idx); + } + + #[inline] + pub fn scatter_poses(&mut self, idx: [u32; SIMD_WIDTH], poses: SolverPose) { + #[cfg(not(feature = "simd-is-enabled"))] + if (idx[0] as usize) < self.poses.len() { + self.poses[idx[0] as usize] = poses; + } + + #[cfg(feature = "simd-is-enabled")] + poses.scatter(&mut self.poses, idx); + } + + #[inline] + pub fn scatter_poses_unchecked(&mut self, idx: [u32; SIMD_WIDTH], poses: SolverPose) { + #[cfg(not(feature = "simd-is-enabled"))] + unsafe { + *self.poses.get_unchecked_mut(idx[0] as usize) = poses + } + + #[cfg(feature = "simd-is-enabled")] + poses.scatter_unchecked(&mut self.poses, idx); + } +} + +// Total 7/13 +#[repr(C)] +#[cfg_attr(feature = "simd-is-enabled", repr(align(16)))] +#[derive(Copy, Clone, Default)] +pub struct SolverVel { + pub linear: Vector, // 2/3 + pub angular: AngVector, // 1/3 + // TODO: explicit padding are useful for static assertions. + // But might be wasteful for the SolverVel + // specialization. + #[cfg(feature = "simd-is-enabled")] + #[cfg(feature = "dim2")] + padding: [T; 1], + #[cfg(feature = "simd-is-enabled")] + #[cfg(feature = "dim3")] + padding: [T; 2], +} + +#[cfg(feature = "simd-is-enabled")] +#[repr(C)] +struct SolverVelRepr { + data0: SimdReal, + #[cfg(feature = "dim3")] + data1: SimdReal, +} + +#[cfg(feature = "simd-is-enabled")] +impl SolverVelRepr { + pub fn zero() -> Self { + Self { + data0: na::zero(), + #[cfg(feature = "dim3")] + data1: na::zero(), + } + } +} + +#[cfg(feature = "simd-is-enabled")] +impl SolverVel { + #[inline] + pub unsafe fn gather_unchecked(data: &[SolverVel], idx: [u32; SIMD_WIDTH]) -> Self { + // TODO: double-check that the compiler is using simd loads and + // isn’t generating useless copies. + + let data_repr: &[SolverVelRepr] = unsafe { std::mem::transmute(data) }; + + #[cfg(feature = "dim2")] + { + let aos = aos_unchecked!(data_repr[idx].data0); + let soa = wide::f32x4::transpose(transmute_to_wide(aos)); + unsafe { std::mem::transmute(soa) } + } + + #[cfg(feature = "dim3")] + { + let aos0 = aos_unchecked!(data_repr[idx].data0); + let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0)); + let aos1 = aos_unchecked!(data_repr[idx].data1); + let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1)); + unsafe { std::mem::transmute((soa0, soa1)) } + } + } + + #[inline] + pub fn gather(data: &[SolverVel], idx: [u32; SIMD_WIDTH]) -> Self { + // TODO: double-check that the compiler is using simd loads and + // isn’t generating useless copies. + + let zero = SolverVelRepr::zero(); + let data_repr: &[SolverVelRepr] = unsafe { std::mem::transmute(data) }; + + #[cfg(feature = "dim2")] + { + let aos = aos!(data_repr[idx].data0, zero); + let soa = wide::f32x4::transpose(transmute_to_wide(aos)); + unsafe { std::mem::transmute(soa) } + } + + #[cfg(feature = "dim3")] + { + let aos0 = aos!(data_repr[idx].data0, zero); + let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0)); + let aos1 = aos!(data_repr[idx].data1, zero); + let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1)); + unsafe { std::mem::transmute((soa0, soa1)) } + } + } + + #[inline] + #[cfg(feature = "dim2")] + pub fn scatter(self, data: &mut [SolverVel], idx: [u32; SIMD_WIDTH]) { + // TODO: double-check that the compiler is using simd loads and no useless copies. + let soa: [wide::f32x4; 4] = unsafe { std::mem::transmute(self) }; + let aos = wide::f32x4::transpose(soa); + scatter!(data[idx[0]] = [aos]); + scatter!(data[idx[1]] = [aos]); + scatter!(data[idx[2]] = [aos]); + scatter!(data[idx[3]] = [aos]); + } + + #[inline] + #[cfg(feature = "dim3")] + pub fn scatter(self, data: &mut [SolverVel], idx: [u32; SIMD_WIDTH]) { + let soa: [[wide::f32x4; 4]; 2] = unsafe { std::mem::transmute(self) }; + // TODO: double-check that the compiler is using simd loads and no useless copies. + let aos0 = wide::f32x4::transpose(soa[0]); + let aos1 = wide::f32x4::transpose(soa[1]); + scatter!(data[idx[0]] = [aos0, aos1]); + scatter!(data[idx[1]] = [aos0, aos1]); + scatter!(data[idx[2]] = [aos0, aos1]); + scatter!(data[idx[3]] = [aos0, aos1]); + } +} + +// Total: 7/16 +#[repr(C)] +#[cfg_attr(feature = "simd-is-enabled", repr(align(16)))] +#[derive(Copy, Clone)] +pub struct SolverPose { + /// Positional change of the rigid-body’s center of mass. + pub pose: Isometry, // 4/7 + pub ii: AngularInertia, // 1/6 + pub im: Vector, // 2/3 + #[cfg(feature = "dim2")] + pub padding: [T; 1], +} + +impl Default for SolverPose { + #[inline] fn default() -> Self { Self { - position: Isometry::identity(), - integrated_vels: RigidBodyVelocity::zero(), - im: na::zero(), - sqrt_ii: AngularInertia::zero(), - world_com: Point::origin(), - ccd_thickness: 0.0, - damping: RigidBodyDamping::default(), - local_com: Point::origin(), + pose: Isometry::identity(), + ii: Default::default(), + im: Default::default(), + #[cfg(feature = "dim2")] + padding: Default::default(), } } } -impl SolverBody { - pub fn from(rb: &RigidBody) -> Self { +#[cfg(feature = "simd-is-enabled")] +#[repr(C)] +struct SolverPoseRepr { + data0: SimdReal, + data1: SimdReal, + #[cfg(feature = "dim3")] + data2: SimdReal, + #[cfg(feature = "dim3")] + data3: SimdReal, +} + +#[cfg(feature = "simd-is-enabled")] +impl SolverPoseRepr { + pub fn identity() -> Self { + // TODO PERF: will the compiler handle this efficiently and generate + // everything at compile-time? + unsafe { std::mem::transmute(SolverPose::default()) } + } +} + +#[cfg(feature = "simd-is-enabled")] +impl SolverPose { + #[inline] + pub unsafe fn gather_unchecked(data: &[SolverPose], idx: [u32; SIMD_WIDTH]) -> Self { + // TODO: double-check that the compiler is using simd loads and + // isn’t generating useless copies. + + let data_repr: &[SolverPoseRepr] = unsafe { std::mem::transmute(data) }; + + #[cfg(feature = "dim2")] + { + let aos0 = aos_unchecked!(data_repr[idx].data0); + let aos1 = aos_unchecked!(data_repr[idx].data1); + let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0)); + let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1)); + unsafe { std::mem::transmute([soa0, soa1]) } + } + + #[cfg(feature = "dim3")] + { + let aos0 = aos_unchecked!(data_repr[idx].data0); + let aos1 = aos_unchecked!(data_repr[idx].data1); + let aos2 = aos_unchecked!(data_repr[idx].data2); + let aos3 = aos_unchecked!(data_repr[idx].data3); + let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0)); + let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1)); + let soa2 = wide::f32x4::transpose(transmute_to_wide(aos2)); + let soa3 = wide::f32x4::transpose(transmute_to_wide(aos3)); + unsafe { std::mem::transmute([soa0, soa1, soa2, soa3]) } + } + } + + #[inline] + pub fn gather(data: &[SolverPose], idx: [u32; SIMD_WIDTH]) -> Self { + // TODO: double-check that the compiler is using simd loads and + // isn’t generating useless copies. + + let identity = SolverPoseRepr::identity(); + let data_repr: &[SolverPoseRepr] = unsafe { std::mem::transmute(data) }; + + #[cfg(feature = "dim2")] + { + let aos0 = aos!(data_repr[idx].data0, identity); + let aos1 = aos!(data_repr[idx].data1, identity); + let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0)); + let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1)); + unsafe { std::mem::transmute([soa0, soa1]) } + } + + #[cfg(feature = "dim3")] + { + let aos0 = aos!(data_repr[idx].data0, identity); + let aos1 = aos!(data_repr[idx].data1, identity); + let aos2 = aos!(data_repr[idx].data2, identity); + let aos3 = aos!(data_repr[idx].data3, identity); + let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0)); + let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1)); + let soa2 = wide::f32x4::transpose(transmute_to_wide(aos2)); + let soa3 = wide::f32x4::transpose(transmute_to_wide(aos3)); + unsafe { std::mem::transmute([soa0, soa1, soa2, soa3]) } + } + } + + #[inline] + #[cfg(feature = "dim2")] + pub fn scatter_unchecked(self, data: &mut [SolverPose], idx: [u32; SIMD_WIDTH]) { + // TODO: double-check that the compiler is using simd loads and no useless copies. + let soa: [[wide::f32x4; 4]; 2] = unsafe { std::mem::transmute(self) }; + let aos0 = wide::f32x4::transpose(soa[0]); + let aos1 = wide::f32x4::transpose(soa[1]); + scatter_unchecked!(data[idx[0]] = [aos0, aos1]); + scatter_unchecked!(data[idx[1]] = [aos0, aos1]); + scatter_unchecked!(data[idx[2]] = [aos0, aos1]); + scatter_unchecked!(data[idx[3]] = [aos0, aos1]); + } + + #[inline] + #[cfg(feature = "dim3")] + pub fn scatter_unchecked(self, data: &mut [SolverPose], idx: [u32; SIMD_WIDTH]) { + let soa: [[wide::f32x4; 4]; 4] = unsafe { std::mem::transmute(self) }; + // TODO: double-check that the compiler is using simd loads and no useless copies. + let aos0 = wide::f32x4::transpose(soa[0]); + let aos1 = wide::f32x4::transpose(soa[1]); + let aos2 = wide::f32x4::transpose(soa[2]); + let aos3 = wide::f32x4::transpose(soa[3]); + scatter_unchecked!(data[idx[0]] = [aos0, aos1, aos2, aos3]); + scatter_unchecked!(data[idx[1]] = [aos0, aos1, aos2, aos3]); + scatter_unchecked!(data[idx[2]] = [aos0, aos1, aos2, aos3]); + scatter_unchecked!(data[idx[3]] = [aos0, aos1, aos2, aos3]); + } + + #[inline] + #[cfg(feature = "dim2")] + pub fn scatter(self, data: &mut [SolverPose], idx: [u32; SIMD_WIDTH]) { + // TODO: double-check that the compiler is using simd loads and no useless copies. + let soa: [[wide::f32x4; 4]; 2] = unsafe { std::mem::transmute(self) }; + let aos0 = wide::f32x4::transpose(soa[0]); + let aos1 = wide::f32x4::transpose(soa[1]); + scatter!(data[idx[0]] = [aos0, aos1]); + scatter!(data[idx[1]] = [aos0, aos1]); + scatter!(data[idx[2]] = [aos0, aos1]); + scatter!(data[idx[3]] = [aos0, aos1]); + } + + #[inline] + #[cfg(feature = "dim3")] + pub fn scatter(self, data: &mut [SolverPose], idx: [u32; SIMD_WIDTH]) { + let soa: [[wide::f32x4; 4]; 4] = unsafe { std::mem::transmute(self) }; + // TODO: double-check that the compiler is using simd loads and no useless copies. + let aos0 = wide::f32x4::transpose(soa[0]); + let aos1 = wide::f32x4::transpose(soa[1]); + let aos2 = wide::f32x4::transpose(soa[2]); + let aos3 = wide::f32x4::transpose(soa[3]); + scatter!(data[idx[0]] = [aos0, aos1, aos2, aos3]); + scatter!(data[idx[1]] = [aos0, aos1, aos2, aos3]); + scatter!(data[idx[2]] = [aos0, aos1, aos2, aos3]); + scatter!(data[idx[3]] = [aos0, aos1, aos2, aos3]); + } +} + +impl SolverVel { + pub fn as_slice(&self) -> &[N; SPATIAL_DIM] { + unsafe { std::mem::transmute(self) } + } + + pub fn as_mut_slice(&mut self) -> &mut [N; SPATIAL_DIM] { + unsafe { std::mem::transmute(self) } + } + + pub fn as_vector_slice(&self) -> DVectorView<'_, N> { + DVectorView::from_slice(&self.as_slice()[..], SPATIAL_DIM) + } + + pub fn as_vector_slice_mut(&mut self) -> DVectorViewMut<'_, N> { + DVectorViewMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM) + } +} + +impl SolverVel { + pub fn zero() -> Self { Self { - position: rb.pos.position, - integrated_vels: RigidBodyVelocity::zero(), - im: rb.mprops.effective_inv_mass, - sqrt_ii: rb.mprops.effective_world_inv_inertia_sqrt, - world_com: rb.mprops.world_com, - ccd_thickness: rb.ccd.ccd_thickness, - damping: rb.damping, - local_com: rb.mprops.local_mprops.local_com, + linear: na::zero(), + angular: na::zero(), + #[cfg(feature = "simd-is-enabled")] + #[cfg(feature = "dim2")] + padding: [na::zero(); 1], + #[cfg(feature = "simd-is-enabled")] + #[cfg(feature = "dim3")] + padding: [na::zero(); 2], } } +} - pub fn copy_from(&mut self, rb: &RigidBody) { - self.position = rb.pos.position; - self.integrated_vels = RigidBodyVelocity::zero(); - self.im = rb.mprops.effective_inv_mass; - self.sqrt_ii = rb.mprops.effective_world_inv_inertia_sqrt; - self.world_com = rb.mprops.world_com; - self.ccd_thickness = rb.ccd.ccd_thickness; - self.damping = rb.damping; - self.local_com = rb.mprops.local_mprops.local_com; +impl AddAssign for SolverVel { + fn add_assign(&mut self, rhs: Self) { + self.linear += rhs.linear; + self.angular += rhs.angular; + } +} + +impl SubAssign for SolverVel { + fn sub_assign(&mut self, rhs: Self) { + self.linear -= rhs.linear; + self.angular -= rhs.angular; + } +} + +impl Sub for SolverVel { + type Output = Self; + + fn sub(self, rhs: Self) -> Self { + SolverVel { + linear: self.linear - rhs.linear, + angular: self.angular - rhs.angular, + #[cfg(feature = "simd-is-enabled")] + padding: self.padding, + } } } diff --git a/src/dynamics/solver/solver_constraints_set.rs b/src/dynamics/solver/solver_constraints_set.rs deleted file mode 100644 index 1161c55..0000000 --- a/src/dynamics/solver/solver_constraints_set.rs +++ /dev/null @@ -1,242 +0,0 @@ -use super::InteractionGroups; -use crate::math::Real; -use na::DVector; - -pub(crate) trait ConstraintTypes { - type OneBody; - type TwoBodies; - type GenericOneBody; - type GenericTwoBodies; - - #[cfg(feature = "simd-is-enabled")] - type SimdOneBody; - #[cfg(feature = "simd-is-enabled")] - type SimdTwoBodies; - - type BuilderOneBody; - type BuilderTwoBodies; - - type GenericBuilderOneBody; - type GenericBuilderTwoBodies; - - #[cfg(feature = "simd-is-enabled")] - type SimdBuilderOneBody; - #[cfg(feature = "simd-is-enabled")] - type SimdBuilderTwoBodies; -} - -#[derive(Debug)] -pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> { - OneBody(&'a mut Constraints::OneBody), - TwoBodies(&'a mut Constraints::TwoBodies), - GenericOneBody(&'a mut Constraints::GenericOneBody), - GenericTwoBodies(&'a mut Constraints::GenericTwoBodies), - #[cfg(feature = "simd-is-enabled")] - SimdOneBody(&'a mut Constraints::SimdOneBody), - #[cfg(feature = "simd-is-enabled")] - SimdTwoBodies(&'a mut Constraints::SimdTwoBodies), -} - -pub(crate) struct SolverConstraintsSet { - pub generic_jacobians: DVector, - pub two_body_interactions: Vec, - pub one_body_interactions: Vec, - pub generic_two_body_interactions: Vec, - pub generic_one_body_interactions: Vec, - pub interaction_groups: InteractionGroups, - pub one_body_interaction_groups: InteractionGroups, - - pub velocity_constraints: Vec, - pub generic_velocity_constraints: Vec, - #[cfg(feature = "simd-is-enabled")] - pub simd_velocity_constraints: Vec, - pub velocity_one_body_constraints: Vec, - pub generic_velocity_one_body_constraints: Vec, - #[cfg(feature = "simd-is-enabled")] - pub simd_velocity_one_body_constraints: Vec, - - pub velocity_constraints_builder: Vec, - pub generic_velocity_constraints_builder: Vec, - #[cfg(feature = "simd-is-enabled")] - pub simd_velocity_constraints_builder: Vec, - pub velocity_one_body_constraints_builder: Vec, - pub generic_velocity_one_body_constraints_builder: Vec, - #[cfg(feature = "simd-is-enabled")] - pub simd_velocity_one_body_constraints_builder: Vec, -} - -impl SolverConstraintsSet { - pub fn new() -> Self { - Self { - generic_jacobians: DVector::zeros(0), - two_body_interactions: vec![], - one_body_interactions: vec![], - generic_two_body_interactions: vec![], - generic_one_body_interactions: vec![], - interaction_groups: InteractionGroups::new(), - one_body_interaction_groups: InteractionGroups::new(), - - velocity_constraints: vec![], - generic_velocity_constraints: vec![], - #[cfg(feature = "simd-is-enabled")] - simd_velocity_constraints: vec![], - velocity_one_body_constraints: vec![], - generic_velocity_one_body_constraints: vec![], - #[cfg(feature = "simd-is-enabled")] - simd_velocity_one_body_constraints: vec![], - - velocity_constraints_builder: vec![], - generic_velocity_constraints_builder: vec![], - #[cfg(feature = "simd-is-enabled")] - simd_velocity_constraints_builder: vec![], - velocity_one_body_constraints_builder: vec![], - generic_velocity_one_body_constraints_builder: vec![], - #[cfg(feature = "simd-is-enabled")] - simd_velocity_one_body_constraints_builder: vec![], - } - } - - #[allow(dead_code)] // Useful for debugging. - pub fn print_counts(&self) { - println!("Solver constraints:"); - println!( - "|__ two_body_interactions: {}", - self.two_body_interactions.len() - ); - println!( - "|__ one_body_interactions: {}", - self.one_body_interactions.len() - ); - println!( - "|__ generic_two_body_interactions: {}", - self.generic_two_body_interactions.len() - ); - println!( - "|__ generic_one_body_interactions: {}", - self.generic_one_body_interactions.len() - ); - - println!( - "|__ velocity_constraints: {}", - self.velocity_constraints.len() - ); - println!( - "|__ generic_velocity_constraints: {}", - self.generic_velocity_constraints.len() - ); - #[cfg(feature = "simd-is-enabled")] - println!( - "|__ simd_velocity_constraints: {}", - self.simd_velocity_constraints.len() - ); - println!( - "|__ velocity_one_body_constraints: {}", - self.velocity_one_body_constraints.len() - ); - println!( - "|__ generic_velocity_one_body_constraints: {}", - self.generic_velocity_one_body_constraints.len() - ); - #[cfg(feature = "simd-is-enabled")] - println!( - "|__ simd_velocity_one_body_constraints: {}", - self.simd_velocity_one_body_constraints.len() - ); - - println!( - "|__ velocity_constraints_builder: {}", - self.velocity_constraints_builder.len() - ); - println!( - "|__ generic_velocity_constraints_builder: {}", - self.generic_velocity_constraints_builder.len() - ); - #[cfg(feature = "simd-is-enabled")] - println!( - "|__ simd_velocity_constraints_builder: {}", - self.simd_velocity_constraints_builder.len() - ); - println!( - "|__ velocity_one_body_constraints_builder: {}", - self.velocity_one_body_constraints_builder.len() - ); - println!( - "|__ generic_velocity_one_body_constraints_builder: {}", - self.generic_velocity_one_body_constraints_builder.len() - ); - #[cfg(feature = "simd-is-enabled")] - println!( - "|__ simd_velocity_one_body_constraints_builder: {}", - self.simd_velocity_one_body_constraints_builder.len() - ); - } - - pub fn clear_constraints(&mut self) { - self.generic_jacobians.fill(0.0); - self.velocity_constraints.clear(); - self.velocity_one_body_constraints.clear(); - self.generic_velocity_constraints.clear(); - self.generic_velocity_one_body_constraints.clear(); - - #[cfg(feature = "simd-is-enabled")] - { - self.simd_velocity_constraints.clear(); - self.simd_velocity_one_body_constraints.clear(); - } - } - - pub fn clear_builders(&mut self) { - self.velocity_constraints_builder.clear(); - self.velocity_one_body_constraints_builder.clear(); - self.generic_velocity_constraints_builder.clear(); - self.generic_velocity_one_body_constraints_builder.clear(); - - #[cfg(feature = "simd-is-enabled")] - { - self.simd_velocity_constraints_builder.clear(); - self.simd_velocity_one_body_constraints_builder.clear(); - } - } - - // Returns the generic jacobians and a mutable iterator through all the constraints. - pub fn iter_constraints_mut( - &mut self, - ) -> ( - &DVector, - impl Iterator>, - ) { - let jac = &self.generic_jacobians; - let a = self - .velocity_constraints - .iter_mut() - .map(AnyConstraintMut::TwoBodies); - let b = self - .generic_velocity_constraints - .iter_mut() - .map(AnyConstraintMut::GenericTwoBodies); - #[cfg(feature = "simd-is-enabled")] - let c = self - .simd_velocity_constraints - .iter_mut() - .map(AnyConstraintMut::SimdTwoBodies); - let d = self - .velocity_one_body_constraints - .iter_mut() - .map(AnyConstraintMut::OneBody); - let e = self - .generic_velocity_one_body_constraints - .iter_mut() - .map(AnyConstraintMut::GenericOneBody); - #[cfg(feature = "simd-is-enabled")] - let f = self - .simd_velocity_one_body_constraints - .iter_mut() - .map(AnyConstraintMut::SimdOneBody); - - #[cfg(feature = "simd-is-enabled")] - return (jac, a.chain(b).chain(c).chain(d).chain(e).chain(f)); - - #[cfg(not(feature = "simd-is-enabled"))] - return (jac, a.chain(b).chain(d).chain(e)); - } -} diff --git a/src/dynamics/solver/solver_vel.rs b/src/dynamics/solver/solver_vel.rs deleted file mode 100644 index be4d691..0000000 --- a/src/dynamics/solver/solver_vel.rs +++ /dev/null @@ -1,66 +0,0 @@ -use crate::math::{AngVector, SPATIAL_DIM, Vector}; -use crate::utils::SimdRealCopy; -use na::{DVectorView, DVectorViewMut, Scalar}; -use std::ops::{AddAssign, Sub, SubAssign}; - -#[derive(Copy, Clone, Debug, Default)] -#[repr(C)] -//#[repr(align(64))] -pub struct SolverVel { - // The linear velocity of a solver body. - pub linear: Vector, - // The angular velocity, multiplied by the inverse sqrt angular inertia, of a solver body. - pub angular: AngVector, -} - -impl SolverVel { - pub fn as_slice(&self) -> &[N; SPATIAL_DIM] { - unsafe { std::mem::transmute(self) } - } - - pub fn as_mut_slice(&mut self) -> &mut [N; SPATIAL_DIM] { - unsafe { std::mem::transmute(self) } - } - - pub fn as_vector_slice(&self) -> DVectorView { - DVectorView::from_slice(&self.as_slice()[..], SPATIAL_DIM) - } - - pub fn as_vector_slice_mut(&mut self) -> DVectorViewMut { - DVectorViewMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM) - } -} - -impl SolverVel { - pub fn zero() -> Self { - Self { - linear: na::zero(), - angular: na::zero(), - } - } -} - -impl AddAssign for SolverVel { - fn add_assign(&mut self, rhs: Self) { - self.linear += rhs.linear; - self.angular += rhs.angular; - } -} - -impl SubAssign for SolverVel { - fn sub_assign(&mut self, rhs: Self) { - self.linear -= rhs.linear; - self.angular -= rhs.angular; - } -} - -impl Sub for SolverVel { - type Output = Self; - - fn sub(self, rhs: Self) -> Self { - SolverVel { - linear: self.linear - rhs.linear, - angular: self.angular - rhs.angular, - } - } -} diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 615387c..ef2d186 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -1,19 +1,21 @@ -use super::{JointConstraintTypes, SolverConstraintsSet}; -use crate::dynamics::solver::solver_body::SolverBody; +use crate::dynamics::solver::JointConstraintsSet; +use crate::dynamics::solver::contact_constraint::ContactConstraintsSet; +use crate::dynamics::solver::solver_body::SolverBodies; use crate::dynamics::{ IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, - MultibodyLinkId, RigidBodySet, - solver::{ContactConstraintTypes, SolverVel}, + MultibodyLinkId, RigidBodySet, RigidBodyType, solver::SolverVel, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::Real; use crate::prelude::RigidBodyVelocity; -use crate::utils::SimdAngularInertia; use na::DVector; +use parry::math::{SIMD_WIDTH, Translation}; + +#[cfg(feature = "dim3")] +use crate::dynamics::FrictionModel; pub(crate) struct VelocitySolver { - pub solver_bodies: Vec, - pub solver_vels: Vec>, + pub solver_bodies: SolverBodies, pub solver_vels_increment: Vec>, pub generic_solver_vels: DVector, pub generic_solver_vels_increment: DVector, @@ -23,8 +25,7 @@ pub(crate) struct VelocitySolver { impl VelocitySolver { pub fn new() -> Self { Self { - solver_bodies: Vec::new(), - solver_vels: Vec::new(), + solver_bodies: SolverBodies::default(), solver_vels_increment: Vec::new(), generic_solver_vels: DVector::zeros(0), generic_solver_vels_increment: DVector::zeros(0), @@ -42,16 +43,20 @@ impl VelocitySolver { manifold_indices: &[ContactManifoldIndex], joints_all: &mut [JointGraphEdge], joint_indices: &[JointIndex], - contact_constraints: &mut SolverConstraintsSet, - joint_constraints: &mut SolverConstraintsSet, + contact_constraints: &mut ContactConstraintsSet, + joint_constraints: &mut JointConstraintsSet, + #[cfg(feature = "dim3")] friction_model: FrictionModel, ) { contact_constraints.init( island_id, islands, bodies, + &self.solver_bodies, multibodies, manifolds_all, manifold_indices, + #[cfg(feature = "dim3")] + friction_model, ); joint_constraints.init( @@ -66,6 +71,7 @@ impl VelocitySolver { pub fn init_solver_velocities_and_solver_bodies( &mut self, + total_step_dt: Real, params: &IntegrationParameters, island_id: usize, islands: &IslandManager, @@ -74,17 +80,14 @@ impl VelocitySolver { ) { self.multibody_roots.clear(); self.solver_bodies.clear(); - self.solver_bodies.resize( - islands.active_island(island_id).len(), - SolverBody::default(), - ); + + let aligned_solver_bodies_len = + islands.active_island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH; + self.solver_bodies.resize(aligned_solver_bodies_len); self.solver_vels_increment.clear(); self.solver_vels_increment - .resize(islands.active_island(island_id).len(), SolverVel::zero()); - self.solver_vels.clear(); - self.solver_vels - .resize(islands.active_island(island_id).len(), SolverVel::zero()); + .resize(aligned_solver_bodies_len, SolverVel::zero()); /* * Initialize solver bodies and delta-velocities (`solver_vels_increment`) with external forces (gravity etc): @@ -92,7 +95,7 @@ impl VelocitySolver { */ // Assign solver ids to multibodies, and collect the relevant roots. - // And init solver_vels for rigidb-bodies. + // And init solver_vels for rigid-bodies. let mut multibody_solver_id = 0; for handle in islands.active_island(island_id) { if let Some(link) = multibodies.rigid_body_link(*handle).copied() { @@ -102,32 +105,26 @@ impl VelocitySolver { if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { multibody.solver_id = multibody_solver_id; - multibody_solver_id += multibody.ndofs(); + multibody_solver_id += multibody.ndofs() as u32; self.multibody_roots.push(link); } } else { let rb = &bodies[*handle]; - let solver_vel = &mut self.solver_vels[rb.ids.active_set_offset]; - let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset]; - let solver_body = &mut self.solver_bodies[rb.ids.active_set_offset]; - solver_body.copy_from(rb); + let solver_vel_incr = + &mut self.solver_vels_increment[rb.ids.active_set_offset as usize]; + self.solver_bodies + .copy_from(total_step_dt, rb.ids.active_set_offset as usize, rb); - // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied - // by the square root of the inertia tensor: solver_vel_incr.angular = - rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt; + rb.mprops.effective_world_inv_inertia * rb.forces.torque * params.dt; solver_vel_incr.linear = rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt; - - solver_vel.linear = rb.vels.linvel; - // PERF: can we avoid the call to effective_angular_inertia_sqrt? - solver_vel.angular = rb.mprops.effective_angular_inertia_sqrt() * rb.vels.angvel; } } - // PERF: don’t reallocate at each iteration. - self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id); - self.generic_solver_vels = DVector::zeros(multibody_solver_id); + // TODO PERF: don’t reallocate at each iteration. + self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id as usize); + self.generic_solver_vels = DVector::zeros(multibody_solver_id as usize); // init solver_vels for multibodies. for link in &self.multibody_roots { @@ -139,10 +136,10 @@ impl VelocitySolver { let mut solver_vels_incr = self .generic_solver_vels_increment - .rows_mut(multibody.solver_id, multibody.ndofs()); + .rows_mut(multibody.solver_id as usize, multibody.ndofs()); let mut solver_vels = self .generic_solver_vels - .rows_mut(multibody.solver_id, multibody.ndofs()); + .rows_mut(multibody.solver_id as usize, multibody.ndofs()); solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0); solver_vels.copy_from(&multibody.velocities); @@ -156,14 +153,16 @@ impl VelocitySolver { num_substeps: usize, bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, - contact_constraints: &mut SolverConstraintsSet, - joint_constraints: &mut SolverConstraintsSet, + contact_constraints: &mut ContactConstraintsSet, + joint_constraints: &mut JointConstraintsSet, ) { for substep_id in 0..num_substeps { let is_last_substep = substep_id == num_substeps - 1; + // TODO PERF: could easily use SIMD. for (solver_vels, incr) in self - .solver_vels + .solver_bodies + .vels .iter_mut() .zip(self.solver_vels_increment.iter()) { @@ -174,28 +173,23 @@ impl VelocitySolver { self.generic_solver_vels += &self.generic_solver_vels_increment; /* - * Update & solve constraints. + * Update & solve constraints with bias. */ joint_constraints.update(params, multibodies, &self.solver_bodies); contact_constraints.update(params, substep_id, multibodies, &self.solver_bodies); if params.warmstart_coefficient != 0.0 { - contact_constraints.warmstart(&mut self.solver_vels, &mut self.generic_solver_vels); + // TODO PERF: we could probably figure out a way to avoid this warmstart when + // step_id > 0? Maybe for that to happen `solver_vels` needs to + // represent velocity changes instead of total rigid-boody velocities. + // Need to be careful wrt. multibody and joints too. + contact_constraints + .warmstart(&mut self.solver_bodies, &mut self.generic_solver_vels); } for _ in 0..params.num_internal_pgs_iterations { - joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels); - contact_constraints - .solve_restitution(&mut self.solver_vels, &mut self.generic_solver_vels); - contact_constraints - .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels); - } - - if is_last_substep { - for _ in 0..params.num_additional_friction_iterations { - contact_constraints - .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels); - } + joint_constraints.solve(&mut self.solver_bodies, &mut self.generic_solver_vels); + contact_constraints.solve(&mut self.solver_bodies, &mut self.generic_solver_vels); } /* @@ -206,18 +200,11 @@ impl VelocitySolver { /* * Resolution without bias. */ - if params.num_internal_stabilization_iterations > 0 { - for _ in 0..params.num_internal_stabilization_iterations { - joint_constraints - .solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels); - contact_constraints.solve_restitution_wo_bias( - &mut self.solver_vels, - &mut self.generic_solver_vels, - ); - } - + for _ in 0..params.num_internal_stabilization_iterations { + joint_constraints + .solve_wo_bias(&mut self.solver_bodies, &mut self.generic_solver_vels); contact_constraints - .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels); + .solve_wo_bias(&mut self.solver_bodies, &mut self.generic_solver_vels); } } } @@ -230,21 +217,43 @@ impl VelocitySolver { bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, ) { - // Integrate positions. - for (solver_vels, solver_body) in self.solver_vels.iter().zip(self.solver_bodies.iter_mut()) + for (solver_vels, solver_pose) in self + .solver_bodies + .vels + .iter() + .zip(self.solver_bodies.poses.iter_mut()) { let linvel = solver_vels.linear; - let angvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular); + let angvel = solver_vels.angular; - let mut new_vels = RigidBodyVelocity { linvel, angvel }; - new_vels = new_vels.apply_damping(params.dt, &solver_body.damping); - let new_pos = - new_vels.integrate(params.dt, &solver_body.position, &solver_body.local_com); - solver_body.integrated_vels += new_vels; - solver_body.position = new_pos; - solver_body.world_com = new_pos * solver_body.local_com; + // TODO: should we add a compile flag (or a simulation parameter) + // to disable the rotation linearization? + let new_vels = RigidBodyVelocity { linvel, angvel }; + new_vels.integrate_linearized(params.dt, &mut solver_pose.pose); } + // TODO PERF: SIMD-optimized integration. Works fine, but doesn’t run faster than the scalar + // one (tested on Apple Silicon/Neon, might be worth double-checking on x86_64/SSE2). + // // SAFETY: this assertion ensures the unchecked gathers are sound. + // assert_eq!(self.solver_bodies.len() % SIMD_WIDTH, 0); + // let dt = SimdReal::splat(params.dt); + // for i in (0..self.solver_bodies.len()).step_by(SIMD_WIDTH) { + // let idx = [i, i + 1, i + 2, i + 3]; + // let solver_vels = unsafe { self.solver_bodies.gather_vels_unchecked(idx) }; + // let mut solver_poses = unsafe { self.solver_bodies.gather_poses_unchecked(idx) }; + // // let solver_consts = unsafe { self.solver_bodies.gather_consts_unchecked(idx) }; + // + // let linvel = solver_vels.linear; + // let angvel = solver_poses.ii_sqrt.transform_vector(solver_vels.angular); + // + // let mut new_vels = RigidBodyVelocity { linvel, angvel }; + // // TODO: store the post-damping velocity? + // // new_vels = new_vels.apply_damping(dt, &solver_consts.damping); + // new_vels.integrate_linearized(dt, &mut solver_poses.pose); + // self.solver_bodies + // .scatter_poses_unchecked(idx, solver_poses); + // } + // Integrate multibody positions. for link in &self.multibody_roots { let multibody = multibodies @@ -252,7 +261,7 @@ impl VelocitySolver { .unwrap(); let solver_vels = self .generic_solver_vels - .rows(multibody.solver_id, multibody.ndofs()); + .rows(multibody.solver_id as usize, multibody.ndofs()); multibody.velocities.copy_from(&solver_vels); multibody.integrate(params.dt); // PERF: don’t write back to the rigid-body poses `bodies` before the last step? @@ -267,7 +276,7 @@ impl VelocitySolver { let mut solver_vels_incr = self .generic_solver_vels_increment - .rows_mut(multibody.solver_id, multibody.ndofs()); + .rows_mut(multibody.solver_id as usize, multibody.ndofs()); solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0); } } @@ -276,7 +285,6 @@ impl VelocitySolver { pub fn writeback_bodies( &mut self, params: &IntegrationParameters, - num_substeps: usize, islands: &IslandManager, island_id: usize, bodies: &mut RigidBodySet, @@ -297,32 +305,41 @@ impl VelocitySolver { if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { let solver_vels = self .generic_solver_vels - .rows(multibody.solver_id, multibody.ndofs()); + .rows(multibody.solver_id as usize, multibody.ndofs()); multibody.velocities.copy_from(&solver_vels); } } else { let rb = bodies.index_mut_internal(*handle); - let solver_body = &self.solver_bodies[rb.ids.active_set_offset]; - let solver_vels = &self.solver_vels[rb.ids.active_set_offset]; + let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_offset as usize]; + let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_offset as usize]; - let dangvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular); + let dangvel = solver_vels.angular; let mut new_vels = RigidBodyVelocity { linvel: solver_vels.linear, angvel: dangvel, }; - new_vels = new_vels.apply_damping(params.dt, &solver_body.damping); + new_vels = new_vels.apply_damping(params.dt, &rb.damping); - // NOTE: using integrated_vels instead of interpolation is interesting for - // high angular velocities. However, it is a bit inexact due to the - // solver integrating at intermediate sub-steps. Should we just switch - // to interpolation? - rb.integrated_vels.linvel = - solver_body.integrated_vels.linvel / num_substeps as Real; - rb.integrated_vels.angvel = - solver_body.integrated_vels.angvel / num_substeps as Real; rb.vels = new_vels; - rb.pos.next_position = solver_body.position; + + // NOTE: if it’s a position-based kinematic body, don’t writeback as we want + // to preserve exactly the value given by the user (it might not be exactly + // equal to the integrated position because of rounding errors). + if rb.body_type != RigidBodyType::KinematicPositionBased { + rb.pos.next_position = + solver_poses.pose * Translation::from(-rb.mprops.local_mprops.local_com); + } + + if rb.ccd.ccd_enabled { + // TODO: Is storing this still necessary instead of just recomputing it + // during CCD? + rb.ccd_vels = rb + .pos + .interpolate_velocity(params.inv_dt(), rb.local_center_of_mass()); + } else { + rb.ccd_vels = RigidBodyVelocity::zero(); + } } } } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 3f569b2..a7757c9 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -1,15 +1,15 @@ +#[cfg(doc)] +use super::Collider; +use super::CollisionEvent; use crate::dynamics::{RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderHandle, ColliderSet, Contact, ContactManifold}; use crate::math::{Point, Real, TangentImpulse, Vector}; use crate::pipeline::EventHandler; use crate::prelude::CollisionEventFlags; +use crate::utils::SimdRealCopy; +use parry::math::{SIMD_WIDTH, SimdReal}; use parry::query::ContactManifoldsWorkspace; -use super::CollisionEvent; - -#[cfg(doc)] -use super::Collider; - bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, PartialEq, Eq, Debug)] @@ -42,6 +42,9 @@ pub struct ContactData { pub warmstart_impulse: Real, /// The friction impulse retained for warmstarting the next simulation step. pub warmstart_tangent_impulse: TangentImpulse, + /// The twist impulse retained for warmstarting the next simulation step. + #[cfg(feature = "dim3")] + pub warmstart_twist_impulse: Real, } impl Default for ContactData { @@ -51,6 +54,8 @@ impl Default for ContactData { tangent_impulse: na::zero(), warmstart_impulse: 0.0, warmstart_tangent_impulse: na::zero(), + #[cfg(feature = "dim3")] + warmstart_twist_impulse: 0.0, } } } @@ -286,45 +291,237 @@ pub struct ContactManifoldData { pub user_data: u32, } +/// A single solver contact. +pub type SolverContact = SolverContactGeneric; +/// A group of `SIMD_WIDTH` solver contacts stored in SoA fashion for SIMD optimizations. +pub type SimdSolverContact = SolverContactGeneric; + /// A contact seen by the constraints solver for computing forces. #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct SolverContact { - /// The index of the manifold contact used to generate this solver contact. - pub(crate) contact_id: u8, +#[cfg_attr( + feature = "serde-serialize", + serde(bound(serialize = "N: serde::Serialize, [u32; LANES]: serde::Serialize")) +)] +#[cfg_attr( + feature = "serde-serialize", + serde(bound( + deserialize = "N: serde::Deserialize<'de>, [u32; LANES]: serde::Deserialize<'de>" + )) +)] +#[repr(C)] +#[repr(align(16))] +pub struct SolverContactGeneric { + // IMPORTANT: don’t change the fields unless `SimdSolverContactRepr` is also changed. + // + // TOTAL: 11/14 = 3*4/4*4-1 /// The contact point in world-space. - pub point: Point, + pub point: Point, // 2/3 /// The distance between the two original contacts points along the contact normal. /// If negative, this is measures the penetration depth. - pub dist: Real, + pub dist: N, // 1/1 /// The effective friction coefficient at this contact point. - pub friction: Real, + pub friction: N, // 1/1 /// The effective restitution coefficient at this contact point. - pub restitution: Real, + pub restitution: N, // 1/1 /// The desired tangent relative velocity at the contact point. /// /// This is set to zero by default. Set to a non-zero value to /// simulate, e.g., conveyor belts. - pub tangent_velocity: Vector, - /// Whether or not this contact existed during the last timestep. - pub is_new: bool, + pub tangent_velocity: Vector, // 2/3 /// Impulse used to warmstart the solve for the normal constraint. - pub warmstart_impulse: Real, + pub warmstart_impulse: N, // 1/1 /// Impulse used to warmstart the solve for the friction constraints. - pub warmstart_tangent_impulse: TangentImpulse, + pub warmstart_tangent_impulse: TangentImpulse, // 1/2 + /// Impulse used to warmstart the solve for the twist friction constraints. + pub warmstart_twist_impulse: N, // 1/1 + /// Whether this contact existed during the last timestep. + /// + /// A value of 0.0 means `false` and `1.0` means `true`. + /// This isn’t a bool for optimizations purpose with SIMD. + pub is_new: N, // 1/1 + /// The index of the manifold contact used to generate this solver contact. + pub(crate) contact_id: [u32; LANES], // 1/1 + #[cfg(feature = "dim3")] + pub(crate) padding: [N; 1], +} + +#[repr(C)] +#[repr(align(16))] +pub struct SimdSolverContactRepr { + data0: SimdReal, + data1: SimdReal, + data2: SimdReal, + #[cfg(feature = "dim3")] + data3: SimdReal, +} + +// NOTE: if these assertion fail with a weird "0 - 1 would overflow" error, it means the equality doesn’t hold. +static_assertions::const_assert_eq!( + align_of::(), + align_of::() +); +#[cfg(feature = "simd-is-enabled")] +static_assertions::assert_eq_size!(SimdSolverContactRepr, SolverContact); +static_assertions::const_assert_eq!( + align_of::(), + align_of::<[SolverContact; SIMD_WIDTH]>() +); +#[cfg(feature = "simd-is-enabled")] +static_assertions::assert_eq_size!(SimdSolverContact, [SolverContact; SIMD_WIDTH]); + +impl SimdSolverContact { + #[cfg(not(feature = "simd-is-enabled"))] + pub unsafe fn gather_unchecked(contacts: &[&[SolverContact]; SIMD_WIDTH], k: usize) -> Self { + contacts[0][k] + } + + #[cfg(feature = "simd-is-enabled")] + pub unsafe fn gather_unchecked(contacts: &[&[SolverContact]; SIMD_WIDTH], k: usize) -> Self { + // TODO PERF: double-check that the compiler is using simd loads and + // isn’t generating useless copies. + + let data_repr: &[&[SimdSolverContactRepr]; SIMD_WIDTH] = + unsafe { std::mem::transmute(contacts) }; + + /* NOTE: this is a manual NEON implementation. To compare with what the compiler generates with `wide`. + unsafe { + use std::arch::aarch64::*; + + assert!(k < SIMD_WIDTH); + + // Fetch. + let aos0_0 = vld1q_f32(&data_repr[0][k].data0.0 as *const _ as *const f32); + let aos0_1 = vld1q_f32(&data_repr[1][k].data0.0 as *const _ as *const f32); + let aos0_2 = vld1q_f32(&data_repr[2][k].data0.0 as *const _ as *const f32); + let aos0_3 = vld1q_f32(&data_repr[3][k].data0.0 as *const _ as *const f32); + + let aos1_0 = vld1q_f32(&data_repr[0][k].data1.0 as *const _ as *const f32); + let aos1_1 = vld1q_f32(&data_repr[1][k].data1.0 as *const _ as *const f32); + let aos1_2 = vld1q_f32(&data_repr[2][k].data1.0 as *const _ as *const f32); + let aos1_3 = vld1q_f32(&data_repr[3][k].data1.0 as *const _ as *const f32); + + let aos2_0 = vld1q_f32(&data_repr[0][k].data2.0 as *const _ as *const f32); + let aos2_1 = vld1q_f32(&data_repr[1][k].data2.0 as *const _ as *const f32); + let aos2_2 = vld1q_f32(&data_repr[2][k].data2.0 as *const _ as *const f32); + let aos2_3 = vld1q_f32(&data_repr[3][k].data2.0 as *const _ as *const f32); + + // Transpose. + let a = vzip1q_f32(aos0_0, aos0_2); + let b = vzip1q_f32(aos0_1, aos0_3); + let c = vzip2q_f32(aos0_0, aos0_2); + let d = vzip2q_f32(aos0_1, aos0_3); + let soa0_0 = vzip1q_f32(a, b); + let soa0_1 = vzip2q_f32(a, b); + let soa0_2 = vzip1q_f32(c, d); + let soa0_3 = vzip2q_f32(c, d); + + let a = vzip1q_f32(aos1_0, aos1_2); + let b = vzip1q_f32(aos1_1, aos1_3); + let c = vzip2q_f32(aos1_0, aos1_2); + let d = vzip2q_f32(aos1_1, aos1_3); + let soa1_0 = vzip1q_f32(a, b); + let soa1_1 = vzip2q_f32(a, b); + let soa1_2 = vzip1q_f32(c, d); + let soa1_3 = vzip2q_f32(c, d); + + let a = vzip1q_f32(aos2_0, aos2_2); + let b = vzip1q_f32(aos2_1, aos2_3); + let c = vzip2q_f32(aos2_0, aos2_2); + let d = vzip2q_f32(aos2_1, aos2_3); + let soa2_0 = vzip1q_f32(a, b); + let soa2_1 = vzip2q_f32(a, b); + let soa2_2 = vzip1q_f32(c, d); + let soa2_3 = vzip2q_f32(c, d); + + // Return. + std::mem::transmute([ + soa0_0, soa0_1, soa0_2, soa0_3, soa1_0, soa1_1, soa1_2, soa1_3, soa2_0, soa2_1, + soa2_2, soa2_3, + ]) + } + */ + + let aos0 = [ + unsafe { data_repr[0].get_unchecked(k).data0.0 }, + unsafe { data_repr[1].get_unchecked(k).data0.0 }, + unsafe { data_repr[2].get_unchecked(k).data0.0 }, + unsafe { data_repr[3].get_unchecked(k).data0.0 }, + ]; + let aos1 = [ + unsafe { data_repr[0].get_unchecked(k).data1.0 }, + unsafe { data_repr[1].get_unchecked(k).data1.0 }, + unsafe { data_repr[2].get_unchecked(k).data1.0 }, + unsafe { data_repr[3].get_unchecked(k).data1.0 }, + ]; + let aos2 = [ + unsafe { data_repr[0].get_unchecked(k).data2.0 }, + unsafe { data_repr[1].get_unchecked(k).data2.0 }, + unsafe { data_repr[2].get_unchecked(k).data2.0 }, + unsafe { data_repr[3].get_unchecked(k).data2.0 }, + ]; + #[cfg(feature = "dim3")] + let aos3 = [ + unsafe { data_repr[0].get_unchecked(k).data3.0 }, + unsafe { data_repr[1].get_unchecked(k).data3.0 }, + unsafe { data_repr[2].get_unchecked(k).data3.0 }, + unsafe { data_repr[3].get_unchecked(k).data3.0 }, + ]; + + use crate::utils::transmute_to_wide; + let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0)); + let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1)); + let soa2 = wide::f32x4::transpose(transmute_to_wide(aos2)); + #[cfg(feature = "dim3")] + let soa3 = wide::f32x4::transpose(transmute_to_wide(aos3)); + + #[cfg(feature = "dim2")] + return unsafe { + std::mem::transmute::<[[wide::f32x4; 4]; 3], SolverContactGeneric>([ + soa0, soa1, soa2, + ]) + }; + #[cfg(feature = "dim3")] + return unsafe { + std::mem::transmute::<[[wide::f32x4; 4]; 4], SolverContactGeneric>([ + soa0, soa1, soa2, soa3, + ]) + }; + } +} + +#[cfg(feature = "simd-is-enabled")] +impl SimdSolverContact { + /// Should we treat this contact as a bouncy contact? + /// If `true`, use [`Self::restitution`]. + pub fn is_bouncy(&self) -> SimdReal { + use na::{SimdPartialOrd, SimdValue}; + + let one = SimdReal::splat(1.0); + let zero = SimdReal::splat(0.0); + + // Treat new collisions as bouncing at first, unless we have zero restitution. + let if_new = one.select(self.restitution.simd_gt(zero), zero); + + // If the contact is still here one step later, it is now a resting contact. + // The exception is very high restitutions, which can never rest + let if_not_new = one.select(self.restitution.simd_ge(one), zero); + + if_new.select(self.is_new.simd_ne(zero), if_not_new) + } } impl SolverContact { /// Should we treat this contact as a bouncy contact? /// If `true`, use [`Self::restitution`]. - pub fn is_bouncy(&self) -> bool { - if self.is_new { + pub fn is_bouncy(&self) -> Real { + if self.is_new != 0.0 { // Treat new collisions as bouncing at first, unless we have zero restitution. - self.restitution > 0.0 + (self.restitution > 0.0) as u32 as Real } else { // If the contact is still here one step later, it is now a resting contact. // The exception is very high restitutions, which can never rest - self.restitution >= 1.0 + (self.restitution >= 1.0) as u32 as Real } } } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 296e71b..0314c10 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -6,7 +6,8 @@ pub use self::collider::{Collider, ColliderBuilder}; pub use self::collider_components::*; pub use self::collider_set::ColliderSet; pub use self::contact_pair::{ - ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags, + ContactData, ContactManifoldData, ContactPair, IntersectionPair, SimdSolverContact, + SolverContact, SolverFlags, }; pub use self::interaction_graph::{ ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex, diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 202612c..ce61b76 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -21,7 +21,7 @@ use crate::pipeline::{ use crate::prelude::{CollisionEventFlags, MultibodyJointSet}; use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; use parry::utils::IsometryOpt; -use std::collections::HashMap; +use parry::utils::hashmap::HashMap; use std::sync::Arc; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -281,8 +281,8 @@ impl NarrowPhase { // TODO: avoid these hash-maps. // They are necessary to handle the swap-remove done internally // by the contact/intersection graphs when a node is removed. - let mut prox_id_remap = HashMap::new(); - let mut contact_id_remap = HashMap::new(); + let mut prox_id_remap = HashMap::default(); + let mut contact_id_remap = HashMap::default(); for collider in removed_colliders { // NOTE: if the collider does not have any graph indices currently, there is nothing @@ -1010,15 +1010,21 @@ impl NarrowPhase { let effective_point = na::center(&world_pt1, &world_pt2); let solver_contact = SolverContact { - contact_id: contact_id as u8, + contact_id: [contact_id as u32], point: effective_point, dist: effective_contact_dist, friction, restitution, tangent_velocity: Vector::zeros(), - is_new: contact.data.impulse == 0.0, + is_new: (contact.data.impulse == 0.0) as u32 as Real, warmstart_impulse: contact.data.warmstart_impulse, warmstart_tangent_impulse: contact.data.warmstart_tangent_impulse, + #[cfg(feature = "dim2")] + warmstart_twist_impulse: na::zero(), + #[cfg(feature = "dim3")] + warmstart_twist_impulse: contact.data.warmstart_twist_impulse, + #[cfg(feature = "dim3")] + padding: Default::default(), }; manifold.data.solver_contacts.push(solver_contact); diff --git a/src/lib.rs b/src/lib.rs index 9e7eaba..331e8c8 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -15,6 +15,7 @@ #![allow(clippy::too_many_arguments)] #![allow(clippy::needless_range_loop)] // TODO: remove this? I find that in the math code using indices adds clarity. #![allow(clippy::module_inception)] +#![cfg_attr(feature = "simd-nightly", feature(portable_simd))] #[cfg(all(feature = "dim2", feature = "f32"))] pub extern crate parry2d as parry; @@ -53,16 +54,41 @@ macro_rules! enable_flush_to_zero( } ); -#[cfg(feature = "simd-is-enabled")] macro_rules! gather( ($callback: expr) => { { #[inline(always)] #[allow(dead_code)] + #[cfg(not(feature = "simd-is-enabled"))] + fn create_arr(mut callback: impl FnMut(usize) -> T) -> T { + callback(0usize) + } + + #[inline(always)] + #[allow(dead_code)] + #[cfg(feature = "simd-is-enabled")] fn create_arr(mut callback: impl FnMut(usize) -> T) -> [T; SIMD_WIDTH] { [callback(0usize), callback(1usize), callback(2usize), callback(3usize)] } + + create_arr($callback) + } + } +); + +macro_rules! array( + ($callback: expr) => { + { + #[inline(always)] + #[allow(dead_code)] + fn create_arr(mut callback: impl FnMut(usize) -> T) -> [T; SIMD_WIDTH] { + #[cfg(not(feature = "simd-is-enabled"))] + return [callback(0usize)]; + #[cfg(feature = "simd-is-enabled")] + return [callback(0usize), callback(1usize), callback(2usize), callback(3usize)]; + } + create_arr($callback) } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index a47dcf1..b507820 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -7,7 +7,7 @@ use crate::dynamics::IslandSolver; use crate::dynamics::JointGraphEdge; use crate::dynamics::{ CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, - RigidBodyChanges, RigidBodyPosition, RigidBodyType, + RigidBodyChanges, RigidBodyType, }; use crate::geometry::{ BroadPhaseBvh, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, @@ -215,11 +215,12 @@ impl PhysicsPipeline { self.counters.stages.island_construction_time.pause(); self.counters.stages.update_time.resume(); - for handle in islands.active_dynamic_bodies() { + for handle in islands.active_bodies() { // TODO: should that be moved to the solver (just like we moved // the multibody dynamics update) since it depends on dt? let rb = bodies.index_mut_internal(*handle); - rb.mprops.update_world_mass_properties(&rb.pos.position); + rb.mprops + .update_world_mass_properties(rb.body_type, &rb.pos.position); let effective_mass = rb.mprops.effective_mass(); rb.forces .compute_effective_force_and_torque(gravity, &effective_mass); @@ -370,8 +371,8 @@ impl PhysicsPipeline { modified_colliders: &mut ModifiedColliders, ) { // Set the rigid-bodies and kinematic bodies to their final position. - for handle in islands.iter_active_bodies() { - let rb = bodies.index_mut_internal(handle); + for handle in islands.active_bodies() { + let rb = bodies.index_mut_internal(*handle); rb.pos.position = rb.pos.next_position; rb.colliders .update_positions(colliders, modified_colliders, &rb.pos.position); @@ -389,7 +390,8 @@ impl PhysicsPipeline { // located before the island computation because we test the velocity // there to determine if this kinematic body should wake-up dynamic // bodies it is touching. - for handle in islands.active_kinematic_bodies() { + for handle in islands.active_bodies() { + // TODO PERF: only iterate on kinematic position-based bodies let rb = bodies.index_mut_internal(*handle); match rb.body_type { @@ -399,14 +401,7 @@ impl PhysicsPipeline { &rb.mprops.local_mprops.local_com, ); } - RigidBodyType::KinematicVelocityBased => { - let new_pos = rb.vels.integrate( - integration_parameters.dt, - &rb.pos.position, - &rb.mprops.local_mprops.local_com, - ); - rb.pos = RigidBodyPosition::from(new_pos); - } + RigidBodyType::KinematicVelocityBased => {} _ => {} } } @@ -661,9 +656,10 @@ impl PhysicsPipeline { // at the beginning of the next timestep) for bodies that were // not modified by the user in the mean time. self.counters.stages.update_time.resume(); - for handle in islands.active_dynamic_bodies() { + for handle in islands.active_bodies() { let rb = bodies.index_mut_internal(*handle); - rb.mprops.update_world_mass_properties(&rb.pos.position); + rb.mprops + .update_world_mass_properties(rb.body_type, &rb.pos.position); } self.counters.stages.update_time.pause(); @@ -945,8 +941,8 @@ mod test { // Expect gravity to be applied on second step after switching to Dynamic assert!(h_y < 0.0); - // Expect body to now be in active_dynamic_set - assert!(islands.active_dynamic_set.contains(&h)); + // Expect body to now be in active_set + assert!(islands.active_set.contains(&h)); } #[test] diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 2474bb3..0ee8144 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -46,7 +46,7 @@ pub struct QueryPipelineMut<'a> { impl QueryPipelineMut<'_> { /// Downgrades the mutable reference to an immutable reference. - pub fn as_ref(&self) -> QueryPipeline { + pub fn as_ref(&self) -> QueryPipeline<'_> { QueryPipeline { dispatcher: self.dispatcher, bvh: self.bvh, diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index c326d6a..e39c066 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -1,6 +1,6 @@ use crate::dynamics::{ ImpulseJointSet, IslandManager, JointEnabled, MultibodyJointSet, RigidBodyChanges, - RigidBodyHandle, RigidBodySet, RigidBodyType, + RigidBodyHandle, RigidBodySet, }; use crate::geometry::{ ColliderChanges, ColliderEnabled, ColliderHandle, ColliderPosition, ColliderSet, @@ -52,8 +52,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( modified_colliders: &mut ModifiedColliders, ) { enum FinalAction { - UpdateActiveKinematicSetId(usize), - UpdateActiveDynamicSetId(usize), RemoveFromIsland, } @@ -75,62 +73,16 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( // The body's status changed. We need to make sure // it is on the correct active set. if let Some(islands) = islands.as_deref_mut() { - if changes.contains(RigidBodyChanges::TYPE) { - match rb.body_type { - RigidBodyType::Dynamic => { - // Remove from the active kinematic set if it was there. - if islands.active_kinematic_set.get(ids.active_set_id) - == Some(handle) - { - islands.active_kinematic_set.swap_remove(ids.active_set_id); - final_action = Some(FinalAction::UpdateActiveKinematicSetId( - ids.active_set_id, - )); - } - } - RigidBodyType::KinematicVelocityBased - | RigidBodyType::KinematicPositionBased => { - // Remove from the active dynamic set if it was there. - if islands.active_dynamic_set.get(ids.active_set_id) == Some(handle) - { - islands.active_dynamic_set.swap_remove(ids.active_set_id); - final_action = Some(FinalAction::UpdateActiveDynamicSetId( - ids.active_set_id, - )); - } - - // Add to the active kinematic set. - if islands.active_kinematic_set.get(ids.active_set_id) - != Some(handle) - { - ids.active_set_id = islands.active_kinematic_set.len(); - islands.active_kinematic_set.push(*handle); - } - } - RigidBodyType::Fixed => {} - } - } - - // Update the active kinematic set. - if (changes.contains(RigidBodyChanges::POSITION) - || changes.contains(RigidBodyChanges::COLLIDERS)) - && rb.is_kinematic() - && islands.active_kinematic_set.get(ids.active_set_id) != Some(handle) - { - ids.active_set_id = islands.active_kinematic_set.len(); - islands.active_kinematic_set.push(*handle); - } - // Push the body to the active set if it is not inside the active set yet, and - // is either not longer sleeping or became dynamic. + // is not longer sleeping or became dynamic. if (changes.contains(RigidBodyChanges::SLEEP) || changes.contains(RigidBodyChanges::TYPE)) && rb.is_enabled() && !rb.activation.sleeping // May happen if the body was put to sleep manually. - && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. - && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle) + && rb.is_dynamic_or_kinematic() // Only dynamic bodies are in the active dynamic set. + && islands.active_set.get(ids.active_set_id) != Some(handle) { - ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. - islands.active_dynamic_set.push(*handle); + ids.active_set_id = islands.active_set.len(); // This will handle the case where the activation_channel contains duplicates. + islands.active_set.push(*handle); } } } @@ -200,6 +152,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( rb.mprops.recompute_mass_properties_from_colliders( colliders, &rb.colliders, + rb.body_type, &rb.pos.position, ); } @@ -216,18 +169,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( let ids = rb.ids; islands.rigid_body_removed(*handle, &ids, bodies); } - FinalAction::UpdateActiveKinematicSetId(id) => { - let active_set = &mut islands.active_kinematic_set; - if id < active_set.len() { - bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; - } - } - FinalAction::UpdateActiveDynamicSetId(id) => { - let active_set = &mut islands.active_dynamic_set; - if id < active_set.len() { - bodies.index_mut_internal(active_set[id]).ids.active_set_id = id; - } - } }; } } diff --git a/src/utils.rs b/src/utils.rs index 9917cb5..371d2d9 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,18 +1,17 @@ //! Miscellaneous utilities. +use crate::math::Real; use na::{ Matrix1, Matrix2, Matrix3, RowVector2, Scalar, SimdRealField, UnitComplex, UnitQuaternion, Vector1, Vector2, Vector3, }; -use num::Zero; -use simba::simd::SimdValue; +use parry::utils::SdpMatrix3; use std::ops::IndexMut; -use parry::utils::SdpMatrix3; -use { - crate::math::{Real, SimdReal}, - na::SimdPartialOrd, - num::One, +#[cfg(feature = "simd-is-enabled")] +use crate::{ + math::{SIMD_WIDTH, SimdReal}, + num::Zero, }; /// The trait for real numbers used by Rapier. @@ -20,6 +19,7 @@ use { /// This includes `f32`, `f64` and their related SIMD types. pub trait SimdRealCopy: SimdRealField + Copy {} impl SimdRealCopy for Real {} +#[cfg(feature = "simd-is-enabled")] impl SimdRealCopy for SimdReal {} const INV_EPSILON: Real = 1.0e-20; @@ -84,6 +84,7 @@ impl> SimdSign> for Vector3 { } } +#[cfg(feature = "simd-is-enabled")] impl SimdSign for SimdReal { fn copy_sign_to(self, to: SimdReal) -> SimdReal { to.simd_copysign(self) @@ -198,6 +199,7 @@ impl SimdCrossMatrix for Real { } } +#[cfg(feature = "simd-is-enabled")] impl SimdCrossMatrix for SimdReal { type CrossMat = Matrix2; type CrossMatTr = Matrix2; @@ -287,6 +289,7 @@ impl SimdDot for Vector1 { } } +#[cfg(feature = "simd-is-enabled")] impl SimdCross> for Vector3 { type Result = Vector3; @@ -295,6 +298,7 @@ impl SimdCross> for Vector3 { } } +#[cfg(feature = "simd-is-enabled")] impl SimdCross> for SimdReal { type Result = Vector2; @@ -303,6 +307,7 @@ impl SimdCross> for SimdReal { } } +#[cfg(feature = "simd-is-enabled")] impl SimdCross> for Vector2 { type Result = SimdReal; @@ -354,7 +359,6 @@ pub(crate) trait SimdAngularInertia { type AngMatrix; fn inverse(&self) -> Self; fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector; - fn squared(&self) -> Self; fn into_matrix(self) -> Self::AngMatrix; } @@ -370,18 +374,14 @@ impl SimdAngularInertia for N { pt * *self } - fn squared(&self) -> N { - *self * *self - } - fn into_matrix(self) -> Self::AngMatrix { self } } -impl SimdAngularInertia for SdpMatrix3 { - type AngVector = Vector3; - type AngMatrix = Matrix3; +impl SimdAngularInertia for SdpMatrix3 { + type AngVector = Vector3; + type AngMatrix = Matrix3; fn inverse(&self) -> Self { let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23; @@ -405,18 +405,7 @@ impl SimdAngularInertia for SdpMatrix3 { } } - fn squared(&self) -> Self { - SdpMatrix3 { - m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13, - m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23, - m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33, - m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23, - m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33, - m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33, - } - } - - fn transform_vector(&self, v: Vector3) -> Vector3 { + fn transform_vector(&self, v: Vector3) -> Vector3 { let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z; @@ -424,61 +413,7 @@ impl SimdAngularInertia for SdpMatrix3 { } #[rustfmt::skip] - fn into_matrix(self) -> Matrix3 { - Matrix3::new( - self.m11, self.m12, self.m13, - self.m12, self.m22, self.m23, - self.m13, self.m23, self.m33, - ) - } -} - -impl SimdAngularInertia for SdpMatrix3 { - type AngVector = Vector3; - type AngMatrix = Matrix3; - - fn inverse(&self) -> Self { - let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23; - let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23; - let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22; - - let determinant = - self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22; - - let zero = ::zero(); - let is_zero = determinant.simd_eq(zero); - let inv_det = (::one() / determinant).select(is_zero, zero); - - SdpMatrix3 { - m11: minor_m12_m23 * inv_det, - m12: -minor_m11_m23 * inv_det, - m13: minor_m11_m22 * inv_det, - m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det, - m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det, - m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det, - } - } - - fn transform_vector(&self, v: Vector3) -> Vector3 { - let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; - let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; - let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z; - Vector3::new(x, y, z) - } - - fn squared(&self) -> Self { - SdpMatrix3 { - m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13, - m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23, - m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33, - m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23, - m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33, - m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33, - } - } - - #[rustfmt::skip] - fn into_matrix(self) -> Matrix3 { + fn into_matrix(self) -> Matrix3 { Matrix3::new( self.m11, self.m12, self.m13, self.m12, self.m22, self.m23, @@ -664,6 +599,18 @@ pub fn smallest_abs_diff_between_angles(a: N, b: N) -> N { s_err.select(s_err_is_smallest, s_err_complement) } +#[cfg(feature = "simd-nightly")] +#[inline(always)] +pub(crate) fn transmute_to_wide(val: [std::simd::f32x4; SIMD_WIDTH]) -> [wide::f32x4; SIMD_WIDTH] { + unsafe { std::mem::transmute(val) } +} + +#[cfg(feature = "simd-stable")] +#[inline(always)] +pub(crate) fn transmute_to_wide(val: [wide::f32x4; SIMD_WIDTH]) -> [wide::f32x4; SIMD_WIDTH] { + val +} + /// Helpers around serialization. #[cfg(feature = "serde-serialize")] pub mod serde { diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs deleted file mode 100644 index bd6aa13..0000000 --- a/src_testbed/box2d_backend.rs +++ /dev/null @@ -1,250 +0,0 @@ -use std::collections::HashMap; - -use na::{Isometry2, Vector2}; -use rapier::counters::Counters; -use rapier::dynamics::{ImpulseJointSet, IntegrationParameters, RigidBodyHandle, RigidBodySet}; -use rapier::geometry::{Collider, ColliderSet}; -use std::f32; - -use wrapped2d::b2; -// use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef}; -use wrapped2d::user_data::NoUserData; - -fn na_vec_to_b2_vec(v: Vector2) -> b2::Vec2 { - b2::Vec2 { x: v.x, y: v.y } -} - -fn b2_vec_to_na_vec(v: b2::Vec2) -> Vector2 { - Vector2::new(v.x, v.y) -} - -fn b2_transform_to_na_isometry(v: b2::Transform) -> Isometry2 { - Isometry2::new(b2_vec_to_na_vec(v.pos), v.rot.angle()) -} - -pub struct Box2dWorld { - world: b2::World, - rapier2box2d: HashMap, -} - -impl Box2dWorld { - #[profiling::function] - pub fn from_rapier( - gravity: Vector2, - bodies: &RigidBodySet, - colliders: &ColliderSet, - impulse_joints: &ImpulseJointSet, - ) -> Self { - let mut world = b2::World::new(&na_vec_to_b2_vec(gravity)); - world.set_continuous_physics(bodies.iter().any(|b| b.1.is_ccd_enabled())); - - let mut res = Box2dWorld { - world, - rapier2box2d: HashMap::new(), - }; - - res.insert_bodies(bodies); - res.insert_colliders(colliders); - res.insert_joints(impulse_joints); - res - } - - fn insert_bodies(&mut self, bodies: &RigidBodySet) { - for (handle, body) in bodies.iter() { - let body_type = if !body.is_dynamic() { - b2::BodyType::Static - } else { - b2::BodyType::Dynamic - }; - - let linear_damping = 0.0; - let angular_damping = 0.0; - - // if let Some(rb) = body.downcast_ref::>() { - // linear_damping = rb.linear_damping(); - // angular_damping = rb.angular_damping(); - // } else { - // linear_damping = 0.0; - // angular_damping = 0.0; - // } - - let def = b2::BodyDef { - body_type, - position: na_vec_to_b2_vec(body.position().translation.vector), - angle: body.position().rotation.angle(), - linear_velocity: na_vec_to_b2_vec(*body.linvel()), - angular_velocity: body.angvel(), - linear_damping, - angular_damping, - bullet: body.is_ccd_enabled(), - ..b2::BodyDef::new() - }; - let b2_handle = self.world.create_body(&def); - self.rapier2box2d.insert(handle, b2_handle); - } - } - - fn insert_colliders(&mut self, colliders: &ColliderSet) { - for (_, collider) in colliders.iter() { - if let Some(parent) = collider.parent() { - let b2_body_handle = self.rapier2box2d[&parent]; - let mut b2_body = self.world.body_mut(b2_body_handle); - Self::create_fixture(&collider, &mut *b2_body); - } - } - } - - fn insert_joints(&mut self, _impulse_joints: &ImpulseJointSet) { - /* - for joint in impulse_joints.iter() { - let body_a = self.rapier2box2d[&joint.1.body1]; - let body_b = self.rapier2box2d[&joint.1.body2]; - - match &joint.1.params { - JointParams::BallJoint(params) => { - let def = RevoluteJointDef { - body_a, - body_b, - collide_connected: true, - local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords), - local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords), - reference_angle: 0.0, - enable_limit: false, - lower_angle: 0.0, - upper_angle: 0.0, - enable_motor: false, - motor_speed: 0.0, - max_motor_torque: 0.0, - }; - - self.world.create_joint(&def); - } - JointParams::FixedJoint(params) => { - let def = WeldJointDef { - body_a, - body_b, - collide_connected: true, - local_anchor_a: na_vec_to_b2_vec(params.local_frame1.translation.vector), - local_anchor_b: na_vec_to_b2_vec(params.local_frame2.translation.vector), - reference_angle: 0.0, - frequency: 0.0, - damping_ratio: 0.0, - }; - - self.world.create_joint(&def); - } - JointParams::PrismaticJoint(params) => { - let def = PrismaticJointDef { - body_a, - body_b, - collide_connected: true, - local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords), - local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords), - local_axis_a: na_vec_to_b2_vec(params.local_axis1().into_inner()), - reference_angle: 0.0, - enable_limit: params.limits_enabled, - lower_translation: params.limits[0], - upper_translation: params.limits[1], - enable_motor: false, - max_motor_force: 0.0, - motor_speed: 0.0, - }; - - self.world.create_joint(&def); - } - } - } - - */ - } - - fn create_fixture(collider: &Collider, body: &mut b2::MetaBody) { - let center = na_vec_to_b2_vec( - collider - .position_wrt_parent() - .copied() - .unwrap_or(na::one()) - .translation - .vector, - ); - let mut fixture_def = b2::FixtureDef::new(); - - fixture_def.restitution = collider.material().restitution; - fixture_def.friction = collider.material().friction; - fixture_def.density = collider.density(); - fixture_def.is_sensor = collider.is_sensor(); - fixture_def.filter = b2::Filter::new(); - - let shape = collider.shape(); - - if let Some(b) = shape.as_ball() { - let mut b2_shape = b2::CircleShape::new(); - b2_shape.set_radius(b.radius); - b2_shape.set_position(center); - body.create_fixture(&b2_shape, &mut fixture_def); - } else if let Some(p) = shape.as_convex_polygon() { - let vertices: Vec<_> = p - .points() - .iter() - .map(|p| na_vec_to_b2_vec(p.coords)) - .collect(); - let b2_shape = b2::PolygonShape::new_with(&vertices); - body.create_fixture(&b2_shape, &mut fixture_def); - } else if let Some(c) = shape.as_cuboid() { - let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y); - body.create_fixture(&b2_shape, &mut fixture_def); - // } else if let Some(polygon) = shape.as_polygon() { - // let points: Vec<_> = poly - // .vertices() - // .iter() - // .map(|p| collider.position_wrt_parent() * p) - // .map(|p| na_vec_to_b2_vec(p.coords)) - // .collect(); - // let b2_shape = b2::PolygonShape::new_with(&points); - // body.create_fixture(&b2_shape, &mut fixture_def); - } else if let Some(heightfield) = shape.as_heightfield() { - let mut segments = heightfield.segments(); - let seg1 = segments.next().unwrap(); - let mut vertices = vec![ - na_vec_to_b2_vec(seg1.a.coords), - na_vec_to_b2_vec(seg1.b.coords), - ]; - - // TODO: this will not handle holes properly. - segments.for_each(|seg| { - vertices.push(na_vec_to_b2_vec(seg.b.coords)); - }); - - let b2_shape = b2::ChainShape::new_chain(&vertices); - body.create_fixture(&b2_shape, &mut fixture_def); - } else { - eprintln!("Creating a shape unknown to the Box2d backend."); - } - } - - #[profiling::function] - pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) { - counters.step_started(); - self.world - .step(params.dt, params.num_solver_iterations.get() as i32, 1); - counters.step_completed(); - } - - #[profiling::function] - pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) { - for (handle, body) in bodies.iter_mut() { - if let Some(pb2_handle) = self.rapier2box2d.get(&handle) { - let b2_body = self.world.body(*pb2_handle); - let pos = b2_transform_to_na_isometry(b2_body.transform().clone()); - body.set_position(pos, false); - - for coll_handle in body.colliders() { - let collider = &mut colliders[*coll_handle]; - collider.set_position( - pos * collider.position_wrt_parent().copied().unwrap_or(na::one()), - ); - } - } - } - } -} diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index 5ef72fb..f3ddf86 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -9,8 +9,6 @@ pub use crate::plugin::TestbedPlugin; pub use crate::testbed::{Testbed, TestbedApp, TestbedGraphics, TestbedState}; pub use bevy::prelude::{Color, KeyCode}; -#[cfg(all(feature = "dim2", feature = "other-backends"))] -mod box2d_backend; #[cfg(feature = "dim2")] mod camera2d; #[cfg(feature = "dim3")] diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index ae77615..91e0ae2 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -173,7 +173,7 @@ impl PhysxWorld { gravity: gravity.into_physx(), thread_count: num_threads as u32, broad_phase_type: BroadPhaseType::Abp, - solver_type: SolverType::Pgs, + solver_type: SolverType::Tgs, friction_type, ccd_max_passes: integration_parameters.max_ccd_substeps as u32, ..SceneDescriptor::new(()) @@ -211,7 +211,7 @@ impl PhysxWorld { actor.set_angular_velocity(&angvel, true); actor.set_solver_iteration_counts( // Use our number of solver iterations as their number of position iterations. - integration_parameters.num_solver_iterations.get() as u32, + integration_parameters.num_solver_iterations as u32, 1, ); @@ -770,6 +770,8 @@ impl AdvanceCallback for OnAdvance { } unsafe extern "C" fn ccd_filter_shader(data: *mut FilterShaderCallbackInfo) -> PxFilterFlags { - (*(*data).pairFlags) |= physx_sys::PxPairFlags::DetectCcdContact; + unsafe { + (*(*data).pairFlags) |= physx_sys::PxPairFlags::DetectCcdContact; + } PxFilterFlags::empty() } diff --git a/src_testbed/save.rs b/src_testbed/save.rs index 825226e..463199a 100644 --- a/src_testbed/save.rs +++ b/src_testbed/save.rs @@ -3,7 +3,7 @@ use crate::camera2d::OrbitCamera; #[cfg(feature = "dim3")] use crate::camera3d::OrbitCamera; use crate::settings::ExampleSettings; -use crate::testbed::{RapierSolverType, RunMode, TestbedStateFlags}; +use crate::testbed::{RunMode, TestbedStateFlags}; use serde::{Deserialize, Serialize}; #[derive(Serialize, Deserialize, PartialEq, Debug, Default, Clone)] @@ -13,22 +13,6 @@ pub struct SerializableTestbedState { pub selected_example: usize, pub selected_backend: usize, pub example_settings: ExampleSettings, - pub solver_type: RapierSolverType, pub physx_use_two_friction_directions: bool, pub camera: OrbitCamera, } - -#[cfg(feature = "dim2")] -#[derive(Serialize, Deserialize, PartialEq, Debug, Default, Clone)] -pub struct SerializableCameraState { - pub zoom: f32, - pub center: na::Point2, -} - -#[cfg(feature = "dim3")] -#[derive(Serialize, Deserialize, PartialEq, Debug, Default, Clone)] -pub struct SerializableCameraState { - pub distance: f32, - pub position: na::Point3, - pub center: na::Point3, -} diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 1432565..ed06101 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -4,7 +4,6 @@ use bevy::prelude::*; use std::env; use std::mem; -use std::num::NonZeroUsize; use crate::debug_render::{DebugRenderPipelineResource, RapierDebugRenderPlugin}; use crate::graphics::BevyMaterialComponent; @@ -30,8 +29,6 @@ use rapier::pipeline::PhysicsHooks; #[cfg(feature = "dim3")] use rapier::{control::DynamicRayCastVehicleController, prelude::QueryFilter}; -#[cfg(all(feature = "dim2", feature = "other-backends"))] -use crate::box2d_backend::Box2dWorld; use crate::harness::{Harness, RapierBroadPhaseType}; #[cfg(all(feature = "dim3", feature = "other-backends"))] use crate::physx_backend::PhysxWorld; @@ -48,8 +45,6 @@ use crate::graphics::BevyMaterial; // use bevy::render::render_resource::RenderPipelineDescriptor; const RAPIER_BACKEND: usize = 0; -#[cfg(all(feature = "dim2", feature = "other-backends"))] -const BOX2D_BACKEND: usize = 1; pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 1; pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 2; @@ -101,14 +96,6 @@ bitflags::bitflags! { } } -#[derive(Copy, Clone, Debug, PartialEq, Eq, Default, serde::Serialize, serde::Deserialize)] -pub enum RapierSolverType { - #[default] - TgsSoft, - TgsSoftNoWarmstart, - PgsLegacy, -} - pub type SimulationBuilders = Vec<(&'static str, fn(&mut Testbed))>; #[derive(Resource)] @@ -131,7 +118,6 @@ pub struct TestbedState { pub selected_example: usize, pub selected_backend: usize, pub example_settings: ExampleSettings, - pub solver_type: RapierSolverType, pub broad_phase_type: RapierBroadPhaseType, pub physx_use_two_friction_directions: bool, pub snapshot: Option, @@ -148,7 +134,6 @@ impl TestbedState { selected_example: self.selected_example, selected_backend: self.selected_backend, example_settings: self.example_settings.clone(), - solver_type: self.solver_type, physx_use_two_friction_directions: self.physx_use_two_friction_directions, camera, } @@ -161,7 +146,6 @@ impl TestbedState { self.selected_example = state.selected_example; self.selected_backend = state.selected_backend; self.example_settings = state.example_settings; - self.solver_type = state.solver_type; self.physx_use_two_friction_directions = state.physx_use_two_friction_directions; *camera = state.camera; } @@ -172,8 +156,6 @@ struct SceneBuilders(SimulationBuilders); #[cfg(feature = "other-backends")] struct OtherBackends { - #[cfg(feature = "dim2")] - box2d: Option, #[cfg(feature = "dim3")] physx: Option, } @@ -222,8 +204,6 @@ impl TestbedApp { #[allow(unused_mut)] let mut backend_names = vec!["rapier"]; - #[cfg(all(feature = "dim2", feature = "other-backends"))] - backend_names.push("box2d"); #[cfg(all(feature = "dim3", feature = "other-backends"))] backend_names.push("physx (patch friction)"); #[cfg(all(feature = "dim3", feature = "other-backends"))] @@ -249,7 +229,6 @@ impl TestbedApp { example_settings: ExampleSettings::default(), selected_example: 0, selected_backend: RAPIER_BACKEND, - solver_type: RapierSolverType::default(), broad_phase_type: RapierBroadPhaseType::default(), physx_use_two_friction_directions: true, nsteps: 1, @@ -260,8 +239,6 @@ impl TestbedApp { let harness = Harness::new_empty(); #[cfg(feature = "other-backends")] let other_backends = OtherBackends { - #[cfg(feature = "dim2")] - box2d: None, #[cfg(feature = "dim3")] physx: None, }; @@ -383,7 +360,7 @@ impl TestbedApp { self.harness .physics .integration_parameters - .num_solver_iterations = NonZeroUsize::new(4).unwrap(); + .num_solver_iterations = 4; // Init world. let mut testbed = Testbed { @@ -403,20 +380,6 @@ impl TestbedApp { self.harness.step(); } - #[cfg(all(feature = "dim2", feature = "other-backends"))] - { - if self.state.selected_backend == BOX2D_BACKEND { - self.other_backends.box2d.as_mut().unwrap().step( - &mut self.harness.physics.pipeline.counters, - &self.harness.physics.integration_parameters, - ); - self.other_backends.box2d.as_mut().unwrap().sync( - &mut self.harness.physics.bodies, - &mut self.harness.physics.colliders, - ); - } - } - #[cfg(all(feature = "dim3", feature = "other-backends"))] { if self.state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION @@ -671,18 +634,6 @@ impl Testbed<'_, '_, '_, '_, '_, '_, '_, '_, '_, '_, '_, '_, '_> { self.state.vehicle_controller = None; } - #[cfg(all(feature = "dim2", feature = "other-backends"))] - { - if self.state.selected_backend == BOX2D_BACKEND { - self.other_backends.box2d = Some(Box2dWorld::from_rapier( - self.harness.physics.gravity, - &self.harness.physics.bodies, - &self.harness.physics.colliders, - &self.harness.physics.impulse_joints, - )); - } - } - #[cfg(all(feature = "dim3", feature = "other-backends"))] { if self.state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION @@ -1462,22 +1413,6 @@ fn update_testbed( } } - #[cfg(all(feature = "dim2", feature = "other-backends"))] - { - if state.selected_backend == BOX2D_BACKEND { - let harness = &mut *harness; - other_backends.box2d.as_mut().unwrap().step( - &mut harness.physics.pipeline.counters, - &harness.physics.integration_parameters, - ); - other_backends - .box2d - .as_mut() - .unwrap() - .sync(&mut harness.physics.bodies, &mut harness.physics.colliders); - } - } - #[cfg(all(feature = "dim3", feature = "other-backends"))] { if state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 4f2b107..17bd644 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -1,12 +1,11 @@ use rapier::counters::Counters; use rapier::math::Real; -use std::num::NonZeroUsize; use crate::debug_render::DebugRenderPipelineResource; use crate::harness::{Harness, RapierBroadPhaseType}; use crate::testbed::{ - PHYSX_BACKEND_PATCH_FRICTION, PHYSX_BACKEND_TWO_FRICTION_DIR, RapierSolverType, RunMode, - TestbedActionFlags, TestbedState, TestbedStateFlags, + PHYSX_BACKEND_PATCH_FRICTION, PHYSX_BACKEND_TWO_FRICTION_DIR, RunMode, TestbedActionFlags, + TestbedState, TestbedStateFlags, }; pub use bevy_egui::egui; @@ -15,9 +14,11 @@ use crate::PhysicsState; use crate::settings::SettingValue; use bevy_egui::EguiContexts; use bevy_egui::egui::{ComboBox, Slider, Ui, Window}; -use rapier::dynamics::IntegrationParameters; use web_time::Instant; +#[cfg(feature = "dim3")] +use rapier::dynamics::FrictionModel; + pub(crate) fn update_ui( ui_context: &mut EguiContexts, state: &mut TestbedState, @@ -113,45 +114,11 @@ pub(crate) fn update_ui( if state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION || state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR { - let mut num_iterations = integration_parameters.num_solver_iterations.get(); - ui.add(Slider::new(&mut num_iterations, 1..=40).text("pos. iters.")); - integration_parameters.num_solver_iterations = - NonZeroUsize::new(num_iterations).unwrap(); + ui.add( + Slider::new(&mut integration_parameters.num_solver_iterations, 0..=10) + .text("pos. iters."), + ); } else { - // Solver type. - let mut changed = false; - egui::ComboBox::from_label("solver type") - .width(150.0) - .selected_text(format!("{:?}", state.solver_type)) - .show_ui(ui, |ui| { - let solver_types = [ - RapierSolverType::TgsSoft, - RapierSolverType::TgsSoftNoWarmstart, - RapierSolverType::PgsLegacy, - ]; - for sty in solver_types { - changed = ui - .selectable_value(&mut state.solver_type, sty, format!("{sty:?}")) - .changed() - || changed; - } - }); - - if changed { - match state.solver_type { - RapierSolverType::TgsSoft => { - *integration_parameters = IntegrationParameters::tgs_soft(); - } - RapierSolverType::TgsSoftNoWarmstart => { - *integration_parameters = - IntegrationParameters::tgs_soft_without_warmstart(); - } - RapierSolverType::PgsLegacy => { - *integration_parameters = IntegrationParameters::pgs_legacy(); - } - } - } - // Broad-phase. let mut changed = false; egui::ComboBox::from_label("broad-phase") @@ -177,12 +144,30 @@ pub(crate) fn update_ui( state.action_flags.set(TestbedActionFlags::RESTART, true); } - // Solver iterations. - let mut num_iterations = integration_parameters.num_solver_iterations.get(); - ui.add(Slider::new(&mut num_iterations, 1..=40).text("num solver iters.")); - integration_parameters.num_solver_iterations = - NonZeroUsize::new(num_iterations).unwrap(); + // Friction model. + #[cfg(feature = "dim3")] + egui::ComboBox::from_label("friction-model") + .width(150.0) + .selected_text(format!("{:?}", integration_parameters.friction_model)) + .show_ui(ui, |ui| { + let friction_model = [FrictionModel::Simplified, FrictionModel::Coulomb]; + for model in friction_model { + changed = ui + .selectable_value( + &mut integration_parameters.friction_model, + model, + format!("{model:?}"), + ) + .changed() + || changed; + } + }); + // Solver iterations. + ui.add( + Slider::new(&mut integration_parameters.num_solver_iterations, 0..=10) + .text("num solver iters."), + ); ui.add( Slider::new( &mut integration_parameters.num_internal_pgs_iterations, @@ -190,13 +175,6 @@ pub(crate) fn update_ui( ) .text("num internal PGS iters."), ); - ui.add( - Slider::new( - &mut integration_parameters.num_additional_friction_iterations, - 0..=40, - ) - .text("num additional frict. iters."), - ); ui.add( Slider::new( &mut integration_parameters.num_internal_stabilization_iterations, @@ -210,7 +188,7 @@ pub(crate) fn update_ui( ); let mut substep_params = *integration_parameters; - substep_params.dt /= substep_params.num_solver_iterations.get() as Real; + substep_params.dt /= substep_params.num_solver_iterations as Real; let curr_erp = substep_params.contact_erp(); let curr_cfm_factor = substep_params.contact_cfm_factor(); ui.add( @@ -411,7 +389,7 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { fn serialization_string(timestep_id: usize, physics: &PhysicsState) -> String { let t = Instant::now(); // let t = Instant::now(); - // let bf = bincode::serialize(&physics.broad_phase).unwrap(); + let bf = bincode::serialize(&physics.broad_phase).unwrap(); // println!("bf: {}", Instant::now() - t); // let t = Instant::now(); let nf = bincode::serialize(&physics.narrow_phase).unwrap(); @@ -426,7 +404,7 @@ fn serialization_string(timestep_id: usize, physics: &PhysicsState) -> String { let js = bincode::serialize(&physics.impulse_joints).unwrap(); // println!("js: {}", Instant::now() - t); let serialization_time = Instant::now() - t; - // let hash_bf = md5::compute(&bf); + let hash_bf = md5::compute(&bf); let hash_nf = md5::compute(&nf); let hash_bodies = md5::compute(&bs); let hash_colliders = md5::compute(&cs); @@ -441,8 +419,8 @@ Hashes at frame: {} |_ Joints [{:.1}KB]: {}"#, serialization_time.as_secs_f64() * 1000.0, timestep_id, - "", // bf.len() as f32 / 1000.0, - "", // format!("{:?}", hash_bf).split_at(10).0, + bf.len() as f32 / 1000.0, + format!("{:?}", hash_bf).split_at(10).0, nf.len() as f32 / 1000.0, format!("{hash_nf:?}").split_at(10).0, bs.len() as f32 / 1000.0,