feat: add warmstarting to contact constraints resolution

benchmarks2d/vertical_stacks2.rs

@@ -11,14 +11,14 @@ pub fn init_world(testbed: &mut Testbed) {
     let multibody_joints = MultibodyJointSet::new();
 
 
-    let num = 40;
+    let rad = 0.5; // 50.0 / 2.0; // 0.5;
     let rad = 50.0 / 2.0; // 0.5;
 
     /*
      * Ground
      */
     let ground_size = num as f32 * rad * 10.0;
-    let ground_thickness = 1.0;
+    let ground_thickness = 25.0;
 
     let rigid_body = RigidBodyBuilder::fixed();
     let ground_handle = bodies.insert(rigid_body);
@@ -30,7 +30,7 @@ pub fn init_world(testbed: &mut Testbed) {
      */
 
     let shiftx_centerx = [
-        (rad * 2.0, -(num as f32) * rad * 2.0 * 1.5),
+        (rad * 2.0 + 0.0002, -(num as f32) * rad * 2.0 * 1.5),
          (rad * 2.0 + rad, num as f32 * rad * 2.0 * 1.5),
     ];
 

examples2d/all_examples2.rs

@@ -17,6 +17,7 @@ mod damping2;
 mod debug_box_ball2;
 mod debug_compression2;
 mod debug_total_overlap2;
+mod debug_vertical_column2;
 mod drum2;
 mod heightfield2;
 mod joint_motor_position2;
@@ -28,6 +29,17 @@ mod polyline2;
 mod pyramid2;
 mod restitution2;
 mod rope_joints2;
+mod s2d_arch;
+mod s2d_ball_and_chain;
+mod s2d_bridge;
+mod s2d_card_house;
+mod s2d_confined;
+mod s2d_far_pyramid;
+mod s2d_high_mass_ratio_1;
+mod s2d_high_mass_ratio_2;
+mod s2d_high_mass_ratio_3;
+mod s2d_joint_grid;
+mod s2d_pyramid;
 mod sensor2;
 mod trimesh2;
 
@@ -86,6 +98,21 @@ pub fn main() {
         ("(Debug) box ball", debug_box_ball2::init_world),
         ("(Debug) compression", debug_compression2::init_world),
         ("(Debug) total overlap", debug_total_overlap2::init_world),
+        (
+            "(Debug) vertical column",
+            debug_vertical_column2::init_world,
+        ),
+        ("(s2d) high mass ratio 1", s2d_high_mass_ratio_1::init_world),
+        ("(s2d) high mass ratio 2", s2d_high_mass_ratio_2::init_world),
+        ("(s2d) high mass ratio 3", s2d_high_mass_ratio_3::init_world),
+        ("(s2d) confined", s2d_confined::init_world),
+        ("(s2d) pyramid", s2d_pyramid::init_world),
+        ("(s2d) card house", s2d_card_house::init_world),
+        ("(s2d) arch", s2d_arch::init_world),
+        ("(s2d) bridge", s2d_bridge::init_world),
+        ("(s2d) ball and chain", s2d_ball_and_chain::init_world),
+        ("(s2d) joint grid", s2d_joint_grid::init_world),
+        ("(s2d) far pyramid", s2d_far_pyramid::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.

examples2d/debug_vertical_column2.rs

@@ -0,0 +1,47 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let num = 80;
+    let rad = 0.5;
+
+    /*
+     * Ground
+     */
+    let ground_size = 1.0;
+    let ground_thickness = 1.0;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).friction(0.3);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+
+    for i in 0..num {
+        let y = i as f32 * rad * 2.0 + ground_thickness + rad;
+
+        // Build the rigid body.
+        let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, y]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad).friction(0.3);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    // testbed.harness_mut().physics.gravity.y = -981.0;
+    testbed.look_at(point![0.0, 2.5], 5.0);
+}

examples2d/s2d_arch.rs

@@ -0,0 +1,107 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let mut ps1 = [
+        Point::new(16.0, 0.0),
+        Point::new(14.93803712795643, 5.133601056842984),
+        Point::new(13.79871746027416, 10.24928069555078),
+        Point::new(12.56252963284711, 15.34107019122473),
+        Point::new(11.20040987372525, 20.39856541571217),
+        Point::new(9.66521217819836, 25.40369899225096),
+        Point::new(7.87179930638133, 30.3179337000085),
+        Point::new(5.635199558196225, 35.03820717801641),
+        Point::new(2.405937953536585, 39.09554102558315),
+    ];
+
+    let mut ps2 = [
+        Point::new(24.0, 0.0),
+        Point::new(22.33619528222415, 6.02299846205841),
+        Point::new(20.54936888969905, 12.00964361211476),
+        Point::new(18.60854610798073, 17.9470321677465),
+        Point::new(16.46769273811807, 23.81367936585418),
+        Point::new(14.05325025774858, 29.57079353071012),
+        Point::new(11.23551045834022, 35.13775818285372),
+        Point::new(7.752568160730571, 40.30450679009583),
+        Point::new(3.016931552701656, 44.28891593799322),
+    ];
+
+    let scale = 0.25;
+    let friction = 0.6;
+
+    for i in 0..9 {
+        ps1[i] *= scale;
+        ps2[i] *= scale;
+    }
+
+    /*
+     * Ground
+     */
+    let collider = ColliderBuilder::segment(point![-100.0, 0.0], point![100.0, 0.0]).friction(0.6);
+    colliders.insert(collider);
+
+    /*
+     * Create the arch
+     */
+    for i in 0..8 {
+        let ps = [ps1[i], ps2[i], ps2[i + 1], ps1[i + 1]];
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::convex_hull(&ps)
+            .unwrap()
+            .friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    for i in 0..8 {
+        let ps = [
+            point![-ps2[i].x, ps2[i].y],
+            point![-ps1[i].x, ps1[i].y],
+            point![-ps1[i + 1].x, ps1[i + 1].y],
+            point![-ps2[i + 1].x, ps2[i + 1].y],
+        ];
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::convex_hull(&ps)
+            .unwrap()
+            .friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    {
+        let ps = [
+            ps1[8],
+            ps2[8],
+            point![-ps1[8].x, ps1[8].y],
+            point![-ps2[8].x, ps2[8].y],
+        ];
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::convex_hull(&ps)
+            .unwrap()
+            .friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    for i in 0..4 {
+        let rigid_body =
+            RigidBodyBuilder::dynamic().translation(vector![0.0, 0.5 + ps2[8].y + 1.0 * i as f32]);
+        let ground_handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(2.0, 0.5).friction(friction);
+        colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_ball_and_chain.rs

@@ -0,0 +1,74 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground = bodies.insert(RigidBodyBuilder::fixed());
+
+    /*
+     * Create the bridge.
+     */
+    let count = 40;
+    let hx = 0.5;
+    let density = 20.0;
+    let friction = 0.6;
+    let capsule = ColliderBuilder::capsule_x(hx, 0.125)
+        .friction(friction)
+        .density(20.0);
+
+    let mut prev = ground;
+    for i in 0..count {
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .linear_damping(0.1)
+            .angular_damping(0.1)
+            .translation(vector![(1.0 + 2.0 * i as f32) * hx, count as f32 * hx]);
+        let handle = bodies.insert(rigid_body);
+        colliders.insert_with_parent(capsule.clone(), handle, &mut bodies);
+
+        let pivot = point![(2.0 * i as f32) * hx, count as f32 * hx];
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+            .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
+            .contacts_enabled(false);
+        impulse_joints.insert(prev, handle, joint, true);
+        prev = handle;
+    }
+
+    let radius = 8.0;
+    let rigid_body = RigidBodyBuilder::dynamic()
+        .linear_damping(0.1)
+        .angular_damping(0.1)
+        .translation(vector![
+            (1.0 + 2.0 * count as f32) * hx + radius - hx,
+            count as f32 * hx
+        ]);
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::ball(radius)
+        .friction(friction)
+        .density(20.0);
+    colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    let pivot = point![(2.0 * count as f32) * hx, count as f32 * hx];
+    let joint = RevoluteJointBuilder::new()
+        .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+        .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
+        .contacts_enabled(false);
+    impulse_joints.insert(prev, handle, joint, true);
+    prev = handle;
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_bridge.rs

@@ -0,0 +1,56 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground = bodies.insert(RigidBodyBuilder::fixed());
+
+    /*
+     * Create the bridge.
+     */
+    let density = 20.0;
+    let x_base = -80.0;
+    let count = 160;
+    let mut prev = ground;
+
+    for i in 0..count {
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .linear_damping(0.1)
+            .angular_damping(0.1)
+            .translation(vector![x_base + 0.5 + 1.0 * i as f32, 20.0]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(0.5, 0.125).density(20.0);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+
+        let pivot = point![x_base + 1.0 * i as f32, 20.0];
+        let joint = RevoluteJointBuilder::new()
+            .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+            .local_anchor2(bodies[handle].position().inverse_transform_point(&pivot))
+            .contacts_enabled(false);
+        impulse_joints.insert(prev, handle, joint, true);
+        prev = handle;
+    }
+
+    let pivot = point![x_base + 1.0 * count as f32, 20.0];
+    let joint = RevoluteJointBuilder::new()
+        .local_anchor1(bodies[prev].position().inverse_transform_point(&pivot))
+        .local_anchor2(bodies[ground].position().inverse_transform_point(&pivot))
+        .contacts_enabled(false);
+    impulse_joints.insert(prev, ground, joint, true);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_card_house.rs

@@ -0,0 +1,79 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let extent = 1.0;
+    let friction = 0.7;
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let scale = 10.0;
+    let card_height = 0.2 * scale;
+    let card_thickness = 0.001 * scale;
+    let angle0 = 25.0 * std::f32::consts::PI / 180.0;
+    let angle1 = -25.0 * std::f32::consts::PI / 180.0;
+    let angle2 = 0.5 * std::f32::consts::PI;
+
+    let card_box = ColliderBuilder::cuboid(card_thickness, card_height).friction(friction);
+
+    let mut nb = 5;
+    let mut z0 = 0.0;
+    let mut y = card_height - 0.02 * scale;
+
+    while nb != 0 {
+        let mut z = z0;
+
+        for i in 0..nb {
+            if i != nb - 1 {
+                let rigid_body = RigidBodyBuilder::dynamic()
+                    .translation(vector![z + 0.25 * scale, y + card_height - 0.015 * scale])
+                    .rotation(angle2);
+                let ground_handle = bodies.insert(rigid_body);
+                colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
+            }
+
+            let rigid_body = RigidBodyBuilder::dynamic()
+                .translation(vector![z, y])
+                .rotation(angle1);
+            let ground_handle = bodies.insert(rigid_body);
+            colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
+
+            z += 0.175 * scale;
+
+            let rigid_body = RigidBodyBuilder::dynamic()
+                .translation(vector![z, y])
+                .rotation(angle0);
+            let ground_handle = bodies.insert(rigid_body);
+            colliders.insert_with_parent(card_box.clone(), ground_handle, &mut bodies);
+
+            z += 0.175 * scale;
+        }
+
+        y += card_height * 2.0 - 0.03 * scale;
+        z0 += 0.175 * scale;
+        nb -= 1;
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_confined.rs

@@ -0,0 +1,65 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let radius = 0.5;
+    let grid_count = 25;
+    let friction = 0.6;
+    let max_count = grid_count * grid_count;
+
+    /*
+     * Ground
+     */
+    let collider =
+        ColliderBuilder::capsule(point![-10.5, 0.0], point![10.5, 0.0], radius).friction(friction);
+    colliders.insert(collider);
+    let collider = ColliderBuilder::capsule(point![-10.5, 0.0], point![-10.5, 20.5], radius)
+        .friction(friction);
+    colliders.insert(collider);
+    let collider =
+        ColliderBuilder::capsule(point![10.5, 0.0], point![10.5, 20.5], radius).friction(friction);
+    colliders.insert(collider);
+    let collider = ColliderBuilder::capsule(point![-10.5, 20.5], point![10.5, 20.5], radius)
+        .friction(friction);
+    colliders.insert(collider);
+
+    /*
+     * Create the spheres
+     */
+    let mut row = 0;
+    let mut count = 0;
+    let mut column = 0;
+
+    while count < max_count {
+        row = 0;
+        for i in 0..grid_count {
+            let x = -8.75 + column as f32 * 18.0 / (grid_count as f32);
+            let y = 1.5 + row as f32 * 18.0 / (grid_count as f32);
+            let body = RigidBodyBuilder::dynamic()
+                .translation(vector![x, y])
+                .gravity_scale(0.0);
+            let body_handle = bodies.insert(body);
+            let ball = ColliderBuilder::ball(radius).friction(friction);
+            colliders.insert_with_parent(ball, body_handle, &mut bodies);
+
+            count += 1;
+            row += 1;
+        }
+
+        column += 1;
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_far_pyramid.rs

@@ -0,0 +1,51 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let origin = vector![100_000.0, -80_000.0];
+    let extent = 1.0;
+    let friction = 0.6;
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0] + origin);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let base_count = 10;
+
+    let h = 0.5;
+    let shift = 1.25 * h;
+
+    for i in 0..base_count {
+        let y = (2.0 * i as f32 + 1.0) * shift + 0.5;
+
+        for j in i..base_count {
+            let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift
+                - h * base_count as f32;
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y] + origin);
+            let ground_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(h, h).friction(friction);
+            colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5] + origin, 20.0);
+}

examples2d/s2d_high_mass_ratio_1.rs

@@ -0,0 +1,66 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let extent = 1.0;
+    let friction = 0.5;
+
+    /*
+     * Ground
+     */
+    let ground_width = 66.0 * extent;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::segment(
+        point![-0.5 * 2.0 * ground_width, 0.0],
+        point![0.5 * 2.0 * ground_width, 0.0],
+    )
+    .friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+
+    for j in 0..3 {
+        let mut count = 10;
+        let offset = -20.0 * extent + 2.0 * (count as f32 + 1.0) * extent * j as f32;
+        let mut y = extent;
+
+        while count > 0 {
+            for i in 0..count {
+                let coeff = i as f32 - 0.5 * count as f32;
+                let yy = if count == 1 { y + 2.0 } else { y };
+                let position = vector![2.0 * coeff * extent + offset, yy];
+                let rigid_body = RigidBodyBuilder::dynamic().translation(position);
+                let parent = bodies.insert(rigid_body);
+
+                let collider = ColliderBuilder::cuboid(extent, extent)
+                    .density(if count == 1 {
+                        (j as f32 + 1.0) * 100.0
+                    } else {
+                        1.0
+                    })
+                    .friction(friction);
+                colliders.insert_with_parent(collider, parent, &mut bodies);
+            }
+
+            count -= 1;
+            y += 2.0 * extent;
+        }
+    }
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_high_mass_ratio_2.rs

@@ -0,0 +1,53 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let extent = 1.0;
+    let friction = 0.6;
+
+    /*
+     * Ground
+     */
+    let ground_width = 66.0 * extent;
+
+    let rigid_body = RigidBodyBuilder::fixed();
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::segment(
+        point![-0.5 * 2.0 * ground_width, 0.0],
+        point![0.5 * 2.0 * ground_width, 0.0],
+    )
+    .friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_high_mass_ratio_3.rs

@@ -0,0 +1,49 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let extent = 1.0;
+    let friction = 0.6;
+
+    /*
+     * Ground
+     */
+    let ground_width = 66.0 * extent;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(40.0, 2.0).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![-9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![9.0 * extent, 0.5 * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.5 * extent, 0.5 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, (10.0 + 16.0) * extent]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(10.0 * extent, 10.0 * extent).friction(friction);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_joint_grid.rs

@@ -0,0 +1,68 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground = bodies.insert(RigidBodyBuilder::fixed());
+
+    /*
+     * Create the bridge.
+     */
+    let rad = 0.4;
+    let numi = 100;
+    let numk = 100;
+    let shift = 1.0;
+    let mut index = 0;
+    let mut handles = vec![RigidBodyHandle::invalid(); numi * numk];
+
+    for k in 0..numk {
+        for i in 0..numi {
+            let body_type = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 {
+                RigidBodyType::Fixed
+            } else {
+                RigidBodyType::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(body_type)
+                .translation(vector![k as f32 * shift, -(i as f32) * shift]);
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad);
+            colliders.insert_with_parent(collider, handle, &mut bodies);
+
+            if i > 0 {
+                let joint = RevoluteJointBuilder::new()
+                    .local_anchor1(point![0.0, -0.5 * shift])
+                    .local_anchor2(point![0.0, 0.5 * shift])
+                    .contacts_enabled(false);
+                impulse_joints.insert(handles[index - 1], handle, joint, true);
+            }
+
+            if k > 0 {
+                let joint = RevoluteJointBuilder::new()
+                    .local_anchor1(point![0.5 * shift, 0.0])
+                    .local_anchor2(point![-0.5 * shift, 0.0])
+                    .contacts_enabled(false);
+                impulse_joints.insert(handles[index - numi], handle, joint, true);
+            }
+
+            handles[index] = handle;
+            index += 1;
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

examples2d/s2d_pyramid.rs

@@ -0,0 +1,49 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+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();
+
+    let extent = 1.0;
+
+    /*
+     * Ground
+     */
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -1.0]);
+    let ground_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(100.0, 1.0).friction(0.6);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let base_count = 100;
+
+    let h = 0.5;
+    let shift = 1.0 * h;
+
+    for i in 0..base_count {
+        let y = (2.0 * i as f32 + 1.0) * shift;
+
+        for j in i..base_count {
+            let x = (i as f32 + 1.0) * shift + 2.0 * (j as f32 - i as f32) * shift
+                - h * base_count as f32;
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
+            let ground_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(h, h).friction(0.6);
+            colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}

src/dynamics/integration_parameters.rs

@@ -4,7 +4,6 @@ use std::num::NonZeroUsize;
 // TODO: enabling the block solver in 3d introduces a lot of jitters in
 //       the 3D domino demo. So for now we dont enable it in 3D.
 pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2");
-pub(crate) static DISABLE_FRICTION_LIMIT_REAPPLY: bool = false;
 
 /// Parameters for a time-step of the physics engine.
 #[derive(Copy, Clone, Debug)]
@@ -26,12 +25,12 @@ pub struct IntegrationParameters {
 
     /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
     /// will be compensated for during the velocity solve.
-    /// (default `0.8`).
+    /// (default `0.1`).
     pub erp: Real,
     /// 0-1: the damping ratio used by the springs for Baumgarte constraints stabilization.
     /// Lower values make the constraints more compliant (more "springy", allowing more visible penetrations
     /// before stabilization).
-    /// (default `0.25`).
+    /// (default `20.0`).
     pub damping_ratio: Real,
 
     /// 0-1: multiplier for how much of the joint violation
@@ -40,14 +39,21 @@ pub struct IntegrationParameters {
     pub joint_erp: Real,
 
     /// The fraction of critical damping applied to the joint for constraints regularization.
-    /// (default `0.25`).
+    /// (default `1.0`).
     pub joint_damping_ratio: Real,
 
-    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
+    /// The coefficient in `[0, 1]` applied to warmstart impulses, i.e., impulses that are used as the
+    /// initial solution (instead of 0) at the next simulation step.
+    ///
+    /// This should generally be set to 1. Can be set to 0 if using a large [`Self::erp`] value.
+    /// (default `1.0`).
+    pub warmstart_coefficient: Real,
+
+    /// Amount of penetration the engine won’t attempt to correct (default: `0.001m`).
     pub allowed_linear_error: Real,
     /// Maximum amount of penetration the solver will attempt to resolve in one timestep.
     pub max_penetration_correction: Real,
-    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
+    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`).
     pub prediction_distance: Real,
     /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
     pub num_solver_iterations: NonZeroUsize,
@@ -55,8 +61,8 @@ pub struct IntegrationParameters {
     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 maximum number of stabilization iterations run at each solver iterations (default: `10`).
+    /// The number of stabilization iterations run at each solver iterations (default: `2`).
-    pub max_internal_stabilization_iterations: usize,
+    pub num_internal_stabilization_iterations: usize,
     /// Minimum number of dynamic bodies in each active island (default: `128`).
     pub min_island_size: usize,
     /// Maximum number of substeps performed by the  solver (default: `1`).
@@ -64,51 +70,6 @@ pub struct IntegrationParameters {
 }
 
 impl IntegrationParameters {
-    /// Configures the integration parameters to match the old PGS solver
-    /// from Rapier version <= 0.17.
-    ///
-    /// This solver was slightly faster than the new one but resulted
-    /// in less stable joints and worse convergence rates.
-    ///
-    /// This should only be used for comparison purpose or if you are
-    /// experiencing problems with the new solver.
-    ///
-    /// NOTE: this does not affect any [`RigidBody::additional_solver_iterations`] that will
-    ///       still create solver iterations based on the new "small-steps" PGS solver.
-    /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
-    ///       [`Self::joint_damping_ratio`] to their former default values.
-    pub fn switch_to_standard_pgs_solver(&mut self) {
-        self.num_internal_pgs_iterations *= self.num_solver_iterations.get();
-        self.num_solver_iterations = NonZeroUsize::new(1).unwrap();
-        self.erp = 0.8;
-        self.damping_ratio = 0.25;
-        self.joint_erp = 1.0;
-        self.joint_damping_ratio = 1.0;
-    }
-
-    /// Configures the integration parameters to match the new "small-steps" PGS solver
-    /// from Rapier version >= 0.18.
-    ///
-    /// The "small-steps" PGS solver is the default one given by [`Self::default()`] so
-    /// calling this function is generally not needed unless
-    /// [`Self::switch_to_standard_pgs_solver()`] was called.
-    ///
-    /// This solver results in more stable joints and significantly better convergence
-    /// rates but is slightly slower in its default settings.
-    ///
-    /// NOTE: this resets [`Self::erp`], [`Self::damping_ratio`], [`Self::joint_erp`],
-    ///       [`Self::joint_damping_ratio`] to their default values.
-    pub fn switch_to_small_steps_pgs_solver(&mut self) {
-        self.num_solver_iterations = NonZeroUsize::new(self.num_internal_pgs_iterations).unwrap();
-        self.num_internal_pgs_iterations = 1;
-
-        let default = Self::default();
-        self.erp = default.erp;
-        self.damping_ratio = default.damping_ratio;
-        self.joint_erp = default.joint_erp;
-        self.joint_damping_ratio = default.joint_damping_ratio;
-    }
-
     /// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
     ///
     /// This is zero if `self.dt` is zero.
@@ -161,7 +122,7 @@ impl IntegrationParameters {
         // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
         //     / (dt * inv_erp_minus_one);
         // let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
-        // NOTE: This simplies to cfm = cfm_coefff / projected_mass:
+        // NOTE: This simplifies to cfm = cfm_coeff / projected_mass:
         let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
             / ((1.0 + inv_erp_minus_one) * 4.0 * self.damping_ratio * self.damping_ratio);
 
@@ -180,13 +141,14 @@ impl IntegrationParameters {
         // new_impulse = cfm_factor * (old_impulse - m * delta_vel)
         //
         // The value returned by this function is this cfm_factor that can be used directly
-        // in the constraints solver.
+        // in the constraint solver.
         1.0 / (1.0 + cfm_coeff)
     }
 
     /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization
     pub fn joint_cfm_coeff(&self) -> Real {
         // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
+        // The logic is similar to `Self::cfm_factor`.
         let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0;
         inv_erp_minus_one * inv_erp_minus_one
             / ((1.0 + inv_erp_minus_one)
@@ -194,23 +156,23 @@ impl IntegrationParameters {
                 * self.joint_damping_ratio
                 * self.joint_damping_ratio)
     }
-}
 
-impl Default for IntegrationParameters {
+    /// Initialize the simulation paramaters with settings matching the TGS-soft solver
-    fn default() -> Self {
+    /// with warmstarting.
+    ///
+    /// 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,
-            erp: 0.8,
+            erp: 0.1,
-            damping_ratio: 1.0,
+            damping_ratio: 20.0,
             joint_erp: 1.0,
             joint_damping_ratio: 1.0,
-            allowed_linear_error: 0.001,
+            warmstart_coefficient: 1.0,
-            max_penetration_correction: Real::MAX,
-            prediction_distance: 0.002,
             num_internal_pgs_iterations: 1,
-            max_internal_stabilization_iterations: 10,
+            num_internal_stabilization_iterations: 2,
-            num_additional_friction_iterations: 4,
+            num_additional_friction_iterations: 0,
             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
@@ -218,7 +180,45 @@ impl Default for IntegrationParameters {
             // However we don't want it to be too small and end up with
             // tons of islands, reducing SIMD parallelism opportunities.
             min_island_size: 128,
+            allowed_linear_error: 0.001,
+            max_penetration_correction: Real::MAX,
+            prediction_distance: 0.002,
             max_ccd_substeps: 1,
         }
     }
+
+    /// Initialize the simulation paramaters 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 {
+            erp: 0.8,
+            damping_ratio: 1.0,
+            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 {
+            erp: 0.8,
+            damping_ratio: 0.25,
+            warmstart_coefficient: 0.0,
+            num_additional_friction_iterations: 4,
+            num_solver_iterations: NonZeroUsize::new(1).unwrap(),
+            ..Self::tgs_soft()
+        }
+    }
+}
+
+impl Default for IntegrationParameters {
+    fn default() -> Self {
+        Self::tgs_soft()
+    }
 }

src/dynamics/solver/contact_constraint/contact_constraints_set.rs

@@ -442,6 +442,17 @@ impl ContactConstraintsSet {
         assert_eq!(curr_start, total_num_constraints);
     }
 
+    pub fn warmstart(
+        &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.warmstart(jac, solver_vels, generic_solver_vels);
+        }
+    }
+
     pub fn solve_restitution(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],

src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs

@@ -259,6 +259,24 @@ impl GenericOneBodyConstraint {
         }
     }
 
+    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,
+        );
+    }
+
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,

src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs

@@ -7,6 +7,40 @@ use na::DVector;
 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,
@@ -71,6 +105,22 @@ impl OneBodyConstraintTangentPart<Real> {
 }
 
 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,
@@ -99,6 +149,42 @@ impl OneBodyConstraintNormalPart<Real> {
 }
 
 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,

src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs

@@ -202,7 +202,7 @@ impl GenericTwoBodyConstraintBuilder {
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
                         impulse_accumulator: na::zero(),
-                        impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
                         r,
                         r_mat_elts: [0.0; 2],
                     };
@@ -210,7 +210,8 @@ impl GenericTwoBodyConstraintBuilder {
 
                 // Tangent parts.
                 {
-                    constraint.inner.elements[k].tangent_part.impulse = na::zero();
+                    constraint.inner.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let torque_dir1 = dp1.gcross(tangents1[j]);
@@ -374,6 +375,50 @@ impl GenericTwoBodyConstraint {
         }
     }
 
+    pub fn warmstart(
+        &mut self,
+        jacobians: &DVector<Real>,
+        solver_vels: &mut [SolverVel<Real>],
+        generic_solver_vels: &mut DVector<Real>,
+    ) {
+        let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel1)
+        };
+
+        let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
+            GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
+        } else {
+            GenericRhs::GenericId(self.inner.solver_vel2)
+        };
+
+        let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
+        TwoBodyConstraintElement::generic_warmstart_group(
+            elements,
+            jacobians,
+            &self.inner.dir1,
+            #[cfg(feature = "dim3")]
+            &self.inner.tangent1,
+            &self.inner.im1,
+            &self.inner.im2,
+            self.ndofs1,
+            self.ndofs2,
+            self.j_id,
+            &mut solver_vel1,
+            &mut solver_vel2,
+            generic_solver_vels,
+        );
+
+        if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
+            solver_vels[self.inner.solver_vel1] = solver_vel1;
+        }
+
+        if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
+            solver_vels[self.inner.solver_vel2] = solver_vel2;
+        }
+    }
+
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,

src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs

@@ -88,6 +88,95 @@ impl GenericRhs {
 }
 
 impl TwoBodyConstraintTangentPart<Real> {
+    #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        tangents1: [&Vector<Real>; DIM - 1],
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        ndofs1: usize,
+        ndofs2: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+        #[cfg(feature = "dim3")]
+        let j_step = j_step(ndofs1, ndofs2);
+
+        #[cfg(feature = "dim2")]
+        {
+            solver_vel1.apply_impulse(
+                j_id1,
+                ndofs1,
+                self.impulse[0],
+                jacobians,
+                tangents1[0],
+                &self.gcross1[0],
+                solver_vels,
+                im1,
+            );
+            solver_vel2.apply_impulse(
+                j_id2,
+                ndofs2,
+                self.impulse[0],
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                solver_vels,
+                im2,
+            );
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            solver_vel1.apply_impulse(
+                j_id1,
+                ndofs1,
+                self.impulse[0],
+                jacobians,
+                tangents1[0],
+                &self.gcross1[0],
+                solver_vels,
+                im1,
+            );
+            solver_vel1.apply_impulse(
+                j_id1 + j_step,
+                ndofs1,
+                self.impulse[1],
+                jacobians,
+                tangents1[1],
+                &self.gcross1[1],
+                solver_vels,
+                im1,
+            );
+
+            solver_vel2.apply_impulse(
+                j_id2,
+                ndofs2,
+                self.impulse[0],
+                jacobians,
+                &-tangents1[0],
+                &self.gcross2[0],
+                solver_vels,
+                im2,
+            );
+            solver_vel2.apply_impulse(
+                j_id2 + j_step,
+                ndofs2,
+                self.impulse[1],
+                jacobians,
+                &-tangents1[1],
+                &self.gcross2[1],
+                solver_vels,
+                im2,
+            );
+        }
+    }
+
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -240,6 +329,45 @@ impl TwoBodyConstraintTangentPart<Real> {
 }
 
 impl TwoBodyConstraintNormalPart<Real> {
+    #[inline]
+    pub fn generic_warmstart(
+        &mut self,
+        j_id: usize,
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        ndofs1: usize,
+        ndofs2: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_id1 = j_id1(j_id, ndofs1, ndofs2);
+        let j_id2 = j_id2(j_id, ndofs1, ndofs2);
+
+        solver_vel1.apply_impulse(
+            j_id1,
+            ndofs1,
+            self.impulse,
+            jacobians,
+            dir1,
+            &self.gcross1,
+            solver_vels,
+            im1,
+        );
+        solver_vel2.apply_impulse(
+            j_id2,
+            ndofs2,
+            self.impulse,
+            jacobians,
+            &-dir1,
+            &self.gcross2,
+            solver_vels,
+            im2,
+        );
+    }
+
     #[inline]
     pub fn generic_solve(
         &mut self,
@@ -290,6 +418,74 @@ impl TwoBodyConstraintNormalPart<Real> {
 }
 
 impl TwoBodyConstraintElement<Real> {
+    #[inline]
+    pub fn generic_warmstart_group(
+        elements: &mut [Self],
+        jacobians: &DVector<Real>,
+        dir1: &Vector<Real>,
+        #[cfg(feature = "dim3")] tangent1: &Vector<Real>,
+        im1: &Vector<Real>,
+        im2: &Vector<Real>,
+        // ndofs is 0 for a non-multibody body, or a multibody with zero
+        // degrees of freedom.
+        ndofs1: usize,
+        ndofs2: usize,
+        // Jacobian index of the first constraint.
+        j_id: usize,
+        solver_vel1: &mut GenericRhs,
+        solver_vel2: &mut GenericRhs,
+        solver_vels: &mut DVector<Real>,
+    ) {
+        let j_step = j_step(ndofs1, ndofs2) * DIM;
+
+        // Solve penetration.
+        {
+            let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
+
+            for element in elements.iter_mut() {
+                element.normal_part.generic_warmstart(
+                    nrm_j_id,
+                    jacobians,
+                    dir1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
+                );
+                nrm_j_id += j_step;
+            }
+        }
+
+        // Solve friction.
+        {
+            #[cfg(feature = "dim3")]
+            let tangents1 = [tangent1, &dir1.cross(tangent1)];
+            #[cfg(feature = "dim2")]
+            let tangents1 = [&dir1.orthonormal_vector()];
+            let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
+
+            for element in elements.iter_mut() {
+                let part = &mut element.tangent_part;
+                part.generic_warmstart(
+                    tng_j_id,
+                    jacobians,
+                    tangents1,
+                    im1,
+                    im2,
+                    ndofs1,
+                    ndofs2,
+                    solver_vel1,
+                    solver_vel2,
+                    solver_vels,
+                );
+                tng_j_id += j_step;
+            }
+        }
+    }
+
     #[inline]
     pub fn generic_solve_group(
         cfm_factor: Real,

src/dynamics/solver/contact_constraint/one_body_constraint.rs

@@ -151,7 +151,7 @@ impl OneBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
                         impulse_accumulator: na::zero(),
                         r: projected_mass,
                         r_mat_elts: [0.0; 2],
@@ -160,7 +160,8 @@ impl OneBodyConstraintBuilder {
 
                 // Tangent parts.
                 {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
+                    constraint.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let gcross2 = mprops2
@@ -317,13 +318,13 @@ impl OneBodyConstraintBuilder {
                 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 = na::zero();
+                element.normal_part.impulse *= params.warmstart_coefficient;
             }
 
             // Tangent part.
             {
                 element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse *= params.warmstart_coefficient;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -369,6 +370,21 @@ impl OneBodyConstraint {
         }
     }
 
+    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>],
@@ -400,17 +416,11 @@ impl OneBodyConstraint {
         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.impulse = self.elements[k].normal_part.total_impulse();
 
-            #[cfg(feature = "dim2")]
+            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.tangent_impulse =
+            active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
-                    self.elements[k].tangent_part.total_impulse()[0];
+            active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
-            }
-            #[cfg(feature = "dim3")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
-            }
         }
     }
 

src/dynamics/solver/contact_constraint/one_body_constraint_element.rs

@@ -1,9 +1,7 @@
-use crate::dynamics::integration_parameters::{
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
-    BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY,
-};
 use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
 use crate::dynamics::solver::SolverVel;
-use crate::math::{AngVector, Vector, DIM};
+use crate::math::{AngVector, TangentImpulse, Vector, DIM};
 use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
 use na::Vector2;
 
@@ -12,14 +10,8 @@ pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
     pub gcross2: [AngVector<N>; DIM - 1],
     pub rhs: [N; DIM - 1],
     pub rhs_wo_bias: [N; DIM - 1],
-    #[cfg(feature = "dim2")]
+    pub impulse: TangentImpulse<N>,
-    pub impulse: na::Vector1<N>,
+    pub impulse_accumulator: TangentImpulse<N>,
-    #[cfg(feature = "dim3")]
-    pub impulse: na::Vector2<N>,
-    #[cfg(feature = "dim2")]
-    pub impulse_accumulator: na::Vector1<N>,
-    #[cfg(feature = "dim3")]
-    pub impulse_accumulator: na::Vector2<N>,
     #[cfg(feature = "dim2")]
     pub r: [N; 1],
     #[cfg(feature = "dim3")]
@@ -43,57 +35,29 @@ impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
 
     /// Total impulse applied across all the solver substeps.
     #[inline]
-    #[cfg(feature = "dim2")]
+    pub fn total_impulse(&self) -> TangentImpulse<N> {
-    pub fn total_impulse(&self) -> na::Vector1<N> {
-        self.impulse_accumulator + self.impulse
-    }
-
-    /// Total impulse applied across all the solver substeps.
-    #[inline]
-    #[cfg(feature = "dim3")]
-    pub fn total_impulse(&self) -> na::Vector2<N> {
         self.impulse_accumulator + self.impulse
     }
 
     #[inline]
-    pub fn apply_limit(
+    pub fn warmstart(
         &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>,
-    {
-        if DISABLE_FRICTION_LIMIT_REAPPLY {
-            return;
-        }
-
         #[cfg(feature = "dim2")]
         {
-            let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
-            let dlambda = new_impulse - self.impulse[0];
+            solver_vel2.angular += self.gcross2[0] * 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 new_impulse = self.impulse;
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
-            let new_impulse = {
+                + tangents1[1].component_mul(im2) * -self.impulse[1];
-                let _disable_fe_except =
+            solver_vel2.angular +=
-                    crate::utils::DisableFloatingPointExceptionsFlags::
+                self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
-                    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];
         }
     }
 
@@ -184,6 +148,12 @@ impl<N: SimdRealCopy> OneBodyConstraintNormalPart<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,
@@ -257,6 +227,25 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
         }
     }
 
+    #[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,
@@ -293,13 +282,6 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
                         im2,
                         solver_vel2,
                     );
-
-                    // There is one constraint left to solve if there isn’t an even number.
-                    for i in 0..2 {
-                        let limit = limit * elements[i].normal_part.impulse;
-                        let part = &mut elements[i].tangent_part;
-                        part.apply_limit(tangents1, im2, limit, solver_vel2);
-                    }
                 }
 
                 if elements.len() % 2 == 1 {
@@ -307,18 +289,12 @@ impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
                     element
                         .normal_part
                         .solve(cfm_factor, dir1, im2, solver_vel2);
-                    let limit = limit * element.normal_part.impulse;
-                    let part = &mut element.tangent_part;
-                    part.apply_limit(tangents1, im2, limit, solver_vel2);
                 }
             } else {
                 for element in elements.iter_mut() {
                     element
                         .normal_part
                         .solve(cfm_factor, dir1, im2, solver_vel2);
-                    let limit = limit * element.normal_part.impulse;
-                    let part = &mut element.tangent_part;
-                    part.apply_limit(tangents1, im2, limit, solver_vel2);
                 }
             }
         }

src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs

@@ -8,8 +8,8 @@ use crate::dynamics::{
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
+    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
-    SIMD_WIDTH,
+    MAX_MANIFOLD_POINTS, SIMD_WIDTH,
 };
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
@@ -120,6 +120,11 @@ impl SimdOneBodyConstraintBuilder {
                 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]);
@@ -155,7 +160,7 @@ impl SimdOneBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
+                        impulse: warmstart_impulse,
                         impulse_accumulator: na::zero(),
                         r: projected_mass,
                         r_mat_elts: [SimdReal::zero(); 2],
@@ -163,7 +168,7 @@ impl SimdOneBodyConstraintBuilder {
                 }
 
                 // tangent parts.
-                constraint.elements[k].tangent_part.impulse = na::zero();
+                constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
 
                 for j in 0..DIM - 1 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -263,6 +268,7 @@ impl SimdOneBodyConstraintBuilder {
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
         let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+        let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
 
         let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
         let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
@@ -309,13 +315,13 @@ impl SimdOneBodyConstraintBuilder {
                 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 = na::zero();
+                element.normal_part.impulse *= warmstart_coeff;
             }
 
             // tangent parts.
             {
                 element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse *= warmstart_coeff;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -344,6 +350,27 @@ pub(crate) struct OneBodyConstraintSimd {
 }
 
 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>],
@@ -377,27 +404,21 @@ impl OneBodyConstraintSimd {
     // 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();
-            #[cfg(feature = "dim2")]
-            let tangent_impulses: [_; SIMD_WIDTH] =
-                self.elements[k].tangent_part.total_impulse()[0].into();
-            #[cfg(feature = "dim3")]
             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.impulse = impulses[ii];
 
-                #[cfg(feature = "dim2")]
+                active_contact.data.warmstart_impulse = warmstart_impulses[ii];
-                {
+                active_contact.data.warmstart_tangent_impulse =
-                    active_contact.data.tangent_impulse = tangent_impulses[ii];
+                    warmstart_tangent_impulses.extract(ii);
-                }
+                active_contact.data.impulse = impulses[ii];
-                #[cfg(feature = "dim3")]
+                active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
-                {
-                    active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
-                }
             }
         }
     }

src/dynamics/solver/contact_constraint/two_body_constraint.rs

@@ -25,6 +25,25 @@ impl<'a> AnyConstraintMut<'a, ContactConstraintTypes> {
             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,
@@ -224,7 +243,7 @@ impl TwoBodyConstraintBuilder {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: na::zero(),
+                        impulse: manifold_point.warmstart_impulse,
                         impulse_accumulator: na::zero(),
                         r: projected_mass,
                         r_mat_elts: [0.0; 2],
@@ -233,7 +252,8 @@ impl TwoBodyConstraintBuilder {
 
                 // Tangent parts.
                 {
-                    constraint.elements[k].tangent_part.impulse = na::zero();
+                    constraint.elements[k].tangent_part.impulse =
+                        manifold_point.warmstart_tangent_impulse;
 
                     for j in 0..DIM - 1 {
                         let gcross1 = mprops1
@@ -398,13 +418,13 @@ impl TwoBodyConstraintBuilder {
                 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 = na::zero();
+                element.normal_part.impulse *= params.warmstart_coefficient;
             }
 
             // Tangent part.
             {
                 element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse *= params.warmstart_coefficient;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -418,6 +438,25 @@ impl TwoBodyConstraintBuilder {
 }
 
 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>],
@@ -452,17 +491,10 @@ impl TwoBodyConstraint {
         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();
-            #[cfg(feature = "dim2")]
-            {
-                active_contact.data.tangent_impulse =
-                    self.elements[k].tangent_part.total_impulse()[0];
-            }
-            #[cfg(feature = "dim3")]
-            {
-                active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
-            }
         }
     }
 

src/dynamics/solver/contact_constraint/two_body_constraint_element.rs

@@ -1,9 +1,7 @@
-use crate::dynamics::integration_parameters::{
+use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
-    BLOCK_SOLVER_ENABLED, DISABLE_FRICTION_LIMIT_REAPPLY,
-};
 use crate::dynamics::solver::contact_constraint::OneBodyConstraintNormalPart;
 use crate::dynamics::solver::SolverVel;
-use crate::math::{AngVector, Vector, DIM};
+use crate::math::{AngVector, TangentImpulse, Vector, DIM};
 use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
 use na::{Matrix2, Vector2};
 use num::Zero;
@@ -47,67 +45,41 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
 
     /// Total impulse applied across all the solver substeps.
     #[inline]
-    #[cfg(feature = "dim2")]
+    pub fn total_impulse(&self) -> TangentImpulse<N> {
-    pub fn total_impulse(&self) -> na::Vector1<N> {
-        self.impulse_accumulator + self.impulse
-    }
-
-    /// Total impulse applied across all the solver substeps.
-    #[inline]
-    #[cfg(feature = "dim3")]
-    pub fn total_impulse(&self) -> na::Vector2<N> {
         self.impulse_accumulator + self.impulse
     }
 
     #[inline]
-    pub fn apply_limit(
+    pub fn warmstart(
         &mut self,
         tangents1: [&Vector<N>; DIM - 1],
         im1: &Vector<N>,
         im2: &Vector<N>,
-        limit: N,
         solver_vel1: &mut SolverVel<N>,
         solver_vel2: &mut SolverVel<N>,
     ) where
         AngVector<N>: SimdDot<AngVector<N>, Result = N>,
     {
-        if DISABLE_FRICTION_LIMIT_REAPPLY {
-            return;
-        }
-
         #[cfg(feature = "dim2")]
         {
-            let new_impulse = self.impulse[0].simd_clamp(-limit, limit);
+            solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0];
-            let dlambda = new_impulse - self.impulse[0];
+            solver_vel1.angular += self.gcross1[0] * self.impulse[0];
-            self.impulse[0] = new_impulse;
 
-            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
-            solver_vel1.angular += self.gcross1[0] * dlambda;
+            solver_vel2.angular += self.gcross2[0] * self.impulse[0];
-
-            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
-            solver_vel2.angular += self.gcross2[0] * dlambda;
         }
 
         #[cfg(feature = "dim3")]
         {
-            let new_impulse = self.impulse;
+            solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0]
-            let new_impulse = {
+                + tangents1[1].component_mul(im1) * self.impulse[1];
-                let _disable_fe_except =
+            solver_vel1.angular +=
-                    crate::utils::DisableFloatingPointExceptionsFlags::
+                self.gcross1[0] * self.impulse[0] + self.gcross1[1] * self.impulse[1];
-                    disable_floating_point_exceptions();
-                new_impulse.simd_cap_magnitude(limit)
-            };
 
-            let dlambda = new_impulse - self.impulse;
+            solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
-            self.impulse = new_impulse;
+                + tangents1[1].component_mul(im2) * -self.impulse[1];
-
+            solver_vel2.angular +=
-            solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+                self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
-                + tangents1[1].component_mul(im1) * dlambda[1];
-            solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
-
-            solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
-                + tangents1[1].component_mul(im2) * -dlambda[1];
-            solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
         }
     }
 
@@ -220,6 +192,22 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
         self.impulse_accumulator + self.impulse
     }
 
+    #[inline]
+    pub fn warmstart(
+        &mut self,
+        dir1: &Vector<N>,
+        im1: &Vector<N>,
+        im2: &Vector<N>,
+        solver_vel1: &mut SolverVel<N>,
+        solver_vel2: &mut SolverVel<N>,
+    ) {
+        solver_vel1.linear += dir1.component_mul(im1) * self.impulse;
+        solver_vel1.angular += self.gcross1 * self.impulse;
+
+        solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
+        solver_vel2.angular += self.gcross2 * self.impulse;
+    }
+
     #[inline]
     pub fn solve(
         &mut self,
@@ -339,6 +327,31 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
         }
     }
 
+    #[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,
@@ -350,19 +363,13 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
         limit: N,
         solver_vel1: &mut SolverVel<N>,
         solver_vel2: &mut SolverVel<N>,
-        solve_normal: bool,
+        solve_restitution: bool,
         solve_friction: bool,
     ) where
         Vector<N>: SimdBasis,
         AngVector<N>: SimdDot<AngVector<N>, Result = N>,
     {
-        #[cfg(feature = "dim3")]
+        if solve_restitution {
-        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 {
@@ -379,12 +386,6 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
                         solver_vel1,
                         solver_vel2,
                     );
-
-                    for i in 0..2 {
-                        let limit = limit * elements[i].normal_part.impulse;
-                        let part = &mut elements[i].tangent_part;
-                        part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
-                    }
                 }
 
                 // There is one constraint left to solve if there isn’t an even number.
@@ -393,24 +394,22 @@ impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
                     element
                         .normal_part
                         .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
-                    let limit = limit * element.normal_part.impulse;
-                    let part = &mut element.tangent_part;
-                    part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
                 }
             } else {
                 for element in elements.iter_mut() {
                     element
                         .normal_part
                         .solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
-                    let limit = limit * element.normal_part.impulse;
-                    let part = &mut element.tangent_part;
-                    part.apply_limit(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
                 }
             }
         }
 
-        // Solve friction.
         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;

src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs

@@ -8,8 +8,8 @@ use crate::dynamics::{
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
-    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS,
+    AngVector, AngularInertia, Isometry, Point, Real, SimdReal, TangentImpulse, Vector, DIM,
-    SIMD_WIDTH,
+    MAX_MANIFOLD_POINTS, SIMD_WIDTH,
 };
 #[cfg(feature = "dim2")]
 use crate::utils::SimdBasis;
@@ -104,6 +104,11 @@ impl TwoBodyConstraintBuilderSimd {
                 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]);
@@ -140,7 +145,7 @@ impl TwoBodyConstraintBuilderSimd {
                         gcross2,
                         rhs: na::zero(),
                         rhs_wo_bias: na::zero(),
-                        impulse: SimdReal::splat(0.0),
+                        impulse: warmstart_impulse,
                         impulse_accumulator: SimdReal::splat(0.0),
                         r: projected_mass,
                         r_mat_elts: [SimdReal::zero(); 2],
@@ -148,7 +153,7 @@ impl TwoBodyConstraintBuilderSimd {
                 }
 
                 // tangent parts.
-                constraint.elements[k].tangent_part.impulse = na::zero();
+                constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
 
                 for j in 0..DIM - 1 {
                     let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -253,6 +258,7 @@ impl TwoBodyConstraintBuilderSimd {
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
         let max_penetration_correction = SimdReal::splat(params.max_penetration_correction);
+        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]]];
@@ -297,13 +303,13 @@ impl TwoBodyConstraintBuilderSimd {
                 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 = na::zero();
+                element.normal_part.impulse *= warmstart_coeff;
             }
 
             // tangent parts.
             {
                 element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
-                element.tangent_part.impulse = na::zero();
+                element.tangent_part.impulse *= warmstart_coeff;
 
                 for j in 0..DIM - 1 {
                     let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
@@ -334,6 +340,38 @@ pub(crate) struct TwoBodyConstraintSimd {
 }
 
 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>],
@@ -377,27 +415,20 @@ impl TwoBodyConstraintSimd {
 
     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();
-            #[cfg(feature = "dim2")]
-            let tangent_impulses: [_; SIMD_WIDTH] =
-                self.elements[k].tangent_part.total_impulse()[0].into();
-            #[cfg(feature = "dim3")]
             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);
-                #[cfg(feature = "dim2")]
-                {
-                    active_contact.data.tangent_impulse = tangent_impulses[ii];
-                }
-                #[cfg(feature = "dim3")]
-                {
-                    active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
-                }
             }
         }
     }

src/dynamics/solver/velocity_solver.rs

@@ -1,5 +1,4 @@
 use super::{JointConstraintTypes, SolverConstraintsSet};
-use crate::dynamics::integration_parameters::DISABLE_FRICTION_LIMIT_REAPPLY;
 use crate::dynamics::solver::solver_body::SolverBody;
 use crate::dynamics::{
     solver::{ContactConstraintTypes, SolverVel},
@@ -180,6 +179,10 @@ impl VelocitySolver {
             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);
+            }
+
             for _ in 0..params.num_internal_pgs_iterations {
                 joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels);
                 contact_constraints
@@ -203,60 +206,17 @@ impl VelocitySolver {
             /*
              * Resolution without bias.
              */
-            let compute_max_dlinvel = |vels: &[SolverVel<Real>]| {
+            for _ in 0..params.num_internal_stabilization_iterations {
-                vels.iter()
-                    .map(|v| v.linear.norm())
-                    .max_by_key(|v| OrderedFloat(*v))
-                    .unwrap_or_default()
-            };
-
-            let mut prev_dlinvel = f32::MAX;
-            let mut prev_solver_vels = self.solver_vels.clone();
-
-            for kk in 0..params.max_internal_stabilization_iterations {
-                prev_solver_vels.clone_from_slice(&self.solver_vels);
                 joint_constraints
                     .solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
                 contact_constraints.solve_restitution_wo_bias(
                     &mut self.solver_vels,
                     &mut self.generic_solver_vels,
                 );
-
-                if DISABLE_FRICTION_LIMIT_REAPPLY {
-                    contact_constraints
-                        .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
-                }
-
-                for (prev, new) in prev_solver_vels.iter_mut().zip(self.solver_vels.iter()) {
-                    *prev -= *new;
-                }
-
-                let new_max_linvel = compute_max_dlinvel(&self.solver_vels);
-
-                println!(">> {} >> max_linvel: {}", kk, new_max_linvel);
-
-                if new_max_linvel > prev_dlinvel {
-                    break;
-                }
-
-                prev_dlinvel = new_max_linvel;
-
-                if prev_solver_vels
-                    .iter()
-                    .zip(self.solver_vels.iter())
-                    .all(|(diff, vels)| {
-                        diff.linear.norm() < 1.0e-3
-                            || diff.linear.norm() <= 0.2 * vels.linear.norm()
-                    })
-                {
-                    break;
-                }
-
-                // if (new_max_dlinvel - max_dlinvel).abs() <= 0.2 * max_dlinvel {
-                //     println!("Num effective stab steps: {}", kk + 1);
-                //     break;
-                // }
             }
+
+            contact_constraints
+                .solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
         }
     }
 

src/geometry/contact_pair.rs

@@ -1,8 +1,9 @@
 use crate::dynamics::{RigidBodyHandle, RigidBodySet};
 use crate::geometry::{ColliderHandle, ColliderSet, Contact, ContactManifold};
-use crate::math::{Point, Real, Vector};
+use crate::math::{Point, Real, TangentImpulse, Vector, ANG_DIM};
 use crate::pipeline::EventHandler;
 use crate::prelude::CollisionEventFlags;
+use parry::math::AngVector;
 use parry::query::ContactManifoldsWorkspace;
 
 use super::CollisionEvent;
@@ -33,12 +34,11 @@ pub struct ContactData {
     pub impulse: Real,
     /// The friction impulse along the vector orthonormal to the contact normal, applied to the first
     /// collider's rigid-body.
-    #[cfg(feature = "dim2")]
+    pub tangent_impulse: TangentImpulse<Real>,
-    pub tangent_impulse: Real,
+    /// The impulse retained for warmstarting the next simulation step.
-    /// The friction impulses along the basis orthonormal to the contact normal, applied to the first
+    pub warmstart_impulse: Real,
-    /// collider's rigid-body.
+    /// The friction impulse retained for warmstarting the next simulation step.
-    #[cfg(feature = "dim3")]
+    pub warmstart_tangent_impulse: TangentImpulse<Real>,
-    pub tangent_impulse: na::Vector2<Real>,
 }
 
 impl Default for ContactData {
@@ -46,6 +46,8 @@ impl Default for ContactData {
         Self {
             impulse: 0.0,
             tangent_impulse: na::zero(),
+            warmstart_impulse: 0.0,
+            warmstart_tangent_impulse: na::zero(),
         }
     }
 }
@@ -299,6 +301,8 @@ pub struct SolverContact {
     pub tangent_velocity: Vector<Real>,
     /// Whether or not this contact existed during the last timestep.
     pub is_new: bool,
+    pub warmstart_impulse: Real,
+    pub warmstart_tangent_impulse: TangentImpulse<Real>,
 }
 
 impl SolverContact {

src/geometry/narrow_phase.rs

@@ -987,6 +987,8 @@ impl NarrowPhase {
                                 restitution,
                                 tangent_velocity: Vector::zeros(),
                                 is_new: contact.data.impulse == 0.0,
+                                warmstart_impulse: contact.data.warmstart_impulse,
+                                warmstart_tangent_impulse: contact.data.warmstart_tangent_impulse,
                             };
 
                             manifold.data.solver_contacts.push(solver_contact);

src/lib.rs

@@ -166,6 +166,9 @@ pub mod math {
     #[cfg(feature = "dim2")]
     pub type JacobianViewMut<'a, N> = na::MatrixViewMut3xX<'a, N>;
 
+    #[cfg(feature = "dim2")]
+    pub type TangentImpulse<N> = na::Vector1<N>;
+
     /// The maximum number of possible rotations and translations of a rigid body.
     #[cfg(feature = "dim2")]
     pub const SPATIAL_DIM: usize = 3;
@@ -195,6 +198,9 @@ pub mod math {
     #[cfg(feature = "dim3")]
     pub type JacobianViewMut<'a, N> = na::MatrixViewMut6xX<'a, N>;
 
+    #[cfg(feature = "dim3")]
+    pub type TangentImpulse<N> = na::Vector2<N>;
+
     /// The maximum number of possible rotations and translations of a rigid body.
     #[cfg(feature = "dim3")]
     pub const SPATIAL_DIM: usize = 6;

src_testbed/testbed.rs

@@ -102,10 +102,12 @@ bitflags! {
     }
 }
 
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)]
 pub enum RapierSolverType {
-    SmallStepsPgs,
+    #[default]
-    StandardPgs,
+    TgsSoft,
+    TgsSoftNoWarmstart,
+    PgsLegacy,
 }
 
 pub type SimulationBuilders = Vec<(&'static str, fn(&mut Testbed))>;
@@ -214,7 +216,7 @@ impl TestbedApp {
             example_names: Vec::new(),
             selected_example: 0,
             selected_backend: RAPIER_BACKEND,
-            solver_type: RapierSolverType::SmallStepsPgs,
+            solver_type: RapierSolverType::default(),
             physx_use_two_friction_directions: true,
             nsteps: 1,
             camera_locked: false,
@@ -1202,18 +1204,6 @@ fn update_testbed(
             }
             plugins.0.clear();
 
-            if state.selected_example != prev_example {
-                harness.physics.integration_parameters = IntegrationParameters::default();
-
-                match state.solver_type {
-                    RapierSolverType::SmallStepsPgs => {} // It’s already the default.
-                    RapierSolverType::StandardPgs => harness
-                        .physics
-                        .integration_parameters
-                        .switch_to_standard_pgs_solver(),
-                }
-            }
-
             let selected_example = state.selected_example;
             let graphics = &mut *graphics;
             let meshes = &mut *meshes;

src_testbed/ui.rs

@@ -12,6 +12,7 @@ use crate::testbed::{
 use crate::PhysicsState;
 use bevy_egui::egui::Slider;
 use bevy_egui::{egui, EguiContexts};
+use rapier::dynamics::IntegrationParameters;
 
 pub fn update_ui(
     ui_context: &mut EguiContexts,
@@ -109,8 +110,9 @@ pub fn update_ui(
                 .selected_text(format!("{:?}", state.solver_type))
                 .show_ui(ui, |ui| {
                     let solver_types = [
-                        RapierSolverType::SmallStepsPgs,
+                        RapierSolverType::TgsSoft,
-                        RapierSolverType::StandardPgs,
+                        RapierSolverType::TgsSoftNoWarmstart,
+                        RapierSolverType::PgsLegacy,
                     ];
                     for sty in solver_types {
                         changed = ui
@@ -122,11 +124,15 @@ pub fn update_ui(
 
             if changed {
                 match state.solver_type {
-                    RapierSolverType::SmallStepsPgs => {
+                    RapierSolverType::TgsSoft => {
-                        integration_parameters.switch_to_small_steps_pgs_solver()
+                        *integration_parameters = IntegrationParameters::tgs_soft();
                     }
-                    RapierSolverType::StandardPgs => {
+                    RapierSolverType::TgsSoftNoWarmstart => {
-                        integration_parameters.switch_to_standard_pgs_solver()
+                        *integration_parameters =
+                            IntegrationParameters::tgs_soft_without_warmstart();
+                    }
+                    RapierSolverType::PgsLegacy => {
+                        *integration_parameters = IntegrationParameters::pgs_legacy();
                     }
                 }
             }
@@ -146,17 +152,31 @@ pub fn update_ui(
             ui.add(
                 Slider::new(
                     &mut integration_parameters.num_additional_friction_iterations,
-                    1..=40,
+                    0..=40,
                 )
                 .text("num additional frict. iters."),
             );
             ui.add(
                 Slider::new(
-                    &mut integration_parameters.max_internal_stabilization_iterations,
+                    &mut integration_parameters.num_internal_stabilization_iterations,
                     1..=100,
                 )
                 .text("max internal stabilization iters."),
             );
+            ui.add(
+                Slider::new(&mut integration_parameters.warmstart_coefficient, 0.0..=1.0)
+                    .text("warmstart coefficient"),
+            );
+            ui.add(Slider::new(&mut integration_parameters.erp, 0.0..=1.0).text("erp"));
+            ui.add(
+                Slider::new(&mut integration_parameters.damping_ratio, 0.0..=20.0)
+                    .text("damping ratio"),
+            );
+            ui.add(Slider::new(&mut integration_parameters.joint_erp, 0.0..=1.0).text("joint erp"));
+            ui.add(
+                Slider::new(&mut integration_parameters.joint_damping_ratio, 0.0..=20.0)
+                    .text("joint damping ratio"),
+            );
         }
 
         #[cfg(feature = "parallel")]