feat: add warmstarting to contact constraints resolution

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