feat: solver improvements + release v0.29.0 (#876)

* 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

@@ -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

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" }

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);

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);

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 <sebcrozet@dimforge.com>"]
 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"
+parry2d-f64 = "0.24.0"
-simba = "0.9"
+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"

crates/rapier2d/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "rapier2d"
-version = "0.28.0"
+version = "0.29.0"
 authors = ["Sébastien Crozet <sebcrozet@dimforge.com>"]
 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-stable = ["simba/wide", "parry2d/simd-stable", "simd-is-enabled"]
-simd-nightly = ["simba/portable_simd", "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"
+parry2d = "0.24.0"
-simba = "0.9"
+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"

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 <sebcrozet@dimforge.com>"]
 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"
+parry3d-f64 = "0.24.0"
-simba = "0.9"
+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"

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 <sebcrozet@dimforge.com>"]
 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" }

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 <sebcrozet@dimforge.com>"]
 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 = { version = "0.29.0", path = "../rapier3d" }
-rapier3d-meshloader = { version = "0.9.0", path = "../rapier3d-meshloader", default-features = false, optional = true }
+rapier3d-meshloader = { version = "0.10.0", path = "../rapier3d-meshloader", default-features = false, optional = true }

crates/rapier3d/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "rapier3d"
-version = "0.28.0"
+version = "0.29.0"
 authors = ["Sébastien Crozet <sebcrozet@dimforge.com>"]
 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"
+parry3d = "0.24.0"
-simba = "0.9"
+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"

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 <sebcrozet@dimforge.com>"]
 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"]

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 <sebcrozet@dimforge.com>"]
 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"]

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 <sebcrozet@dimforge.com>"]
 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"]

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 <sebcrozet@dimforge.com>"]
 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"]

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);

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))> =
+    let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
-        vec![("(Debug) serialized", debug_serialized3::init_world)];
+        ("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('(')) {

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(

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.
+            <f64 as ComplexField>::sin(x) + <f64 as ComplexField>::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());
+}

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),

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);

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);

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]);
+}

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);

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);

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);
         }
     });
 

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);

src/control/pid_controller.rs

@@ -79,8 +79,8 @@ pub struct PdErrors {
     pub angular: AngVector<Real>,
 }
 
-impl From<RigidBodyVelocity> for PdErrors {
+impl From<RigidBodyVelocity<Real>> for PdErrors {
-    fn from(vels: RigidBodyVelocity) -> Self {
+    fn from(vels: RigidBodyVelocity<Real>) -> Self {
         Self {
             linear: vels.linvel,
             angular: vels.angvel,
@@ -173,8 +173,8 @@ impl PdController {
         &self,
         rb: &RigidBody,
         target_pose: Isometry<Real>,
-        target_vels: RigidBodyVelocity,
+        target_vels: RigidBodyVelocity<Real>,
-    ) -> RigidBodyVelocity {
+    ) -> RigidBodyVelocity<Real> {
         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<Real> {
         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<Real>,
-        target_vels: RigidBodyVelocity,
+        target_vels: RigidBodyVelocity<Real>,
-    ) -> RigidBodyVelocity {
+    ) -> RigidBodyVelocity<Real> {
         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<Real> {
         self.lin_integral += pose_errors.linear * dt;
         self.ang_integral += pose_errors.angular * dt;
 

src/control/ray_cast_vehicle_controller.rs

@@ -723,9 +723,7 @@ impl<'a> WheelContactPoint<'a> {
         fn impulse_denominator(body: &RigidBody, pos: &Point<Real>, n: &Vector<Real>) -> Real {
             let dpt = pos - body.center_of_mass();
             let gcross = dpt.gcross(*n);
-            let v = (body.mprops.effective_world_inv_inertia_sqrt
+            let v = (body.mprops.effective_world_inv_inertia * gcross).gcross(dpt);
-                * (body.mprops.effective_world_inv_inertia_sqrt * 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 iaj = body1.mprops.effective_world_inv_inertia * aj;
-    let ibj = body2.mprops.effective_world_inv_inertia_sqrt * bj;
+    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<Real>, 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;

src/data/arena.rs

@@ -687,7 +687,7 @@ impl<T> Arena<T> {
     ///     println!("{} is at index {:?}", value, idx);
     /// }
     /// ```
-    pub fn iter(&self) -> Iter<T> {
+    pub fn iter(&self) -> Iter<'_, T> {
         Iter {
             len: self.len,
             inner: self.items.iter().enumerate(),
@@ -714,7 +714,7 @@ impl<T> Arena<T> {
     ///     *value += 5;
     /// }
     /// ```
-    pub fn iter_mut(&mut self) -> IterMut<T> {
+    pub fn iter_mut(&mut self) -> IterMut<'_, T> {
         IterMut {
             len: self.len,
             inner: self.items.iter_mut().enumerate(),
@@ -746,7 +746,7 @@ impl<T> Arena<T> {
     /// assert!(arena.get(idx_1).is_none());
     /// assert!(arena.get(idx_2).is_none());
     /// ```
-    pub fn drain(&mut self) -> Drain<T> {
+    pub fn drain(&mut self) -> Drain<'_, T> {
         Drain {
             inner: self.items.drain(..).enumerate(),
         }

src/data/graph.rs

@@ -389,7 +389,7 @@ impl<N, E> Graph<N, E> {
     ///
     /// Produces an empty iterator if the node doesn't exist.<br>
     /// Iterator element type is `EdgeReference<E, Ix>`.
-    pub fn edges(&self, a: NodeIndex) -> Edges<E> {
+    pub fn edges(&self, a: NodeIndex) -> Edges<'_, E> {
         self.edges_directed(a, Direction::Outgoing)
     }
 
@@ -404,7 +404,7 @@ impl<N, E> Graph<N, E> {
     ///
     /// Produces an empty iterator if the node `a` doesn't exist.<br>
     /// Iterator element type is `EdgeReference<E, Ix>`.
-    pub fn edges_directed(&self, a: NodeIndex, dir: Direction) -> Edges<E> {
+    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<E>(
     edges: &mut [Edge<E>],
     next: EdgeIndex,
     dir: Direction,
-) -> EdgesWalkerMut<E> {
+) -> EdgesWalkerMut<'_, E> {
     EdgesWalkerMut { edges, next, dir }
 }
 

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 =
+                let new_pos = rb
-                    rb.integrated_vels
+                    .ccd_vels
-                        .integrate(toi.max(min_toi), &rb.pos.position, local_com);
+                    .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| {
+                    .map(|t| rb1.ccd_vels.integrate(*t, &rb1.pos.position, local_com1))
-                        rb1.integrated_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| {
+                    .map(|t| rb2.ccd_vels.integrate(*t, &rb2.pos.position, local_com2))
-                        rb2.integrated_vels
-                            .integrate(*t, &rb2.pos.position, local_com2)
-                    })
                     .unwrap_or(rb2.pos.next_position);
                 pos2 * co_parent2.pos_wrt_parent
             } else {

src/dynamics/ccd/toi_entry.rs

@@ -54,14 +54,14 @@ impl TOIEntry {
             return None;
         }
 
-        let linvel1 = frozen1.is_none() as u32 as Real
+        let linvel1 =
-            * rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
+            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
+        let linvel2 =
-            * rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
+            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
+        let angvel1 =
-            * rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
+            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
+        let angvel2 =
-            * rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
+            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.ccd_vels.linvel,
-                rb.integrated_vels.angvel,
+                rb.ccd_vels.angvel,
             )
         } else {
             NonlinearRigidMotion::constant_position(rb.pos.next_position)

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,
+    pub num_solver_iterations: usize,
-    /// Number of addition friction resolution iteration run during the last solver sub-step (default: `0`).
-    pub num_additional_friction_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
+impl Default for IntegrationParameters {
-    /// with warmstarting.
+    fn default() -> Self {
-    ///
-    /// This is the default configuration, equivalent to [`IntegrationParameters::default()`].
-    pub fn tgs_soft() -> 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_internal_stabilization_iterations: 1,
-            num_additional_friction_iterations: 0,
+            num_solver_iterations: 4,
-            num_solver_iterations: NonZeroUsize::new(4).unwrap(),
             // 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,
-        }
+            #[cfg(feature = "dim3")]
-    }
+            friction_model: FrictionModel::default(),
-
-    /// 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()
         }
     }
 }
-
-impl Default for IntegrationParameters {
-    fn default() -> Self {
-        Self::tgs_soft()
-    }
-}

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<RigidBodyHandle>,
+    pub(crate) active_set: Vec<RigidBodyHandle>,
-    pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
     pub(crate) active_islands: Vec<usize>,
     pub(crate) active_islands_additional_solver_iterations: Vec<usize>,
     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_set: vec![],
-            active_kinematic_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 {
+        while i < self.active_set.len() {
-            let mut i = 0;
+            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() {
+                if i < self.active_set.len() {
-                let handle = active_set[i];
+                    // Update the self.active_set_id for the body that has been swapped.
-                if bodies.get(handle).is_none() {
+                    if let Some(swapped_rb) = bodies.get_mut_internal(self.active_set[i]) {
-                    // This rigid-body no longer exists, so we need to remove it from the active set.
+                        swapped_rb.ids.active_set_id = i;
-                    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;
-                        }
                     }
-                } 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 let Some(replacement) = self
-            if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
+                .active_set
-                active_set.swap_remove(removed_ids.active_set_id);
+                .get(removed_ids.active_set_id)
-
+                .and_then(|h| bodies.get_mut_internal(*h))
-                if let Some(replacement) = active_set
+            {
-                    .get(removed_ids.active_set_id)
+                replacement.ids.active_set_id = 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()
+                if rb.is_enabled() && self.active_set.get(rb.ids.active_set_id) != Some(&handle) {
-                    && self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle)
+                    rb.ids.active_set_id = self.active_set.len();
-                {
+                    self.active_set.push(handle);
-                    rb.ids.active_set_id = self.active_dynamic_set.len();
-                    self.active_dynamic_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)]
+    /// Handls of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping).
-    pub(crate) fn iter_active_bodies(&self) -> impl Iterator<Item = RigidBodyHandle> + '_ {
+    #[inline]
-        self.active_dynamic_set
+    pub fn active_bodies(&self) -> &[RigidBodyHandle] {
-            .iter()
+        &self.active_set
-            .copied()
-            .chain(self.active_kinematic_set.iter().copied())
     }
 
     #[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,

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<Real>; 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(

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_or_kinematic() || rb2.is_dynamic_or_kinematic())
-                && (!rb1.is_dynamic() || !rb1.is_sleeping())
+                && (!rb1.is_dynamic_or_kinematic() || !rb1.is_sleeping())
-                && (!rb2.is_dynamic() || !rb2.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

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<Real> {
+    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<Real> {
+    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<Real> {
+    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<Real> {
+    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);
                 }
             }
         }

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<Real> {
         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<Real>,
-        constraints: &mut [JointGenericOneBodyConstraint],
+        constraints: &mut [GenericJointConstraint],
     ) -> usize {
         let j_id = &mut j_id;
         let locked_bits = self.data.locked_axes.bits();

src/dynamics/joint/multibody_joint/multibody_link.rs

@@ -27,7 +27,7 @@ pub struct MultibodyLink {
     pub(crate) shift23: Vector<Real>,
 
     /// The velocity added by the joint, in world-space.
-    pub(crate) joint_velocity: RigidBodyVelocity,
+    pub(crate) joint_velocity: RigidBodyVelocity<Real>,
 }
 
 impl MultibodyLink {

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<RigidBodyVelocity>,
+    pub accs: Vec<RigidBodyVelocity<Real>>,
     pub ndofs_vec: DVector<Real>,
 }
 

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<Real>,
-    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<Real>,
-    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,

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::*;

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<Real>,
+    pub(crate) vels: RigidBodyVelocity<Real>,
+    pub(crate) forces: RigidBodyForces,
+    pub(crate) mprops: RigidBodyMassProps,
+
+    pub(crate) ccd_vels: RigidBodyVelocity<Real>,
+    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<Real> {
         &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<Real>, 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<Real>) {
         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<Real>) {
         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<Real>) {
         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.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse;
-                * (self.mprops.effective_world_inv_inertia_sqrt * 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<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.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse;
-                * (self.mprops.effective_world_inv_inertia_sqrt * 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<Real> {
+        // 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<Real>) -> 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<Real>) -> 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);

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<Real>) -> RigidBodyVelocity {
+    pub fn interpolate_velocity(
+        &self,
+        inv_dt: Real,
+        local_com: &Point<Real>,
+    ) -> RigidBodyVelocity<Real> {
         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<Real>,
         mprops: &RigidBodyMassProps,
     ) -> Isometry<Real> {
         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<Box<RigidBodyAdditionalMassProps>>,
     /// The world-space center of mass of the rigid-body.
     pub world_com: Point<Real>,
     /// The inverse mass taking into account translation locking.
     pub effective_inv_mass: Vector<Real>,
     /// 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<Real>,
+    pub effective_world_inv_inertia: AngularInertia<Real>,
+    /// 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<Box<RigidBodyAdditionalMassProps>>,
 }
 
 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<Real> {
+    pub fn effective_angular_inertia(&self) -> AngularInertia<Real> {
         #[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<Real> {
-        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<Real>,
     ) {
         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<Real>) {
+    pub fn update_world_mass_properties(
+        &mut self,
+        body_type: RigidBodyType,
+        position: &Isometry<Real>,
+    ) {
         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.effective_world_inv_inertia = self.local_mprops.world_inv_inertia(&position.rotation);
-            self.local_mprops.world_inv_inertia_sqrt(&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) {
+            if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
-                self.effective_world_inv_inertia_sqrt = 0.0;
+                self.effective_world_inv_inertia = 0.0;
             }
         }
         #[cfg(feature = "dim3")]
         {
-            if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
+            if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
-                self.effective_world_inv_inertia_sqrt.m11 = 0.0;
+                self.effective_world_inv_inertia.m11 = 0.0;
-                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia.m12 = 0.0;
-                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+                self.effective_world_inv_inertia.m13 = 0.0;
             }
 
-            if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
+            if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
-                self.effective_world_inv_inertia_sqrt.m22 = 0.0;
+                self.effective_world_inv_inertia.m22 = 0.0;
-                self.effective_world_inv_inertia_sqrt.m12 = 0.0;
+                self.effective_world_inv_inertia.m12 = 0.0;
-                self.effective_world_inv_inertia_sqrt.m23 = 0.0;
+                self.effective_world_inv_inertia.m23 = 0.0;
             }
-            if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
+            if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
-                self.effective_world_inv_inertia_sqrt.m33 = 0.0;
+                self.effective_world_inv_inertia.m33 = 0.0;
-                self.effective_world_inv_inertia_sqrt.m13 = 0.0;
+                self.effective_world_inv_inertia.m13 = 0.0;
-                self.effective_world_inv_inertia_sqrt.m23 = 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<T: SimdRealCopy> {
     /// The linear velocity of the rigid-body.
-    pub linvel: Vector<Real>,
+    pub linvel: Vector<T>,
     /// The angular velocity of the rigid-body.
-    pub angvel: AngVector<Real>,
+    pub angvel: AngVector<T>,
 }
 
-impl Default for RigidBodyVelocity {
+impl Default for RigidBodyVelocity<Real> {
     fn default() -> Self {
         Self::zero()
     }
 }
 
-impl RigidBodyVelocity {
+impl RigidBodyVelocity<Real> {
     /// Create a new rigid-body velocity component.
     #[must_use]
     pub fn new(linvel: Vector<Real>, angvel: AngVector<Real>) -> 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<Real>, world_com: &Point<Real>) -> Vector<Real> {
@@ -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<Real>,
-        local_com: &Point<Real>,
-    ) -> Isometry<Real> {
-        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() {
+        if !rb_mprops.effective_world_inv_inertia.is_zero() {
-            let inertia_sqrt = 1.0 / rb_mprops.effective_world_inv_inertia_sqrt;
+            let inertia = 1.0 / rb_mprops.effective_world_inv_inertia;
-            energy += (inertia_sqrt * self.angvel).powi(2) / 2.0;
+            energy += inertia * self.angvel * self.angvel / 2.0;
         }
 
         #[cfg(feature = "dim3")]
-        if !rb_mprops.effective_world_inv_inertia_sqrt.is_zero() {
+        if !rb_mprops.effective_world_inv_inertia.is_zero() {
-            let inertia_sqrt = rb_mprops
+            let inertia = rb_mprops.effective_world_inv_inertia.inverse_unchecked();
-                .effective_world_inv_inertia_sqrt
+            energy += self.angvel.dot(&(inertia * self.angvel)) / 2.0;
-                .inverse_unchecked();
-            energy += (inertia_sqrt * self.angvel).norm_squared() / 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
+        self.angvel += rb_mprops.effective_world_inv_inertia * torque_impulse;
-            * (rb_mprops.effective_world_inv_inertia_sqrt * 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<Real>,
     ) {
-        self.angvel += rb_mprops.effective_world_inv_inertia_sqrt
+        self.angvel += rb_mprops.effective_world_inv_inertia * torque_impulse;
-            * (rb_mprops.effective_world_inv_inertia_sqrt * 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<Real> for RigidBodyVelocity {
+impl<T: SimdRealCopy> RigidBodyVelocity<T> {
+    /// Returns the update velocities after applying the given damping.
+    #[must_use]
+    pub fn apply_damping(&self, dt: T, damping: &RigidBodyDamping<T>) -> 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<T>, local_com: &Point<T>) -> Isometry<T> {
+        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<T>) -> Isometry<T> {
+        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<T>) {
+        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<T>) {
+        // 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<Real> for RigidBodyVelocity<Real> {
     type Output = Self;
 
     fn mul(self, rhs: Real) -> Self {
@@ -735,7 +781,7 @@ impl std::ops::Mul<Real> for RigidBodyVelocity {
     }
 }
 
-impl std::ops::Add<RigidBodyVelocity> for RigidBodyVelocity {
+impl std::ops::Add<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
     type Output = Self;
 
     fn add(self, rhs: Self) -> Self {
@@ -746,14 +792,14 @@ impl std::ops::Add<RigidBodyVelocity> for RigidBodyVelocity {
     }
 }
 
-impl std::ops::AddAssign<RigidBodyVelocity> for RigidBodyVelocity {
+impl std::ops::AddAssign<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
     fn add_assign(&mut self, rhs: Self) {
         self.linvel += rhs.linvel;
         self.angvel += rhs.angvel;
     }
 }
 
-impl std::ops::Sub<RigidBodyVelocity> for RigidBodyVelocity {
+impl std::ops::Sub<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
     type Output = Self;
 
     fn sub(self, rhs: Self) -> Self {
@@ -764,7 +810,7 @@ impl std::ops::Sub<RigidBodyVelocity> for RigidBodyVelocity {
     }
 }
 
-impl std::ops::SubAssign<RigidBodyVelocity> for RigidBodyVelocity {
+impl std::ops::SubAssign<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
     fn sub_assign(&mut self, rhs: Self) {
         self.linvel -= rhs.linvel;
         self.angvel -= rhs.angvel;
@@ -774,18 +820,18 @@ impl std::ops::SubAssign<RigidBodyVelocity> 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<T> {
     /// 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<T: SimdRealCopy> Default for RigidBodyDamping<T> {
     fn default() -> Self {
         Self {
-            linear_damping: 0.0,
+            linear_damping: T::zero(),
-            angular_damping: 0.0,
+            angular_damping: T::zero(),
         }
     }
 }
@@ -805,6 +851,9 @@ pub struct RigidBodyForces {
     pub user_force: Vector<Real>,
     /// Torque applied by the user.
     pub user_torque: AngVector<Real>,
+    /// 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<Real>,
         mprops: &RigidBodyMassProps,
-    ) -> RigidBodyVelocity {
+    ) -> RigidBodyVelocity<Real> {
         let linear_acc = self.force.component_mul(&mprops.effective_inv_mass);
-        let angular_acc = mprops.effective_world_inv_inertia_sqrt
+        let angular_acc = mprops.effective_world_inv_inertia * self.torque;
-            * (mprops.effective_world_inv_inertia_sqrt * 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>) -> 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<Real>,
         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

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<RigidBody>,
     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(),
         }
     }
 

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<ContactManifoldIndex>,
     out_two_body: &mut Vec<ContactManifoldIndex>,
-    out_generic_one_body: &mut Vec<ContactManifoldIndex>,
     out_generic_two_body: &mut Vec<ContactManifoldIndex>,
 ) {
     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_two_body.push(*manifold_i);
-                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)
         } 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<JointIndex>,
     two_body_joints: &mut Vec<JointIndex>,
-    generic_one_body_joints: &mut Vec<JointIndex>,
     generic_two_body_joints: &mut Vec<JointIndex>,
 ) {
     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_two_body_joints.push(*joint_i);
-                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);
         } else {
             two_body_joints.push(*joint_i);
         }

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<Real>,
+        solver_vels: &mut SolverBodies,
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        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<Real>,
+        bodies: &mut SolverBodies,
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        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),
+        }
+    }
+}

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<N: SimdRealCopy> {
+pub(crate) struct ContactConstraintTwistPart<N: SimdRealCopy> {
-    pub gcross1: [AngVector<N>; DIM - 1],
+    // pub twist_dir: AngVector<N>, // NOTE: The torque direction equals the normal in 3D and 1.0 in 2D.
-    pub gcross2: [AngVector<N>; DIM - 1],
+    pub ii_twist_dir1: AngVector<N>,
+    pub ii_twist_dir2: AngVector<N>,
+    pub rhs: N,
+    pub impulse: N,
+    pub impulse_accumulator: N,
+    pub r: N,
+}
+
+#[cfg(feature = "dim3")]
+impl<N: SimdRealCopy> ContactConstraintTwistPart<N> {
+    #[inline]
+    pub fn warmstart(&mut self, solver_vel1: &mut SolverVel<N>, solver_vel2: &mut SolverVel<N>)
+    where
+        AngVector<N>: SimdDot<AngVector<N>, 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<N>,
+        limit: N,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) where
+        AngVector<N>: SimdDot<AngVector<N>, 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<N: SimdRealCopy> {
+    pub torque_dir1: [AngVector<N>; DIM - 1],
+    pub torque_dir2: [AngVector<N>; DIM - 1],
+    pub ii_torque_dir1: [AngVector<N>; DIM - 1],
+    pub ii_torque_dir2: [AngVector<N>; 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<N: SimdRealCopy> {
     pub r: [N; DIM],
 }
 
-impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
+impl<N: SimdRealCopy> ContactConstraintTangentPart<N> {
-    fn zero() -> Self {
+    pub fn zero() -> Self {
         Self {
-            gcross1: [na::zero(); DIM - 1],
+            torque_dir1: [na::zero(); DIM - 1],
-            gcross2: [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<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
         #[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]
+            solver_vel1.linear += (tangents1[0] * self.impulse[0] + tangents1[1] * self.impulse[1])
-                + tangents1[1].component_mul(im1) * 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]
+            solver_vel2.linear += (tangents1[0] * -self.impulse[0]
-                + tangents1[1].component_mul(im2) * -self.impulse[1];
+                + 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<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
         #[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<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
             let dlambda = new_impulse - self.impulse;
             self.impulse = new_impulse;
 
-            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+            solver_vel1.linear +=
-                + tangents1[1].component_mul(im1) * dlambda[1];
+                (tangents1[0] * dlambda[0] + tangents1[1] * dlambda[1]).component_mul(im1);
-            solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
+            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]
+            solver_vel2.linear +=
-                + tangents1[1].component_mul(im2) * -dlambda[1];
+                (tangents1[0] * -dlambda[0] + tangents1[1] * -dlambda[1]).component_mul(im2);
-            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
+            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<N: SimdRealCopy> {
+pub(crate) struct ContactConstraintNormalPart<N: SimdRealCopy> {
-    pub gcross1: AngVector<N>,
+    pub torque_dir1: AngVector<N>,
-    pub gcross2: AngVector<N>,
+    pub torque_dir2: AngVector<N>,
+    pub ii_torque_dir1: AngVector<N>,
+    pub ii_torque_dir2: AngVector<N>,
     pub rhs: N,
     pub rhs_wo_bias: N,
     pub impulse: N,
@@ -170,11 +220,13 @@ pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
     pub r_mat_elts: [N; 2],
 }
 
-impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
+impl<N: SimdRealCopy> ContactConstraintNormalPart<N> {
-    fn zero() -> Self {
+    pub fn zero() -> Self {
         Self {
-            gcross1: na::zero(),
+            torque_dir1: na::zero(),
-            gcross2: 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<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
         solver_vel2: &mut SolverVel<N>,
     ) {
         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<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
     ) where
         AngVector<N>: SimdDot<AngVector<N>, 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<N>,
         prev_impulse: Vector2<N>,
@@ -280,12 +332,12 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
     {
         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.torque_dir1.gdot(solver_vel1.angular)
-            + constraint_a.gcross2.gdot(solver_vel2.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.torque_dir1.gdot(solver_vel1.angular)
-            + constraint_b.gcross2.gdot(solver_vel2.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<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
         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;
+        solver_vel2.angular +=
-    }
+            constraint_a.ii_torque_dir2 * dlambda.x + constraint_b.ii_torque_dir2 * dlambda.y;
-}
-
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct TwoBodyConstraintElement<N: SimdRealCopy> {
-    pub normal_part: TwoBodyConstraintNormalPart<N>,
-    pub tangent_part: TwoBodyConstraintTangentPart<N>,
-}
-
-impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
-    pub fn zero() -> Self {
-        Self {
-            normal_part: TwoBodyConstraintNormalPart::zero(),
-            tangent_part: TwoBodyConstraintTangentPart::zero(),
-        }
-    }
-
-    #[inline]
-    pub fn warmstart_group(
-        elements: &mut [Self],
-        dir1: &Vector<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im1: &Vector<N>,
-        im2: &Vector<N>,
-        solver_vel1: &mut SolverVel<N>,
-        solver_vel2: &mut SolverVel<N>,
-    ) {
-        #[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<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im1: &Vector<N>,
-        im2: &Vector<N>,
-        limit: N,
-        solver_vel1: &mut SolverVel<N>,
-        solver_vel2: &mut SolverVel<N>,
-        solve_restitution: bool,
-        solve_friction: bool,
-    ) where
-        Vector<N>: SimdBasis,
-        AngVector<N>: SimdDot<AngVector<N>, 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);
-            }
-        }
     }
 }

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,
+    ContactWithCoulombFriction, ContactWithCoulombFrictionBuilder, GenericContactConstraint,
-    GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder,
+    GenericContactConstraintBuilder,
-    TwoBodyConstraint, TwoBodyConstraintBuilder,
 };
-use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::interaction_groups::InteractionGroups;
-use crate::dynamics::solver::solver_vel::SolverVel;
+use crate::dynamics::solver::reset_buffer;
-use crate::dynamics::solver::{ConstraintTypes, SolverConstraintsSet, 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::any_contact_constraint::AnyContactConstraintMut;
-use {
+#[cfg(feature = "dim3")]
-    crate::dynamics::solver::contact_constraint::{
+use crate::dynamics::{
-        OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd,
+    FrictionModel,
-        TwoBodyConstraintSimd,
+    solver::contact_constraint::{ContactWithTwistFriction, ContactWithTwistFrictionBuilder},
-    },
-    crate::math::SIMD_WIDTH,
 };
 
 #[derive(Debug)]
@@ -63,30 +61,85 @@ impl ConstraintsCounts {
     }
 }
 
-#[derive(Copy, Clone, Debug)]
+pub(crate) struct ContactConstraintsSet {
-pub(crate) struct ContactConstraintTypes;
+    pub generic_jacobians: DVector<Real>,
+    pub two_body_interactions: Vec<ContactManifoldIndex>,
+    pub generic_two_body_interactions: Vec<ContactManifoldIndex>,
+    pub interaction_groups: InteractionGroups,
 
-impl ConstraintTypes for ContactConstraintTypes {
+    pub generic_velocity_constraints: Vec<GenericContactConstraint>,
-    type OneBody = OneBodyConstraint;
+    pub simd_velocity_coulomb_constraints: Vec<ContactWithCoulombFriction>,
-    type TwoBodies = TwoBodyConstraint;
+    #[cfg(feature = "dim3")]
-    type GenericOneBody = GenericOneBodyConstraint;
+    pub simd_velocity_twist_constraints: Vec<ContactWithTwistFriction>,
-    type GenericTwoBodies = GenericTwoBodyConstraint;
-    #[cfg(feature = "simd-is-enabled")]
-    type SimdOneBody = OneBodyConstraintSimd;
-    #[cfg(feature = "simd-is-enabled")]
-    type SimdTwoBodies = TwoBodyConstraintSimd;
 
-    type BuilderOneBody = OneBodyConstraintBuilder;
+    pub generic_velocity_constraints_builder: Vec<GenericContactConstraintBuilder>,
-    type BuilderTwoBodies = TwoBodyConstraintBuilder;
+    pub simd_velocity_coulomb_constraints_builder: Vec<ContactWithCoulombFrictionBuilder>,
-    type GenericBuilderOneBody = GenericOneBodyConstraintBuilder;
+    #[cfg(feature = "dim3")]
-    type GenericBuilderTwoBodies = GenericTwoBodyConstraintBuilder;
+    pub simd_velocity_twist_constraints_builder: Vec<ContactWithTwistFrictionBuilder>,
-    #[cfg(feature = "simd-is-enabled")]
-    type SimdBuilderOneBody = SimdOneBodyConstraintBuilder;
-    #[cfg(feature = "simd-is-enabled")]
-    type SimdBuilderTwoBodies = TwoBodyConstraintBuilderSimd;
 }
 
-pub type ContactConstraintsSet = SolverConstraintsSet<ContactConstraintTypes>;
+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<Real>,
+        impl Iterator<Item = AnyContactConstraintMut<'_>>,
+    ) {
+        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
+        // self.interaction_groups
-        //            .nongrouped_interactions
+        //     .nongrouped_interactions
-        //            .append(&mut self.interaction_groups.simd_interactions);
+        //     .append(&mut self.interaction_groups.simd_interactions);
-        //        self.one_body_interaction_groups
+        // self.one_body_interaction_groups
-        //            .nongrouped_interactions
+        //     .nongrouped_interactions
-        //            .append(&mut self.one_body_interaction_groups.simd_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")]
+        // let t0 = std::time::Instant::now();
-        {
+        // #[cfg(feature = "simd-is-enabled")]
-            self.simd_compute_one_body_constraints(bodies, manifolds);
+        // {
+        //     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(
+        // let t_init_constraints_simd = t0.elapsed().as_secs_f32();
-            bodies,
+        // let num_simd_constraints = self.simd_velocity_constraints.len();
-            multibody_joints,
+        // println!(
-            manifolds,
+        //     "t_init_group: {:?}, t_init_constraints_simd: {}: {:?}, t_debug: {:?}",
-            &mut jacobian_id,
+        //     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")]
+    // #[cfg(feature = "simd-is-enabled")]
-    fn simd_compute_constraints(
+    // 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::<usize>();
+    //
+    //     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::<usize>()
+            + self
+                .interaction_groups
+                .nongrouped_interactions
+                .iter()
+                .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
+                .sum::<usize>();
 
         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 manifold_id = array![|ii| manifolds_i[ii]];
-            let manifolds = gather![|ii| &*manifolds_all[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..],
+                solver_bodies,
-                &mut self.simd_velocity_constraints[curr_start..],
+                &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
+            .simd_interactions
-            .iter()
+            .chunks_exact(SIMD_WIDTH)
-            .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
+            .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints)
-            .sum();
+            .sum::<usize>()
+            + self
+                .interaction_groups
+                .nongrouped_interactions
+                .iter()
+                .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
+                .sum::<usize>();
 
         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 {
+        for manifolds_i in self
-            let manifold = &manifolds_all[*manifold_i];
+            .interaction_groups
-            let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
+            .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(
+            ContactWithCoulombFrictionBuilder::generate(
-                *manifold_i,
+                manifold_id,
-                manifold,
+                manifolds,
                 bodies,
-                &mut self.velocity_constraints_builder[curr_start..],
+                solver_bodies,
-                &mut self.velocity_constraints[curr_start..],
+                &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::<usize>();
 
         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<Real>],
+        solver_bodies: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
     ) {
         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<Real>],
+        solver_bodies: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
     ) {
         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<Real>],
+        solver_bodies: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
     ) {
         let (jac, constraints) = self.iter_constraints_mut();
         for mut c in constraints {
             c.remove_bias();
-            c.solve_restitution(jac, solver_vels, generic_solver_vels);
+            c.solve(jac, solver_bodies, generic_solver_vels);
-        }
-    }
-
-    #[profiling::function]
-    pub fn solve_friction(
-        &mut self,
-        solver_vels: &mut [SolverVel<Real>],
-        generic_solver_vels: &mut DVector<Real>,
-    ) {
-        let (jac, constraints) = self.iter_constraints_mut();
-        for mut c in constraints {
-            c.solve_friction(jac, solver_vels, 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,
+            simd_velocity_coulomb_constraints_builder,
-            generic_velocity_one_body_constraints
+            simd_velocity_coulomb_constraints
         );
+        #[cfg(feature = "dim3")]
         update_contacts!(
-            velocity_one_body_constraints_builder,
+            simd_velocity_twist_constraints_builder,
-            velocity_one_body_constraints
+            simd_velocity_twist_constraints
-        );
-        #[cfg(feature = "simd-is-enabled")]
-        update_contacts!(
-            simd_velocity_one_body_constraints_builder,
-            simd_velocity_one_body_constraints
         );
     }
 }

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<N: SimdRealCopy> {
+    pub tangent_vel: Vector<N>, // PERF: could be one float less, be shared by both contact point infos?
+    pub normal_vel: N,
+    pub local_p1: Point<N>,
+    pub local_p2: Point<N>,
+    pub dist: N,
+}
+
+impl<N: SimdRealCopy> Default for CoulombContactPointInfos<N> {
+    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<SimdReal>; 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::<SimdReal>::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<SimdReal>, // Non-penetration force direction for the first body.
+    pub im1: Vector<SimdReal>,
+    pub im2: Vector<SimdReal>,
+    pub cfm_factor: SimdReal,
+    pub limit: SimdReal,
+
+    #[cfg(feature = "dim3")]
+    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
+    pub normal_part: [ContactConstraintNormalPart<SimdReal>; MAX_MANIFOLD_POINTS],
+    pub tangent_part: [ContactConstraintTangentPart<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 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;
+        }
+    }
+}

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<N: SimdRealCopy> {
+    // 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<N>,
+    pub local_p2: Point<N>,
+    pub dist: N,
+}
+
+impl<N: SimdRealCopy> Default for TwistContactPointInfos<N> {
+    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<SimdReal>; MAX_MANIFOLD_POINTS],
+    local_friction_center1: Point<SimdReal>,
+    local_friction_center2: Point<SimdReal>,
+    tangent_vel: Vector<SimdReal>,
+}
+
+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::<SimdReal>::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<SimdReal>, // Non-penetration force direction for the first body.
+    pub im1: Vector<SimdReal>,
+    pub im2: Vector<SimdReal>,
+    pub cfm_factor: SimdReal,
+    pub limit: SimdReal,
+
+    #[cfg(feature = "dim3")]
+    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
+    pub normal_part: [ContactConstraintNormalPart<SimdReal>; MAX_MANIFOLD_POINTS],
+    // The twist friction model emulates coulomb with only one tangent
+    // constraint + one twist constraint per manifold.
+    pub tangent_part: ContactConstraintTangentPart<SimdReal>,
+    // Twist constraint (angular-only) to compensate the lack of angular resistance on the tangent plane.
+    pub twist_part: ContactConstraintTwistPart<SimdReal>,
+    // 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;
+    }
+}

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 super::{ContactConstraintNormalPart, ContactConstraintTangentPart};
-use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::CoulombContactPointInfos;
-use crate::dynamics::solver::{ContactPointInfos, SolverVel};
+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<Real>; 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_builders: &mut [GenericContactConstraintBuilder],
-        out_constraints: &mut [GenericTwoBodyConstraint],
+        out_constraints: &mut [GenericContactConstraint],
         jacobians: &mut DVector<Real>,
         jacobian_id: &mut usize,
     ) {
-        let handle1 = manifold.data.rigid_body1.unwrap();
+        // TODO PERF: we haven’t tried to optimized this codepath yet (since it relies
-        let handle2 = manifold.data.rigid_body2.unwrap();
+        //            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 rb1 = &bodies.get(handle1).unwrap_or(&bodies.default_fixed);
-        let rb2 = &bodies[handle2];
+        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
+        let solver_vel1 =
-            .map(|mb| mb.0.solver_id)
+            multibody1
-            .unwrap_or(if type1.is_dynamic() {
+                .map(|mb| mb.0.solver_id)
-                rb1.ids.active_set_offset
+                .unwrap_or(if type1.is_dynamic_or_kinematic() {
-            } else {
+                    rb1.ids.active_set_offset
-                0
+                } else {
-            });
+                    u32::MAX
-        let solver_vel2 = multibody2
+                });
-            .map(|mb| mb.0.solver_id)
+        let solver_vel2 =
-            .unwrap_or(if type2.is_dynamic() {
+            multibody2
-                rb2.ids.active_set_offset
+                .map(|mb| mb.0.solver_id)
-            } else {
+                .unwrap_or(if type2.is_dynamic_or_kinematic() {
-                0
+                    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.dir1 = force_dir1;
-            constraint.inner.im1 = if type1.is_dynamic() {
+            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.solver_vel1 = solver_vel1;
-            constraint.inner.solver_vel2 = solver_vel2;
+            constraint.solver_vel2 = solver_vel2;
-            constraint.inner.manifold_id = manifold_id;
+            constraint.manifold_id = manifold_id;
-            constraint.inner.num_contacts = manifold_points.len() as u8;
+            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.limit = manifold_point.friction;
-                constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
+                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 {
+                    constraint.normal_part[k] = ContactConstraintNormalPart {
-                        gcross1,
+                        torque_dir1,
-                        gcross2,
+                        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 =
+                    constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_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.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
-                        constraint.inner.elements[k].tangent_part.rhs[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;
+                builder.infos[k] = infos;
-                constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
+                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)
+                | (!type1.is_dynamic_or_kinematic() as u8)
-                | ((!type2.is_dynamic() as u8) << 1);
+                | ((!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 cfm_factor = params.contact_cfm_factor();
-        let pos1 = multibodies
+        let inv_dt = params.inv_dt();
-            .rigid_body_link(self.handle1)
+        let erp_inv_dt = params.contact_erp_inv_dt();
-            .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);
 
-        self.inner
+        // We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
-            .update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner);
+        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 {
+pub(crate) struct GenericContactConstraint {
-    // We just build the generic constraint on top of the velocity constraint,
+    /*
-    // adding some information we can use in the generic case.
+     * Fields specific to multibodies.
-    pub inner: TwoBodyConstraint,
+     */
     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<Real>, // Non-penetration force direction for the first body.
+    #[cfg(feature = "dim3")]
+    pub tangent1: Vector<Real>, // One of the friction force directions.
+    pub im1: Vector<Real>,
+    pub im2: Vector<Real>,
+    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<Real>; MAX_MANIFOLD_POINTS],
+    pub tangent_part: [ContactConstraintTangentPart<Real>; 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<Real>,
-        solver_vels: &mut [SolverVel<Real>],
+        bodies: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
     ) {
-        let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
+        let mut solver_vel1 = if self.solver_vel1 == u32::MAX {
-            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
+            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 {
+        let mut solver_vel2 = if self.solver_vel2 == u32::MAX {
-            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
+            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];
+        let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
-        TwoBodyConstraintElement::generic_warmstart_group(
+        let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
-            elements,
+        Self::generic_warmstart_group(
+            normal_parts,
+            tangent_parts,
             jacobians,
-            &self.inner.dir1,
+            &self.dir1,
             #[cfg(feature = "dim3")]
-            &self.inner.tangent1,
+            &self.tangent1,
-            &self.inner.im1,
+            &self.im1,
-            &self.inner.im2,
+            &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<Real>,
-        solver_vels: &mut [SolverVel<Real>],
+        bodies: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
         solve_restitution: bool,
         solve_friction: bool,
     ) {
-        let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
+        let mut solver_vel1 = if self.solver_vel1 == u32::MAX {
-            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
+            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 {
+        let mut solver_vel2 = if self.solver_vel2 == u32::MAX {
-            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
+            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];
+        let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
-        TwoBodyConstraintElement::generic_solve_group(
+        let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
-            self.inner.cfm_factor,
+        Self::generic_solve_group(
-            elements,
+            self.cfm_factor,
+            normal_parts,
+            tangent_parts,
             jacobians,
-            &self.inner.dir1,
+            &self.dir1,
             #[cfg(feature = "dim3")]
-            &self.inner.tangent1,
+            &self.tangent1,
-            &self.inner.im1,
+            &self.im1,
-            &self.inner.im2,
+            &self.im2,
-            self.inner.limit,
+            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;
+        }
     }
 }

src/dynamics/solver/contact_constraint/generic_contact_constraint_element.rs

@@ -1,7 +1,6 @@
 use crate::dynamics::solver::SolverVel;
-use crate::dynamics::solver::{
+use crate::dynamics::solver::contact_constraint::GenericContactConstraint;
-    TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart,
+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<Real>),
-    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<Real>,
         dir: &Vector<Real>,
-        gcross: &AngVector<Real>,
+        ii_torque_dir: &AngVector<Real>,
         solver_vels: &mut DVector<Real>,
         inv_mass: &Vector<Real>,
     ) {
         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<Real> {
+impl ContactConstraintTangentPart<Real> {
     #[inline]
     pub fn generic_warmstart(
         &mut self,
@@ -115,7 +117,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 self.impulse[0],
                 jacobians,
                 tangents1[0],
-                &self.gcross1[0],
+                &self.ii_torque_dir1[0],
                 solver_vels,
                 im1,
             );
@@ -125,7 +127,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 self.impulse[0],
                 jacobians,
                 &-tangents1[0],
-                &self.gcross2[0],
+                &self.ii_torque_dir2[0],
                 solver_vels,
                 im2,
             );
@@ -139,7 +141,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 self.impulse[0],
                 jacobians,
                 tangents1[0],
-                &self.gcross1[0],
+                &self.ii_torque_dir1[0],
                 solver_vels,
                 im1,
             );
@@ -149,7 +151,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 self.impulse[1],
                 jacobians,
                 tangents1[1],
-                &self.gcross1[1],
+                &self.ii_torque_dir1[1],
                 solver_vels,
                 im1,
             );
@@ -160,7 +162,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 self.impulse[0],
                 jacobians,
                 &-tangents1[0],
-                &self.gcross2[0],
+                &self.ii_torque_dir2[0],
                 solver_vels,
                 im2,
             );
@@ -170,7 +172,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 self.impulse[1],
                 jacobians,
                 &-tangents1[1],
-                &self.gcross2[1],
+                &self.ii_torque_dir2[1],
                 solver_vels,
                 im2,
             );
@@ -204,14 +206,14 @@ impl TwoBodyConstraintTangentPart<Real> {
                 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<Real> {
                 dlambda,
                 jacobians,
                 tangents1[0],
-                &self.gcross1[0],
+                &self.ii_torque_dir1[0],
                 solver_vels,
                 im1,
             );
@@ -235,7 +237,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 dlambda,
                 jacobians,
                 &-tangents1[0],
-                &self.gcross2[0],
+                &self.ii_torque_dir2[0],
                 solver_vels,
                 im2,
             );
@@ -248,14 +250,14 @@ impl TwoBodyConstraintTangentPart<Real> {
                 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<Real> {
                 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<Real> {
                 dlambda[0],
                 jacobians,
                 tangents1[0],
-                &self.gcross1[0],
+                &self.ii_torque_dir1[0],
                 solver_vels,
                 im1,
             );
@@ -299,7 +301,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 dlambda[1],
                 jacobians,
                 tangents1[1],
-                &self.gcross1[1],
+                &self.ii_torque_dir1[1],
                 solver_vels,
                 im1,
             );
@@ -310,7 +312,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 dlambda[0],
                 jacobians,
                 &-tangents1[0],
-                &self.gcross2[0],
+                &self.ii_torque_dir2[0],
                 solver_vels,
                 im2,
             );
@@ -320,7 +322,7 @@ impl TwoBodyConstraintTangentPart<Real> {
                 dlambda[1],
                 jacobians,
                 &-tangents1[1],
-                &self.gcross2[1],
+                &self.ii_torque_dir2[1],
                 solver_vels,
                 im2,
             );
@@ -328,7 +330,7 @@ impl TwoBodyConstraintTangentPart<Real> {
     }
 }
 
-impl TwoBodyConstraintNormalPart<Real> {
+impl ContactConstraintNormalPart<Real> {
     #[inline]
     pub fn generic_warmstart(
         &mut self,
@@ -352,7 +354,7 @@ impl TwoBodyConstraintNormalPart<Real> {
             self.impulse,
             jacobians,
             dir1,
-            &self.gcross1,
+            &self.ii_torque_dir1,
             solver_vels,
             im1,
         );
@@ -362,7 +364,7 @@ impl TwoBodyConstraintNormalPart<Real> {
             self.impulse,
             jacobians,
             &-dir1,
-            &self.gcross2,
+            &self.ii_torque_dir2,
             solver_vels,
             im2,
         );
@@ -386,9 +388,21 @@ impl TwoBodyConstraintNormalPart<Real> {
         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)
+        let dvel = solver_vel1.dvel(
-            + solver_vel2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, solver_vels)
+            j_id1,
-            + self.rhs;
+            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<Real> {
             dlambda,
             jacobians,
             dir1,
-            &self.gcross1,
+            &self.ii_torque_dir1,
             solver_vels,
             im1,
         );
@@ -410,17 +424,18 @@ impl TwoBodyConstraintNormalPart<Real> {
             dlambda,
             jacobians,
             &-dir1,
-            &self.gcross2,
+            &self.ii_torque_dir2,
             solver_vels,
             im2,
         );
     }
 }
 
-impl TwoBodyConstraintElement<Real> {
+impl GenericContactConstraint {
     #[inline]
     pub fn generic_warmstart_group(
-        elements: &mut [Self],
+        normal_parts: &mut [ContactConstraintNormalPart<Real>],
+        tangent_parts: &mut [ContactConstraintTangentPart<Real>],
         jacobians: &DVector<Real>,
         dir1: &Vector<Real>,
         #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
@@ -442,8 +457,8 @@ impl TwoBodyConstraintElement<Real> {
         {
             let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
 
-            for element in elements.iter_mut() {
+            for normal_part in normal_parts {
-                element.normal_part.generic_warmstart(
+                normal_part.generic_warmstart(
                     nrm_j_id,
                     jacobians,
                     dir1,
@@ -467,9 +482,8 @@ impl TwoBodyConstraintElement<Real> {
             let tangents1 = [&dir1.orthonormal_vector()];
             let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
 
-            for element in elements.iter_mut() {
+            for tangent_part in tangent_parts {
-                let part = &mut element.tangent_part;
+                tangent_part.generic_warmstart(
-                part.generic_warmstart(
                     tng_j_id,
                     jacobians,
                     tangents1,
@@ -489,7 +503,8 @@ impl TwoBodyConstraintElement<Real> {
     #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,
-        elements: &mut [Self],
+        normal_parts: &mut [ContactConstraintNormalPart<Real>],
+        tangent_parts: &mut [ContactConstraintTangentPart<Real>],
         jacobians: &DVector<Real>,
         dir1: &Vector<Real>,
         #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
@@ -514,8 +529,8 @@ impl TwoBodyConstraintElement<Real> {
         if solve_restitution {
             let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
 
-            for element in elements.iter_mut() {
+            for normal_part in &mut *normal_parts {
-                element.normal_part.generic_solve(
+                normal_part.generic_solve(
                     cfm_factor,
                     nrm_j_id,
                     jacobians,
@@ -540,10 +555,9 @@ impl TwoBodyConstraintElement<Real> {
             let tangents1 = [&dir1.orthonormal_vector()];
             let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
 
-            for element in elements.iter_mut() {
+            for (normal_part, tangent_part) in normal_parts.iter().zip(tangent_parts.iter_mut()) {
-                let limit = limit * element.normal_part.impulse;
+                let limit = limit * normal_part.impulse;
-                let part = &mut element.tangent_part;
+                tangent_part.generic_solve(
-                part.generic_solve(
                     tng_j_id,
                     jacobians,
                     tangents1,

src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs

@@ -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<Real>,
-        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<Real>,
-        generic_solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real>,
-        generic_solver_vels: &mut DVector<Real>,
-        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();
-    }
-}

src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs

@@ -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<Real> {
-    #[inline]
-    pub fn generic_warmstart(
-        &mut self,
-        j_id2: usize,
-        jacobians: &DVector<Real>,
-        ndofs2: usize,
-        solver_vel2: usize,
-        solver_vels: &mut DVector<Real>,
-    ) {
-        #[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<Real>,
-        ndofs2: usize,
-        limit: Real,
-        solver_vel2: usize,
-        solver_vels: &mut DVector<Real>,
-    ) {
-        #[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<Real> {
-    #[inline]
-    pub fn generic_warmstart(
-        &mut self,
-        j_id2: usize,
-        jacobians: &DVector<Real>,
-        ndofs2: usize,
-        solver_vel2: usize,
-        solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real>,
-        ndofs2: usize,
-        solver_vel2: usize,
-        solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real> {
-    #[inline]
-    pub fn generic_warmstart_group(
-        elements: &mut [Self],
-        jacobians: &DVector<Real>,
-        ndofs2: usize,
-        // Jacobian index of the first constraint.
-        j_id: usize,
-        solver_vel2: usize,
-        solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real>,
-        limit: Real,
-        ndofs2: usize,
-        // Jacobian index of the first constraint.
-        j_id: usize,
-        solver_vel2: usize,
-        solver_vels: &mut DVector<Real>,
-        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;
-            }
-        }
-    }
-}

src/dynamics/solver/contact_constraint/mod.rs

@@ -1,29 +1,61 @@
-pub(crate) use generic_one_body_constraint::*;
+pub(crate) use contact_constraint_element::*;
-// pub(crate) use generic_one_body_constraint_element::*;
+pub(crate) use contact_constraints_set::{ConstraintsCounts, ContactConstraintsSet};
-pub(crate) use contact_constraints_set::{
+pub(crate) use contact_with_coulomb_friction::*;
-    ConstraintsCounts, ContactConstraintTypes, ContactConstraintsSet,
+pub(crate) use generic_contact_constraint::*;
-};
+pub(crate) use generic_contact_constraint_element::*;
-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::*;
 
+#[cfg(feature = "dim3")]
+pub(crate) use contact_with_twist_friction::*;
+
+mod contact_constraint_element;
 mod contact_constraints_set;
-mod generic_one_body_constraint;
+mod contact_with_coulomb_friction;
-mod generic_one_body_constraint_element;
+mod generic_contact_constraint;
-mod generic_two_body_constraint;
+mod generic_contact_constraint_element;
-mod generic_two_body_constraint_element;
+
-mod one_body_constraint;
+mod any_contact_constraint;
-mod one_body_constraint_element;
+#[cfg(feature = "dim3")]
-#[cfg(feature = "simd-is-enabled")]
+mod contact_with_twist_friction;
-mod one_body_constraint_simd;
+
-mod two_body_constraint;
+#[cfg(feature = "dim3")]
-mod two_body_constraint_element;
+use crate::{
-#[cfg(feature = "simd-is-enabled")]
+    math::{DIM, Real, Vector},
-mod two_body_constraint_simd;
+    utils::{DisableFloatingPointExceptionsFlags, SimdBasis, SimdRealCopy},
+};
+
+#[inline]
+#[cfg(feature = "dim3")]
+pub(crate) fn compute_tangent_contact_directions<N>(
+    force_dir1: &Vector<N>,
+    linvel1: &Vector<N>,
+    linvel2: &Vector<N>,
+) -> [Vector<N>; DIM - 1]
+where
+    N: SimdRealCopy,
+    Vector<N>: 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]
+}

src/dynamics/solver/contact_constraint/one_body_constraint.rs

@@ -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<N: SimdRealCopy> {
-    pub tangent_vel: Vector<N>,
-    pub local_p1: Point<N>,
-    pub local_p2: Point<N>,
-    pub dist: N,
-    pub normal_rhs_wo_bias: N,
-}
-
-impl<N: SimdRealCopy> Default for ContactPointInfos<N> {
-    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<Real>; 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<Real>,
-        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<Real>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<Real>, // One of the friction force directions.
-    pub im2: Vector<Real>,
-    pub cfm_factor: Real,
-    pub limit: Real,
-    pub elements: [OneBodyConstraintElement<Real>; 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<Real>]) {
-        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<Real>],
-        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;
-        }
-    }
-}

src/dynamics/solver/contact_constraint/one_body_constraint_element.rs

@@ -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<N: SimdRealCopy> {
-    pub gcross2: [AngVector<N>; DIM - 1],
-    pub rhs: [N; DIM - 1],
-    pub rhs_wo_bias: [N; DIM - 1],
-    pub impulse: TangentImpulse<N>,
-    pub impulse_accumulator: TangentImpulse<N>,
-    #[cfg(feature = "dim2")]
-    pub r: [N; 1],
-    #[cfg(feature = "dim3")]
-    pub r: [N; DIM],
-}
-
-impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
-    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<N> {
-        self.impulse_accumulator + self.impulse
-    }
-
-    #[inline]
-    pub fn warmstart(
-        &mut self,
-        tangents1: [&Vector<N>; DIM - 1],
-        im2: &Vector<N>,
-        solver_vel2: &mut SolverVel<N>,
-    ) {
-        #[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<N>; DIM - 1],
-        im2: &Vector<N>,
-        limit: N,
-        solver_vel2: &mut SolverVel<N>,
-    ) where
-        AngVector<N>: SimdDot<AngVector<N>, 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<N: SimdRealCopy> {
-    pub gcross2: AngVector<N>,
-    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<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
-    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<N>, im2: &Vector<N>, solver_vel2: &mut SolverVel<N>) {
-        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<N>,
-        im2: &Vector<N>,
-        solver_vel2: &mut SolverVel<N>,
-    ) where
-        AngVector<N>: SimdDot<AngVector<N>, 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<N>,
-        im2: &Vector<N>,
-        solver_vel2: &mut SolverVel<N>,
-    ) where
-        AngVector<N>: SimdDot<AngVector<N>, 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<N: SimdRealCopy> {
-    pub normal_part: OneBodyConstraintNormalPart<N>,
-    pub tangent_part: OneBodyConstraintTangentPart<N>,
-}
-
-impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
-    pub fn zero() -> Self {
-        Self {
-            normal_part: OneBodyConstraintNormalPart::zero(),
-            tangent_part: OneBodyConstraintTangentPart::zero(),
-        }
-    }
-
-    #[inline]
-    pub fn warmstart_group(
-        elements: &mut [Self],
-        dir1: &Vector<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im2: &Vector<N>,
-        solver_vel2: &mut SolverVel<N>,
-    ) {
-        #[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<N>,
-        #[cfg(feature = "dim3")] tangent1: &Vector<N>,
-        im2: &Vector<N>,
-        limit: N,
-        solver_vel2: &mut SolverVel<N>,
-        solve_normal: bool,
-        solve_friction: bool,
-    ) where
-        Vector<N>: SimdBasis,
-        AngVector<N>: SimdDot<AngVector<N>, 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);
-            }
-        }
-    }
-}

src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs

@@ -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<SimdReal>; 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<SimdReal> =
-            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
-
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
-
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::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<SimdReal>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
-    pub elements: [OneBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub num_contacts: u8,
-    pub im2: Vector<SimdReal>,
-    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<Real>]) {
-        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<Real>],
-        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;
-        }
-    }
-}

src/dynamics/solver/contact_constraint/two_body_constraint.rs

@@ -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<Real>,
-        solver_vels: &mut [SolverVel<Real>],
-        generic_solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real>,
-        solver_vels: &mut [SolverVel<Real>],
-        generic_solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real>,
-        solver_vels: &mut [SolverVel<Real>],
-        generic_solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<Real>, // One of the friction force directions.
-    pub im1: Vector<Real>,
-    pub im2: Vector<Real>,
-    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<Real>; 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<Real>; 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<Real>,
-        rb2_pos: &Isometry<Real>,
-        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<Real>]) {
-        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<Real>],
-        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<N>(
-    force_dir1: &Vector<N>,
-    linvel1: &Vector<N>,
-    linvel2: &Vector<N>,
-) -> [Vector<N>; DIM - 1]
-where
-    N: utils::SimdRealCopy,
-    Vector<N>: 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]
-}

src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs

@@ -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<SimdReal>; 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<SimdReal> =
-            AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
-
-        let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
-        let angvel1 = AngVector::<SimdReal>::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<SimdReal> =
-            AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
-
-        let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
-        let angvel2 = AngVector::<SimdReal>::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<SimdReal>, // Non-penetration force direction for the first body.
-    #[cfg(feature = "dim3")]
-    pub tangent1: Vector<SimdReal>, // One of the friction force directions.
-    pub elements: [TwoBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
-    pub num_contacts: u8,
-    pub im1: Vector<SimdReal>,
-    pub im2: Vector<SimdReal>,
-    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<Real>]) {
-        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<Real>],
-        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;
-        }
-    }
-}

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 =
+                    let color_mask = bcolors[rb1.ids.active_set_offset as usize]
-                        bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset];
+                        | 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[rb1.ids.active_set_offset as usize] |= 1 << *color;
-                    bcolors[rb2.ids.active_set_offset] |= 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<u128>,
-    #[cfg(feature = "simd-is-enabled")]
+    pub simd_interactions: Vec<ContactManifoldIndex>,
-    pub simd_interactions: Vec<usize>,
+    pub nongrouped_interactions: Vec<ContactManifoldIndex>,
-    pub nongrouped_interactions: Vec<usize>,
 }
 
 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_fixed1 = !rb1.is_dynamic_or_kinematic();
-            let is_fixed2 = !rb2.is_dynamic();
+            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 =
+            let conflicts = self
-                self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
+                .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_fixed1 = !status1.is_dynamic_or_kinematic();
-                let is_fixed2 = !status2.is_dynamic();
+                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 mask1 = if !is_fixed1 {
-                let mask2 = if !is_fixed2 { self.body_masks[i2] } else { 0 };
+                    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;
                 }
             }
 

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,
                 &params,
                 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(
+        self.velocity_solver
-            base_params,
+            .writeback_bodies(base_params, islands, island_id, bodies, multibodies);
-            num_solver_iterations,
-            islands,
-            island_id,
-            bodies,
-            multibodies,
-        );
         counters.solver.velocity_writeback_time.pause();
     }
 }

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::{
+use crate::dynamics::solver::joint_constraint::generic_joint_constraint::GenericJointConstraint;
-    JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
+use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::JointConstraint;
-};
-use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
-    JointOneBodyConstraint, JointTwoBodyConstraint,
-};
-use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes};
 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::{
+use crate::math::{SIMD_WIDTH, SimdReal};
-    dynamics::solver::joint_constraint::joint_constraint_builder::{
-        JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
-    },
-    math::{SIMD_WIDTH, SimdReal},
-};
 
-use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
+use crate::dynamics::solver::solver_body::SolverBodies;
-    JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
-};
 
-pub struct JointConstraintTypes;
+#[derive(Debug)]
+pub enum AnyJointConstraintMut<'a> {
+    Generic(&'a mut GenericJointConstraint),
+    Rigid(&'a mut JointConstraint<Real, 1>),
+    #[cfg(feature = "simd-is-enabled")]
+    SimdRigid(&'a mut JointConstraint<SimdReal, SIMD_WIDTH>),
+}
 
-impl AnyConstraintMut<'_, JointConstraintTypes> {
+impl AnyJointConstraintMut<'_> {
     pub fn remove_bias(&mut self) {
         match self {
-            Self::OneBody(c) => c.remove_bias_from_rhs(),
+            Self::Rigid(c) => c.remove_bias_from_rhs(),
-            Self::TwoBodies(c) => c.remove_bias_from_rhs(),
+            Self::Generic(c) => c.remove_bias_from_rhs(),
-            Self::GenericOneBody(c) => c.remove_bias_from_rhs(),
-            Self::GenericTwoBodies(c) => c.remove_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
-            Self::SimdOneBody(c) => c.remove_bias_from_rhs(),
+            Self::SimdRigid(c) => c.remove_bias_from_rhs(),
-            #[cfg(feature = "simd-is-enabled")]
-            Self::SimdTwoBodies(c) => c.remove_bias_from_rhs(),
         }
     }
 
     pub fn solve(
         &mut self,
         generic_jacobians: &DVector<Real>,
-        solver_vels: &mut [SolverVel<Real>],
+        solver_vels: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
     ) {
         match self {
-            Self::OneBody(c) => c.solve(solver_vels),
+            Self::Rigid(c) => c.solve(solver_vels),
-            Self::TwoBodies(c) => c.solve(solver_vels),
+            Self::Generic(c) => c.solve(generic_jacobians, solver_vels, generic_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)
-            }
             #[cfg(feature = "simd-is-enabled")]
-            Self::SimdOneBody(c) => c.solve(solver_vels),
+            Self::SimdRigid(c) => c.solve(solver_vels),
-            #[cfg(feature = "simd-is-enabled")]
-            Self::SimdTwoBodies(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::Rigid(c) => c.writeback_impulses(joints_all),
-            Self::TwoBodies(c) => c.writeback_impulses(joints_all),
+            Self::Generic(c) => c.writeback_impulses(joints_all),
-            Self::GenericOneBody(c) => c.writeback_impulses(joints_all),
-            Self::GenericTwoBodies(c) => c.writeback_impulses(joints_all),
             #[cfg(feature = "simd-is-enabled")]
-            Self::SimdOneBody(c) => c.writeback_impulses(joints_all),
+            Self::SimdRigid(c) => c.writeback_impulses(joints_all),
-            #[cfg(feature = "simd-is-enabled")]
-            Self::SimdTwoBodies(c) => c.writeback_impulses(joints_all),
         }
     }
 }
-
-impl ConstraintTypes for JointConstraintTypes {
-    type OneBody = JointOneBodyConstraint<Real, 1>;
-    type TwoBodies = JointTwoBodyConstraint<Real, 1>;
-    type GenericOneBody = JointGenericOneBodyConstraint;
-    type GenericTwoBodies = JointGenericTwoBodyConstraint;
-
-    #[cfg(feature = "simd-is-enabled")]
-    type SimdOneBody = JointOneBodyConstraint<SimdReal, SIMD_WIDTH>;
-    #[cfg(feature = "simd-is-enabled")]
-    type SimdTwoBodies = JointTwoBodyConstraint<SimdReal, SIMD_WIDTH>;
-
-    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;
-}

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<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        mb1: LinkOrBodyRef,
+        mb2: LinkOrBodyRef,
+        frame1: &Isometry<Real>,
+        frame2: &Isometry<Real>,
+        joint: &GenericJoint,
+        jacobians: &mut DVector<Real>,
+        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<Real>,
+    ) -> 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<Real>,
+    ) -> 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<Real>,
+    ) -> 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<Real>,
+    ) -> 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<Real>,
+        solver_vels: &mut SolverBodies,
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        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;
+    }
+}

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<Real, 1>,
+    local_body2: JointSolverBody<Real, 1>,
+    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<Real>,
+        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<Real>,
+        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<Real>,
+        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<Real>,
+        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<Real, 1> {
+    pub fn fill_jacobians(
+        &self,
+        unit_force: Vector<Real>,
+        unit_torque: SVector<Real, ANG_DIM>,
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
+    ) {
+        let wj_id = *j_id + SPATIAL_DIM;
+        jacobians
+            .fixed_rows_mut::<DIM>(*j_id)
+            .copy_from(&unit_force);
+        jacobians
+            .fixed_rows_mut::<ANG_DIM>(*j_id + DIM)
+            .copy_from(&unit_torque);
+
+        {
+            let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id);
+            out_invm_j
+                .fixed_rows_mut::<DIM>(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::<ANG_DIM>(DIM).gemv(
+                    1.0,
+                    &self.ii.into_matrix(),
+                    &unit_torque,
+                    0.0,
+                );
+            }
+        }
+
+        *j_id += SPATIAL_DIM * 2;
+    }
+}
+
+impl JointConstraintHelper<Real> {
+    pub fn lock_jacobians_generic(
+        &self,
+        jacobians: &mut DVector<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        mb1: LinkOrBodyRef,
+        mb2: LinkOrBodyRef,
+        writeback_id: WritebackId,
+        lin_jac: Vector<Real>,
+        ang_jac1: SVector<Real, ANG_DIM>,
+        ang_jac2: SVector<Real, ANG_DIM>,
+    ) -> 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<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        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<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        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<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        mb1: LinkOrBodyRef,
+        mb2: LinkOrBodyRef,
+        motor_axis: usize,
+        motor_params: &MotorParameters<Real>,
+        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<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        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<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        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<Real>,
+        j_id: &mut usize,
+        joint_id: JointIndex,
+        body1: &JointSolverBody<Real, 1>,
+        body2: &JointSolverBody<Real, 1>,
+        mb1: LinkOrBodyRef,
+        mb2: LinkOrBodyRef,
+        _motor_axis: usize,
+        motor_params: &MotorParameters<Real>,
+        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<Real>,
+        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;
+            }
+        }
+    }
+}

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,
+    AnyJointConstraintMut, GenericJointConstraint, JointGenericExternalConstraintBuilder,
-    JointGenericTwoBodyConstraint, JointGenericTwoBodyConstraintBuilder,
+    JointGenericInternalConstraintBuilder, reset_buffer,
-    JointGenericVelocityOneBodyExternalConstraintBuilder,
-    JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet, 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::{
+use crate::dynamics::solver::interaction_groups::InteractionGroups;
-    JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
+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::{
+    crate::dynamics::solver::joint_constraint::joint_constraint_builder::JointConstraintBuilderSimd,
-        JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
+    crate::math::{SIMD_WIDTH, SimdReal},
-    },
-    crate::math::SIMD_WIDTH,
 };
 
-pub type JointConstraintsSet = SolverConstraintsSet<JointConstraintTypes>;
+pub struct JointConstraintsSet {
+    pub generic_jacobians: DVector<Real>,
+    pub two_body_interactions: Vec<usize>,
+    pub generic_two_body_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+
+    pub generic_velocity_constraints: Vec<GenericJointConstraint>,
+    pub velocity_constraints: Vec<JointConstraint<Real, 1>>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub simd_velocity_constraints: Vec<JointConstraint<SimdReal, SIMD_WIDTH>>,
+
+    pub generic_velocity_constraints_builder: Vec<GenericJointConstraintBuilder>,
+    pub velocity_constraints_builder: Vec<JointConstraintBuilder>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub simd_velocity_constraints_builder: Vec<JointConstraintBuilderSimd>,
+}
+
+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<Real>,
+        impl Iterator<Item = AnyJointConstraintMut<'_>>,
+    ) {
+        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_generic_joint_constraints(
-
-        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(
             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(
+                if JointGenericInternalConstraintBuilder::num_constraints(multibodies, link_id) > 0
-                    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(
+        // Preallocate builders buffer.
-            total_num_builders,
+        self.generic_velocity_constraints_builder
-            JointGenericOneBodyConstraintBuilder::invalid(),
+            .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(
+        for (joint_i, builder) in self
-            self.generic_velocity_one_body_constraints_builder
+            .generic_two_body_interactions
-                .iter_mut(),
+            .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 constraints buffer now that we know the total count.
-            .resize(num_constraints, JointGenericOneBodyConstraint::invalid());
+        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 joints_id = array![|ii| joints_i[ii]];
-            let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
+            let impulse_joints = array![|ii| &joints_all[joints_i[ii]].weight];
-            JointTwoBodyConstraintBuilderSimd::generate(
+            JointConstraintBuilderSimd::generate(
                 impulse_joints,
                 bodies,
                 joints_id,
@@ -364,7 +297,7 @@ impl JointConstraintsSet {
     #[profiling::function]
     pub fn solve(
         &mut self,
-        solver_vels: &mut [SolverVel<Real>],
+        solver_vels: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
     ) {
         let (jac, constraints) = self.iter_constraints_mut();
@@ -375,7 +308,7 @@ impl JointConstraintsSet {
 
     pub fn solve_wo_bias(
         &mut self,
-        solver_vels: &mut [SolverVel<Real>],
+        solver_vels: &mut SolverBodies,
         generic_solver_vels: &mut DVector<Real>,
     ) {
         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(
+            match builder {
-                params,
+                GenericJointConstraintBuilder::External(builder) => {
-                multibodies,
+                    builder.update(
-                solver_bodies,
+                        params,
-                &mut self.generic_jacobians,
+                        multibodies,
-                &mut self.generic_velocity_constraints,
+                        solver_bodies,
-            );
+                        &mut self.generic_jacobians,
-        }
+                        &mut self.generic_velocity_constraints,
-
+                    );
-        for builder in &mut self.generic_velocity_one_body_constraints_builder {
+                }
-            builder.update(
+                GenericJointConstraintBuilder::Internal(builder) => {
-                params,
+                    builder.update(
-                multibodies,
+                        params,
-                solver_bodies,
+                        multibodies,
-                &mut self.generic_jacobians,
+                        &mut self.generic_jacobians,
-                &mut self.generic_velocity_one_body_constraints,
+                        &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,
-            );
-        }
     }
 }

src/dynamics/solver/joint_constraint/joint_generic_constraint.rs

@@ -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<Real, 1>,
-        body2: &JointSolverBody<Real, 1>,
-        mb1: Option<(&Multibody, usize)>,
-        mb2: Option<(&Multibody, usize)>,
-        frame1: &Isometry<Real>,
-        frame2: &Isometry<Real>,
-        joint: &GenericJoint,
-        jacobians: &mut DVector<Real>,
-        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<Real>],
-        generic_solver_vels: &'a DVector<Real>,
-    ) -> 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<Real>],
-        generic_solver_vels: &'a mut DVector<Real>,
-    ) -> 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<Real>],
-        generic_solver_vels: &'a DVector<Real>,
-    ) -> 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<Real>],
-        generic_solver_vels: &'a mut DVector<Real>,
-    ) -> 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<Real>,
-        solver_vels: &mut [SolverVel<Real>],
-        generic_solver_vels: &mut DVector<Real>,
-    ) {
-        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<Real>,
-        body2: &JointSolverBody<Real, 1>,
-        mb2: (&Multibody, usize),
-        frame1: &Isometry<Real>,
-        frame2: &Isometry<Real>,
-        joint: &GenericJoint,
-        jacobians: &mut DVector<Real>,
-        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<Real>],
-        generic_solver_vels: &'a DVector<Real>,
-    ) -> DVectorView<'a, Real> {
-        generic_solver_vels.rows(self.solver_vel2, self.ndofs2)
-    }
-
-    fn solver_vel2_mut<'a>(
-        &self,
-        _solver_vels: &'a mut [SolverVel<Real>],
-        generic_solver_vels: &'a mut DVector<Real>,
-    ) -> DVectorViewMut<'a, Real> {
-        generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2)
-    }
-
-    pub fn solve(
-        &mut self,
-        jacobians: &DVector<Real>,
-        solver_vels: &mut [SolverVel<Real>],
-        generic_solver_vels: &mut DVector<Real>,
-    ) {
-        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;
-    }
-}

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 {
+use crate::math::{SIMD_WIDTH, SimdReal};
-    crate::math::{SIMD_WIDTH, SimdReal},
+#[cfg(feature = "dim2")]
-    na::SimdValue,
+use crate::num::Zero;
-};
 
 #[derive(Copy, Clone, PartialEq, Debug)]
 pub struct MotorParameters<N: SimdRealCopy> {
@@ -50,44 +49,26 @@ pub enum WritebackId {
 #[derive(Copy, Clone)]
 pub struct JointSolverBody<N: SimdRealCopy, const LANES: usize> {
     pub im: Vector<N>,
-    pub sqrt_ii: AngularInertia<N>,
+    pub ii: AngularInertia<N>,
-    pub world_com: Point<N>,
+    pub world_com: Point<N>, // TODO: is this still needed now that the solver body poses are expressed at the center of mass?
-    pub solver_vel: [usize; LANES],
+    pub solver_vel: [u32; LANES],
 }
 
 impl<N: SimdRealCopy, const LANES: usize> JointSolverBody<N, LANES> {
     pub fn invalid() -> Self {
         Self {
             im: Vector::zeros(),
-            sqrt_ii: AngularInertia::zero(),
+            ii: AngularInertia::zero(),
-            world_com: Point::origin(),
-            solver_vel: [usize::MAX; LANES],
-        }
-    }
-}
-
-#[derive(Copy, Clone)]
-pub struct JointFixedSolverBody<N: SimdRealCopy> {
-    pub linvel: Vector<N>,
-    pub angvel: AngVector<N>,
-    // TODO: is this really needed?
-    pub world_com: Point<N>,
-}
-
-impl<N: SimdRealCopy> JointFixedSolverBody<N> {
-    pub fn invalid() -> Self {
-        Self {
-            linvel: Vector::zeros(),
-            angvel: AngVector::zero(),
             world_com: Point::origin(),
+            solver_vel: [u32::MAX; LANES],
         }
     }
 }
 
 #[derive(Debug, Copy, Clone)]
-pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
+pub struct JointConstraint<N: SimdRealCopy, const LANES: usize> {
-    pub solver_vel1: [usize; LANES],
+    pub solver_vel1: [u32; LANES],
-    pub solver_vel2: [usize; LANES],
+    pub solver_vel2: [u32; LANES],
 
     pub joint_id: [JointIndex; LANES],
 
@@ -97,6 +78,9 @@ pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
     pub ang_jac1: AngVector<N>,
     pub ang_jac2: AngVector<N>,
 
+    pub ii_ang_jac1: AngVector<N>,
+    pub ii_ang_jac2: AngVector<N>,
+
     pub inv_lhs: N,
     pub rhs: N,
     pub rhs_wo_bias: N,
@@ -109,7 +93,7 @@ pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
     pub writeback_id: WritebackId,
 }
 
-impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
+impl<N: SimdRealCopy, const LANES: usize> JointConstraint<N, LANES> {
     #[profiling::function]
     pub fn solve_generic(
         &mut self,
@@ -127,13 +111,13 @@ impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
         self.impulse = total_impulse;
 
         let lin_impulse = self.lin_jac * delta_impulse;
-        let ang_impulse1 = self.ang_jac1 * delta_impulse;
+        let ii_ang_impulse1 = self.ii_ang_jac1 * delta_impulse;
-        let ang_impulse2 = self.ang_jac2 * 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<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
     }
 }
 
-impl JointTwoBodyConstraint<Real, 1> {
+impl JointConstraint<Real, 1> {
-    pub fn lock_axes(
+    pub fn update(
         params: &IntegrationParameters,
         joint_id: JointIndex,
         body1: &JointSolverBody<Real, 1>,
@@ -169,7 +153,7 @@ impl JointTwoBodyConstraint<Real, 1> {
         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<Real, 1> {
             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<Real, 1> {
             );
             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<Real>]) {
+    pub fn solve(&mut self, solver_vels: &mut SolverBodies) {
-        let mut solver_vel1 = solver_vels[self.solver_vel1[0]];
+        let mut solver_vel1 = solver_vels.get_vel(self.solver_vel1[0]);
-        let mut solver_vel2 = solver_vels[self.solver_vel2[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.set_vel(self.solver_vel1[0], solver_vel1);
-        solver_vels[self.solver_vel2[0]] = solver_vel2;
+        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<Real, 1> {
 }
 
 #[cfg(feature = "simd-is-enabled")]
-impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
+impl JointConstraint<SimdReal, SIMD_WIDTH> {
-    pub fn lock_axes(
+    pub fn update(
         params: &IntegrationParameters,
         joint_id: [JointIndex; SIMD_WIDTH],
         body1: &JointSolverBody<SimdReal, SIMD_WIDTH>,
@@ -362,7 +346,7 @@ impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
         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<SimdReal, SIMD_WIDTH> {
             }
         }
 
-        JointTwoBodyConstraintHelper::finalize_constraints(&mut out[..len]);
+        JointConstraintHelper::finalize_constraints(&mut out[..len]);
         len
     }
 
-    pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
+    pub fn solve(&mut self, solver_vels: &mut SolverBodies) {
-        let mut solver_vel1 = SolverVel {
+        let mut solver_vel1 = solver_vels.gather_vels(self.solver_vel1);
-            linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]),
+        let mut solver_vel2 = solver_vels.gather_vels(self.solver_vel2);
-            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]),
-        };
 
         self.solve_generic(&mut solver_vel1, &mut solver_vel2);
 
-        for ii in 0..SIMD_WIDTH {
+        solver_vels.scatter_vels(self.solver_vel1, solver_vel1);
-            solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii);
+        solver_vels.scatter_vels(self.solver_vel2, solver_vel2);
-            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);
-        }
     }
 
     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 {
-                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<N: SimdRealCopy, const LANES: usize> {
-    pub solver_vel2: [usize; LANES],
-
-    pub joint_id: [JointIndex; LANES],
-
-    pub impulse: N,
-    pub impulse_bounds: [N; 2],
-    pub lin_jac: Vector<N>,
-    pub ang_jac2: AngVector<N>,
-
-    pub inv_lhs: N,
-    pub cfm_coeff: N,
-    pub cfm_gain: N,
-    pub rhs: N,
-    pub rhs_wo_bias: N,
-
-    pub im2: Vector<N>,
-
-    pub writeback_id: WritebackId,
-}
-
-impl<N: SimdRealCopy, const LANES: usize> JointOneBodyConstraint<N, LANES> {
-    pub fn solve_generic(&mut self, solver_vel2: &mut SolverVel<N>) {
-        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<Real, 1> {
-    pub fn lock_axes(
-        params: &IntegrationParameters,
-        joint_id: JointIndex,
-        body1: &JointFixedSolverBody<Real>,
-        body2: &JointSolverBody<Real, 1>,
-        frame1: &Isometry<Real>,
-        frame2: &Isometry<Real>,
-        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<Real>]) {
-        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<SimdReal, SIMD_WIDTH> {
-    pub fn lock_axes(
-        params: &IntegrationParameters,
-        joint_id: [JointIndex; SIMD_WIDTH],
-        body1: &JointFixedSolverBody<SimdReal>,
-        body2: &JointSolverBody<SimdReal, SIMD_WIDTH>,
-        frame1: &Isometry<SimdReal>,
-        frame2: &Isometry<SimdReal>,
-        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<Real>]) {
-        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?
         for ii in 0..SIMD_WIDTH {
             let joint = &mut joints_all[self.joint_id[ii]].weight;
             match self.writeback_id {

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 any_joint_constraint::AnyJointConstraintMut;
-pub use joint_constraint_builder::JointTwoBodyConstraintHelper;
+pub use generic_joint_constraint::GenericJointConstraint;
-pub use joint_constraints_set::JointConstraintsSet;
+pub use generic_joint_constraint_builder::{
-pub use joint_generic_constraint::{JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint};
+    JointGenericExternalConstraintBuilder, JointGenericInternalConstraintBuilder, LinkOrBodyRef,
-pub use joint_generic_constraint_builder::{
-    JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
-    JointGenericVelocityOneBodyExternalConstraintBuilder,
-    JointGenericVelocityOneBodyInternalConstraintBuilder,
 };
+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;

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_body::SolverVel;
-use solver_constraints_set::{AnyConstraintMut, ConstraintTypes};
-use solver_vel::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<T>(buffer: &mut Vec<T>, 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);
     }
 }

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;
                             }
                         }

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<ContactConstraintTypes> {
                     }
                     #[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<JointConstraintTypes> {
                     }
                     #[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) => {

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,
+    IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet,
-    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;

src/dynamics/solver/solver_body.rs

@@ -1,59 +1,555 @@
-use crate::dynamics::{RigidBody, RigidBodyVelocity};
+use crate::dynamics::RigidBody;
-use crate::math::{AngularInertia, Isometry, Point, Real, Vector};
+use crate::math::{AngularInertia, Isometry, Real, SPATIAL_DIM, Vector};
-use crate::prelude::RigidBodyDamping;
+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")]
+#[cfg(feature = "simd-is-enabled")]
-use crate::num::Zero;
+use crate::utils::transmute_to_wide;
 
-#[derive(Copy, Clone, Debug)]
+#[cfg(feature = "simd-is-enabled")]
-pub(crate) struct SolverBody {
+macro_rules! aos(
-    pub position: Isometry<Real>,
+    ($data_repr: ident [ $idx: ident ] . $data_n: ident, $fallback: ident) => {
-    pub integrated_vels: RigidBodyVelocity,
+        [
-    pub im: Vector<Real>,
+            if ($idx[0] as usize) < $data_repr.len() {
-    pub sqrt_ii: AngularInertia<Real>,
+                $data_repr[$idx[0] as usize].$data_n.0
-    pub world_com: Point<Real>,
+            } else {
-    pub ccd_thickness: Real,
+                $fallback.$data_n.0
-    pub damping: RigidBodyDamping,
+            },
-    pub local_com: Point<Real>,
+            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<SolverVel<Real>>,
+    pub poses: Vec<SolverPose<Real>>,
 }
 
-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<SimdReal> {
+        #[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<SimdReal> {
+        #[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<Real> {
+        self.vels.get(i as usize).copied().unwrap_or_default()
+    }
+
+    #[inline]
+    pub fn scatter_vels(&mut self, idx: [u32; SIMD_WIDTH], vels: SolverVel<SimdReal>) {
+        #[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<Real>) {
+        if (i as usize) < self.vels.len() {
+            self.vels[i as usize] = vel;
+        }
+    }
+
+    #[inline]
+    pub fn get_pose(&self, i: u32) -> SolverPose<Real> {
+        self.poses.get(i as usize).copied().unwrap_or_default()
+    }
+
+    #[inline]
+    pub unsafe fn gather_poses_unchecked(&self, idx: [u32; SIMD_WIDTH]) -> SolverPose<SimdReal> {
+        #[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<SimdReal> {
+        #[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<SimdReal>) {
+        #[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<SimdReal>) {
+        #[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<T: SimdRealCopy> {
+    pub linear: Vector<T>,     // 2/3
+    pub angular: AngVector<T>, // 1/3
+    // TODO: explicit padding are useful for static assertions.
+    //       But might be wasteful for the SolverVel<SimdReal>
+    //       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<SimdReal> {
+    #[inline]
+    pub unsafe fn gather_unchecked(data: &[SolverVel<Real>], 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<Real>], 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<Real>], 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<Real>], 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<T> {
+    /// Positional change of the rigid-body’s center of mass.
+    pub pose: Isometry<T>, // 4/7
+    pub ii: AngularInertia<T>, // 1/6
+    pub im: Vector<T>,         // 2/3
+    #[cfg(feature = "dim2")]
+    pub padding: [T; 1],
+}
+
+impl Default for SolverPose<Real> {
+    #[inline]
     fn default() -> Self {
         Self {
-            position: Isometry::identity(),
+            pose: Isometry::identity(),
-            integrated_vels: RigidBodyVelocity::zero(),
+            ii: Default::default(),
-            im: na::zero(),
+            im: Default::default(),
-            sqrt_ii: AngularInertia::zero(),
+            #[cfg(feature = "dim2")]
-            world_com: Point::origin(),
+            padding: Default::default(),
-            ccd_thickness: 0.0,
-            damping: RigidBodyDamping::default(),
-            local_com: Point::origin(),
         }
     }
 }
 
-impl SolverBody {
+#[cfg(feature = "simd-is-enabled")]
-    pub fn from(rb: &RigidBody) -> Self {
+#[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<SimdReal> {
+    #[inline]
+    pub unsafe fn gather_unchecked(data: &[SolverPose<Real>], 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<Real>], 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<Real>], 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<Real>], 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<Real>], 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<Real>], 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<N: SimdRealCopy> SolverVel<N> {
+    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<N: SimdRealCopy> SolverVel<N> {
+    pub fn zero() -> Self {
         Self {
-            position: rb.pos.position,
+            linear: na::zero(),
-            integrated_vels: RigidBodyVelocity::zero(),
+            angular: na::zero(),
-            im: rb.mprops.effective_inv_mass,
+            #[cfg(feature = "simd-is-enabled")]
-            sqrt_ii: rb.mprops.effective_world_inv_inertia_sqrt,
+            #[cfg(feature = "dim2")]
-            world_com: rb.mprops.world_com,
+            padding: [na::zero(); 1],
-            ccd_thickness: rb.ccd.ccd_thickness,
+            #[cfg(feature = "simd-is-enabled")]
-            damping: rb.damping,
+            #[cfg(feature = "dim3")]
-            local_com: rb.mprops.local_mprops.local_com,
+            padding: [na::zero(); 2],
         }
     }
+}
 
-    pub fn copy_from(&mut self, rb: &RigidBody) {
+impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
-        self.position = rb.pos.position;
+    fn add_assign(&mut self, rhs: Self) {
-        self.integrated_vels = RigidBodyVelocity::zero();
+        self.linear += rhs.linear;
-        self.im = rb.mprops.effective_inv_mass;
+        self.angular += rhs.angular;
-        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;
+impl<N: SimdRealCopy> SubAssign for SolverVel<N> {
-        self.local_com = rb.mprops.local_mprops.local_com;
+    fn sub_assign(&mut self, rhs: Self) {
+        self.linear -= rhs.linear;
+        self.angular -= rhs.angular;
+    }
+}
+
+impl<N: SimdRealCopy> Sub for SolverVel<N> {
+    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,
+        }
     }
 }

src/dynamics/solver/solver_constraints_set.rs

@@ -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<Constraints: ConstraintTypes> {
-    pub generic_jacobians: DVector<Real>,
-    pub two_body_interactions: Vec<usize>,
-    pub one_body_interactions: Vec<usize>,
-    pub generic_two_body_interactions: Vec<usize>,
-    pub generic_one_body_interactions: Vec<usize>,
-    pub interaction_groups: InteractionGroups,
-    pub one_body_interaction_groups: InteractionGroups,
-
-    pub velocity_constraints: Vec<Constraints::TwoBodies>,
-    pub generic_velocity_constraints: Vec<Constraints::GenericTwoBodies>,
-    #[cfg(feature = "simd-is-enabled")]
-    pub simd_velocity_constraints: Vec<Constraints::SimdTwoBodies>,
-    pub velocity_one_body_constraints: Vec<Constraints::OneBody>,
-    pub generic_velocity_one_body_constraints: Vec<Constraints::GenericOneBody>,
-    #[cfg(feature = "simd-is-enabled")]
-    pub simd_velocity_one_body_constraints: Vec<Constraints::SimdOneBody>,
-
-    pub velocity_constraints_builder: Vec<Constraints::BuilderTwoBodies>,
-    pub generic_velocity_constraints_builder: Vec<Constraints::GenericBuilderTwoBodies>,
-    #[cfg(feature = "simd-is-enabled")]
-    pub simd_velocity_constraints_builder: Vec<Constraints::SimdBuilderTwoBodies>,
-    pub velocity_one_body_constraints_builder: Vec<Constraints::BuilderOneBody>,
-    pub generic_velocity_one_body_constraints_builder: Vec<Constraints::GenericBuilderOneBody>,
-    #[cfg(feature = "simd-is-enabled")]
-    pub simd_velocity_one_body_constraints_builder: Vec<Constraints::SimdBuilderOneBody>,
-}
-
-impl<Constraints: ConstraintTypes> SolverConstraintsSet<Constraints> {
-    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<Real>,
-        impl Iterator<Item = AnyConstraintMut<Constraints>>,
-    ) {
-        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));
-    }
-}

src/dynamics/solver/solver_vel.rs

@@ -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<N: Scalar + Copy> {
-    // The linear velocity of a solver body.
-    pub linear: Vector<N>,
-    // The angular velocity, multiplied by the inverse sqrt angular inertia, of a solver body.
-    pub angular: AngVector<N>,
-}
-
-impl<N: Scalar + Copy> SolverVel<N> {
-    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<N: SimdRealCopy> SolverVel<N> {
-    pub fn zero() -> Self {
-        Self {
-            linear: na::zero(),
-            angular: na::zero(),
-        }
-    }
-}
-
-impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
-    fn add_assign(&mut self, rhs: Self) {
-        self.linear += rhs.linear;
-        self.angular += rhs.angular;
-    }
-}
-
-impl<N: SimdRealCopy> SubAssign for SolverVel<N> {
-    fn sub_assign(&mut self, rhs: Self) {
-        self.linear -= rhs.linear;
-        self.angular -= rhs.angular;
-    }
-}
-
-impl<N: SimdRealCopy> Sub for SolverVel<N> {
-    type Output = Self;
-
-    fn sub(self, rhs: Self) -> Self {
-        SolverVel {
-            linear: self.linear - rhs.linear,
-            angular: self.angular - rhs.angular,
-        }
-    }
-}

src/dynamics/solver/velocity_solver.rs

@@ -1,19 +1,21 @@
-use super::{JointConstraintTypes, SolverConstraintsSet};
+use crate::dynamics::solver::JointConstraintsSet;
-use crate::dynamics::solver::solver_body::SolverBody;
+use crate::dynamics::solver::contact_constraint::ContactConstraintsSet;
+use crate::dynamics::solver::solver_body::SolverBodies;
 use crate::dynamics::{
     IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
-    MultibodyLinkId, RigidBodySet,
+    MultibodyLinkId, RigidBodySet, RigidBodyType, solver::SolverVel,
-    solver::{ContactConstraintTypes, 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<SolverBody>,
+    pub solver_bodies: SolverBodies,
-    pub solver_vels: Vec<SolverVel<Real>>,
     pub solver_vels_increment: Vec<SolverVel<Real>>,
     pub generic_solver_vels: DVector<Real>,
     pub generic_solver_vels_increment: DVector<Real>,
@@ -23,8 +25,7 @@ pub(crate) struct VelocitySolver {
 impl VelocitySolver {
     pub fn new() -> Self {
         Self {
-            solver_bodies: Vec::new(),
+            solver_bodies: SolverBodies::default(),
-            solver_vels: Vec::new(),
             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<ContactConstraintTypes>,
+        contact_constraints: &mut ContactConstraintsSet,
-        joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
+        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(),
+        let aligned_solver_bodies_len =
-            SolverBody::default(),
+            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());
+            .resize(aligned_solver_bodies_len, SolverVel::zero());
-        self.solver_vels.clear();
-        self.solver_vels
-            .resize(islands.active_island(island_id).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 =
-                let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset];
+                    &mut self.solver_vels_increment[rb.ids.active_set_offset as usize];
-                let solver_body = &mut self.solver_bodies[rb.ids.active_set_offset];
+                self.solver_bodies
-                solver_body.copy_from(rb);
+                    .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.
+        // TODO PERF: don’t reallocate at each iteration.
-        self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id);
+        self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id as usize);
-        self.generic_solver_vels = DVector::zeros(multibody_solver_id);
+        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<ContactConstraintTypes>,
+        contact_constraints: &mut ContactConstraintsSet,
-        joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
+        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);
+                joint_constraints.solve(&mut self.solver_bodies, &mut self.generic_solver_vels);
-                contact_constraints
+                contact_constraints.solve(&mut self.solver_bodies, &mut self.generic_solver_vels);
-                    .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);
-                }
             }
 
             /*
@@ -206,18 +200,11 @@ impl VelocitySolver {
             /*
              * Resolution without bias.
              */
-            if params.num_internal_stabilization_iterations > 0 {
+            for _ in 0..params.num_internal_stabilization_iterations {
-                for _ in 0..params.num_internal_stabilization_iterations {
+                joint_constraints
-                    joint_constraints
+                    .solve_wo_bias(&mut self.solver_bodies, &mut self.generic_solver_vels);
-                        .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,
-                    );
-                }
-
                 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_pose) in self
-        for (solver_vels, solver_body) in self.solver_vels.iter().zip(self.solver_bodies.iter_mut())
+            .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 };
+            // TODO: should we add a compile flag (or a simulation parameter)
-            new_vels = new_vels.apply_damping(params.dt, &solver_body.damping);
+            //       to disable the rotation linearization?
-            let new_pos =
+            let new_vels = RigidBodyVelocity { linvel, angvel };
-                new_vels.integrate(params.dt, &solver_body.position, &solver_body.local_com);
+            new_vels.integrate_linearized(params.dt, &mut solver_pose.pose);
-            solver_body.integrated_vels += new_vels;
-            solver_body.position = new_pos;
-            solver_body.world_com = new_pos * solver_body.local_com;
         }
 
+        // 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_bodies.vels[rb.ids.active_set_offset as usize];
-                let solver_vels = &self.solver_vels[rb.ids.active_set_offset];
+                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();
+                }
             }
         }
     }

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<Real>,
+    /// 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<Real, 1>;
+/// A group of `SIMD_WIDTH` solver contacts stored in SoA fashion for SIMD optimizations.
+pub type SimdSolverContact = SolverContactGeneric<SimdReal, SIMD_WIDTH>;
+
 /// 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 {
+#[cfg_attr(
-    /// The index of the manifold contact used to generate this solver contact.
+    feature = "serde-serialize",
-    pub(crate) contact_id: u8,
+    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<N: SimdRealCopy, const LANES: usize> {
+    // 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<Real>,
+    pub point: Point<N>, // 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<Real>,
+    pub tangent_velocity: Vector<N>, // 2/3
-    /// Whether or not this contact existed during the last timestep.
-    pub is_new: bool,
     /// 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<Real>,
+    pub warmstart_tangent_impulse: TangentImpulse<N>, // 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::<SimdSolverContactRepr>(),
+    align_of::<SolverContact>()
+);
+#[cfg(feature = "simd-is-enabled")]
+static_assertions::assert_eq_size!(SimdSolverContactRepr, SolverContact);
+static_assertions::const_assert_eq!(
+    align_of::<SimdSolverContact>(),
+    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<SimdReal, 4>>([
+                soa0, soa1, soa2,
+            ])
+        };
+        #[cfg(feature = "dim3")]
+        return unsafe {
+            std::mem::transmute::<[[wide::f32x4; 4]; 4], SolverContactGeneric<SimdReal, 4>>([
+                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 {
+    pub fn is_bouncy(&self) -> Real {
-        if self.is_new {
+        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
         }
     }
 }

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,

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 prox_id_remap = HashMap::default();
-        let mut contact_id_remap = HashMap::new();
+        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);

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<T>(mut callback: impl FnMut(usize) -> T) -> T {
+                callback(0usize)
+            }
+
+            #[inline(always)]
+            #[allow(dead_code)]
+            #[cfg(feature = "simd-is-enabled")]
             fn create_arr<T>(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<T>(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)
         }
     }

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() {
+        for handle in islands.active_bodies() {
-            let rb = bodies.index_mut_internal(handle);
+            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 => {
+                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);
-                }
                 _ => {}
             }
         }
@@ -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
+        // Expect body to now be in active_set
-        assert!(islands.active_dynamic_set.contains(&h));
+        assert!(islands.active_set.contains(&h));
     }
 
     #[test]

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,

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.
+                        && rb.is_dynamic_or_kinematic() // Only dynamic bodies are in the active dynamic set.
-                        && islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
+                        && 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.
+                        ids.active_set_id = islands.active_set.len(); // This will handle the case where the activation_channel contains duplicates.
-                        islands.active_dynamic_set.push(*handle);
+                        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;
-                        }
-                    }
                 };
             }
         }

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 parry::utils::SdpMatrix3;
-use simba::simd::SimdValue;
 use std::ops::IndexMut;
 
-use parry::utils::SdpMatrix3;
+#[cfg(feature = "simd-is-enabled")]
-use {
+use crate::{
-    crate::math::{Real, SimdReal},
+    math::{SIMD_WIDTH, SimdReal},
-    na::SimdPartialOrd,
+    num::Zero,
-    num::One,
 };
 
 /// 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<Element = Real> + 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<N: Scalar + Copy + SimdSign<N>> SimdSign<Vector3<N>> for Vector3<N> {
     }
 }
 
+#[cfg(feature = "simd-is-enabled")]
 impl SimdSign<SimdReal> 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<SimdReal>;
     type CrossMatTr = Matrix2<SimdReal>;
@@ -287,6 +289,7 @@ impl<N: SimdRealCopy> SimdDot<N> for Vector1<N> {
     }
 }
 
+#[cfg(feature = "simd-is-enabled")]
 impl SimdCross<Vector3<SimdReal>> for Vector3<SimdReal> {
     type Result = Vector3<SimdReal>;
 
@@ -295,6 +298,7 @@ impl SimdCross<Vector3<SimdReal>> for Vector3<SimdReal> {
     }
 }
 
+#[cfg(feature = "simd-is-enabled")]
 impl SimdCross<Vector2<SimdReal>> for SimdReal {
     type Result = Vector2<SimdReal>;
 
@@ -303,6 +307,7 @@ impl SimdCross<Vector2<SimdReal>> for SimdReal {
     }
 }
 
+#[cfg(feature = "simd-is-enabled")]
 impl SimdCross<Vector2<SimdReal>> for Vector2<SimdReal> {
     type Result = SimdReal;
 
@@ -354,7 +359,6 @@ pub(crate) trait SimdAngularInertia<N> {
     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<N: SimdRealCopy> SimdAngularInertia<N> for N {
         pt * *self
     }
 
-    fn squared(&self) -> N {
-        *self * *self
-    }
-
     fn into_matrix(self) -> Self::AngMatrix {
         self
     }
 }
 
-impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
+impl<N: SimdRealCopy> SimdAngularInertia<N> for SdpMatrix3<N> {
-    type AngVector = Vector3<Real>;
+    type AngVector = Vector3<N>;
-    type AngMatrix = Matrix3<Real>;
+    type AngMatrix = Matrix3<N>;
 
     fn inverse(&self) -> Self {
         let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23;
@@ -405,18 +405,7 @@ impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
         }
     }
 
-    fn squared(&self) -> Self {
+    fn transform_vector(&self, v: Vector3<N>) -> Vector3<N> {
-        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<Real>) -> Vector3<Real> {
         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<Real> for SdpMatrix3<Real> {
     }
 
     #[rustfmt::skip]
-    fn into_matrix(self) -> Matrix3<Real> {
+    fn into_matrix(self) -> Matrix3<N> {
-        Matrix3::new(
-            self.m11, self.m12, self.m13,
-            self.m12, self.m22, self.m23,
-            self.m13, self.m23, self.m33,
-        )
-    }
-}
-
-impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
-    type AngVector = Vector3<SimdReal>;
-    type AngMatrix = Matrix3<SimdReal>;
-
-    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 = <SimdReal>::zero();
-        let is_zero = determinant.simd_eq(zero);
-        let inv_det = (<SimdReal>::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<SimdReal>) -> Vector3<SimdReal> {
-        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<SimdReal> {
         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<N: SimdRealCopy>(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 {

src_testbed/box2d_backend.rs

@@ -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<f32>) -> b2::Vec2 {
-    b2::Vec2 { x: v.x, y: v.y }
-}
-
-fn b2_vec_to_na_vec(v: b2::Vec2) -> Vector2<f32> {
-    Vector2::new(v.x, v.y)
-}
-
-fn b2_transform_to_na_isometry(v: b2::Transform) -> Isometry2<f32> {
-    Isometry2::new(b2_vec_to_na_vec(v.pos), v.rot.angle())
-}
-
-pub struct Box2dWorld {
-    world: b2::World<NoUserData>,
-    rapier2box2d: HashMap<RigidBodyHandle, b2::BodyHandle>,
-}
-
-impl Box2dWorld {
-    #[profiling::function]
-    pub fn from_rapier(
-        gravity: Vector2<f32>,
-        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::<RigidBody<f32>>() {
-            //                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<NoUserData>) {
-        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()),
-                    );
-                }
-            }
-        }
-    }
-}

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")]

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<PxArticulationLink, PxRigidDynamic> 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()
 }

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<f32>,
-}
-
-#[cfg(feature = "dim3")]
-#[derive(Serialize, Deserialize, PartialEq, Debug, Default, Clone)]
-pub struct SerializableCameraState {
-    pub distance: f32,
-    pub position: na::Point3<f32>,
-    pub center: na::Point3<f32>,
-}

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<PhysicsSnapshot>,
@@ -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<Box2dWorld>,
     #[cfg(feature = "dim3")]
     physx: Option<PhysxWorld>,
 }
@@ -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

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,
+    PHYSX_BACKEND_PATCH_FRICTION, PHYSX_BACKEND_TWO_FRICTION_DIR, RunMode, TestbedActionFlags,
-    TestbedActionFlags, TestbedState, TestbedStateFlags,
+    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(
-            ui.add(Slider::new(&mut num_iterations, 1..=40).text("pos. iters."));
+                Slider::new(&mut integration_parameters.num_solver_iterations, 0..=10)
-            integration_parameters.num_solver_iterations =
+                    .text("pos. iters."),
-                NonZeroUsize::new(num_iterations).unwrap();
+            );
         } 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.
+            // Friction model.
-            let mut num_iterations = integration_parameters.num_solver_iterations.get();
+            #[cfg(feature = "dim3")]
-            ui.add(Slider::new(&mut num_iterations, 1..=40).text("num solver iters."));
+            egui::ComboBox::from_label("friction-model")
-            integration_parameters.num_solver_iterations =
+                .width(150.0)
-                NonZeroUsize::new(num_iterations).unwrap();
+                .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,
-        "<fixme>", // bf.len() as f32 / 1000.0,
+        bf.len() as f32 / 1000.0,
-        "<fixme>", // format!("{:?}", hash_bf).split_at(10).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,