Merge branch 'master' into collider-builder-debug

18 files changed, 1003 insertions, 8 deletions

diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index 4cdbf98..39d85f6 100644
--- a/examples2d/all_examples2.rs
+++ b/examples2d/all_examples2.rs
@@ -15,8 +15,12 @@ mod collision_groups2;
 mod convex_polygons2;
 mod damping2;
 mod debug_box_ball2;
+mod debug_compression2;
+mod debug_total_overlap2;
+mod debug_vertical_column2;
 mod drum2;
 mod heightfield2;
+mod inverse_kinematics2;
 mod joint_motor_position2;
 mod joints2;
 mod locked_rotations2;
@@ -26,6 +30,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;
 
@@ -70,6 +85,7 @@ pub fn main() {
         ("Damping", damping2::init_world),
         ("Drum", drum2::init_world),
         ("Heightfield", heightfield2::init_world),
+        ("Inverse kinematics", inverse_kinematics2::init_world),
         ("Joints", joints2::init_world),
         ("Locked rotations", locked_rotations2::init_world),
         ("One-way platforms", one_way_platforms2::init_world),
@@ -82,6 +98,23 @@ pub fn main() {
         ("Trimesh", trimesh2::init_world),
         ("Joint motor position", joint_motor_position2::init_world),
         ("(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.
diff --git a/examples2d/character_controller2.rs b/examples2d/character_controller2.rs
index 8ecd23c..2325101 100644
--- a/examples2d/character_controller2.rs
+++ b/examples2d/character_controller2.rs
@@ -27,7 +27,7 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0]);
     let character_handle = bodies.insert(rigid_body);
-    let collider = ColliderBuilder::cuboid(0.15, 0.3);
+    let collider = ColliderBuilder::capsule_y(0.3, 0.15);
     colliders.insert_with_parent(collider, character_handle, &mut bodies);
 
     /*
@@ -94,14 +94,18 @@ pub fn init_world(testbed: &mut Testbed) {
      */
     let wall_angle = PI / 2.;
     let wall_size = 2.0;
+    let wall_pos = vector![
+        ground_size + slope_size * 2.0 + impossible_slope_size + 0.35,
+        -ground_height + 2.5 * 2.3
+    ];
     let collider = ColliderBuilder::cuboid(wall_size, ground_height)
-        .translation(vector![
-            ground_size + slope_size * 2.0 + impossible_slope_size + 0.35,
-            -ground_height + 2.5 * 2.3
-        ])
+        .translation(wall_pos)
         .rotation(wall_angle);
     colliders.insert(collider);
 
+    let collider = ColliderBuilder::cuboid(wall_size, ground_height).translation(wall_pos);
+    colliders.insert(collider);
+
     /*
      * Create a moving platform.
      */
diff --git a/examples2d/debug_compression2.rs b/examples2d/debug_compression2.rs
new file mode 100644
index 0000000..e0052d6
--- /dev/null
+++ b/examples2d/debug_compression2.rs
@@ -0,0 +1,75 @@
+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();
+
+    /*
+     * Ground
+     */
+    let width = 75.0;
+    let thickness = 2.0;
+    let ys = [-30.0 - thickness, 30.0 + thickness];
+
+    for y in ys {
+        let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, y]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(width, thickness);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    // Build two compression boxes rigid body.
+    let half_height = (ys[1] - ys[0]) / 2.0 - thickness;
+    let xs = [-width + thickness, width - thickness];
+    let mut handles = [RigidBodyHandle::invalid(); 2];
+
+    for i in 0..2 {
+        let rigid_body = RigidBodyBuilder::dynamic().translation(vector![xs[i], 0.0]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(thickness, half_height);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+        handles[i] = handle;
+    }
+
+    // Build the balls.
+    let num = 8;
+    let rad = half_height / (num as f32);
+    for i in 0..num {
+        for j in 0..num {
+            let x = i as f32 * rad * 2.0 - num as f32 * rad;
+            let y = j as f32 * rad * 2.0 - num as f32 * rad + rad;
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad);
+            colliders.insert_with_parent(collider, handle, &mut bodies);
+        }
+    }
+
+    let mut force = vector![0.0, 0.0];
+
+    testbed.add_callback(move |_, physics, _, _| {
+        let left_plank = &mut physics.bodies[handles[0]];
+        left_plank.reset_forces(true);
+        left_plank.add_force(force, true);
+
+        let right_plank = &mut physics.bodies[handles[1]];
+        right_plank.reset_forces(true);
+        right_plank.add_force(-force, true);
+
+        force.x += 10000.0;
+
+        println!("force: {}", force.x);
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.0], 50.0);
+}
diff --git a/examples2d/debug_total_overlap2.rs b/examples2d/debug_total_overlap2.rs
new file mode 100644
index 0000000..b61ad67
--- /dev/null
+++ b/examples2d/debug_total_overlap2.rs
@@ -0,0 +1,28 @@
+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();
+
+    // Build many balls, all spawned at the same point.
+    let rad = 0.5;
+
+    for _ in 0..100 {
+        let rigid_body = RigidBodyBuilder::dynamic();
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(rad, rad);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.0], 50.0);
+}
diff --git a/examples2d/debug_vertical_column2.rs b/examples2d/debug_vertical_column2.rs
new file mode 100644
index 0000000..4ca50db
--- /dev/null
+++ b/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);
+}
diff --git a/examples2d/inverse_kinematics2.rs b/examples2d/inverse_kinematics2.rs
new file mode 100644
index 0000000..985bdb2
--- /dev/null
+++ b/examples2d/inverse_kinematics2.rs
@@ -0,0 +1,96 @@
+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 mut multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 1.0;
+    let ground_height = 0.01;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]);
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height);
+    colliders.insert_with_parent(collider, floor_handle, &mut bodies);
+
+    /*
+     * Setup groups
+     */
+    let num_segments = 10;
+    let body = RigidBodyBuilder::fixed();
+    let mut last_body = bodies.insert(body);
+    let mut last_link = MultibodyJointHandle::invalid();
+
+    for i in 0..num_segments {
+        let size = 1.0 / num_segments as f32;
+        let body = RigidBodyBuilder::dynamic().can_sleep(false);
+        let new_body = bodies.insert(body);
+        // NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
+        //       the demo look nicer. IK could be used without cuboid.
+        let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0)
+            .density(0.0)
+            .sensor(true);
+        colliders.insert_with_parent(collider, new_body, &mut bodies);
+
+        let link_ab = RevoluteJointBuilder::new()
+            .local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32])
+            .local_anchor2(point![0.0, -size / 2.0])
+            .build()
+            .data;
+
+        last_link = multibody_joints
+            .insert(last_body, new_body, link_ab, true)
+            .unwrap();
+
+        last_body = new_body;
+    }
+
+    let mut displacements = DVector::zeros(0);
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        let Some(graphics) = graphics else { return };
+        if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
+            // Ensure our displacement vector has the right number of elements.
+            if displacements.nrows() < multibody.ndofs() {
+                displacements = DVector::zeros(multibody.ndofs());
+            } else {
+                displacements.fill(0.0);
+            }
+
+            let Some(mouse_point) = graphics.mouse().point else {
+                return;
+            };
+
+            // We will have the endpoint track the mouse position.
+            let target_point = mouse_point.coords;
+
+            let options = InverseKinematicsOption {
+                constrained_axes: JointAxesMask::LIN_AXES,
+                ..Default::default()
+            };
+
+            multibody.inverse_kinematics(
+                &physics.bodies,
+                link_id,
+                &options,
+                &Isometry::from(target_point),
+                &mut displacements,
+            );
+            multibody.apply_displacements(displacements.as_slice());
+        }
+    });
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 0.0], 300.0);
+}
diff --git a/examples2d/s2d_arch.rs b/examples2d/s2d_arch.rs
new file mode 100644
index 0000000..5b935e9
--- /dev/null
+++ b/examples2d/s2d_arch.rs
@@ -0,0 +1,109 @@
+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();
+
+    #[allow(clippy::excessive_precision)]
+    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),
+    ];
+
+    #[allow(clippy::excessive_precision)]
+    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);
+}
diff --git a/examples2d/s2d_ball_and_chain.rs b/examples2d/s2d_ball_and_chain.rs
new file mode 100644
index 0000000..29e0d87
--- /dev/null
+++ b/examples2d/s2d_ball_and_chain.rs
@@ -0,0 +1,73 @@
+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(density);
+
+    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(density);
+    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);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 2.5], 20.0);
+}
diff --git a/examples2d/s2d_bridge.rs b/examples2d/s2d_bridge.rs
new file mode 100644
index 0000000..ab9c96f
--- /dev/null
+++ b/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(density);
+        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);
+}
diff --git a/examples2d/s2d_card_house.rs b/examples2d/s2d_card_house.rs
new file mode 100644
index 0000000..5e2c5dc
--- /dev/null
+++ b/examples2d/s2d_card_house.rs
@@ -0,0 +1,78 @@
+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 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);
+}
diff --git a/examples2d/s2d_confined.rs b/examples2d/s2d_confined.rs
new file mode 100644
index 0000000..ee54408
--- /dev/null
+++ b/examples2d/s2d_confined.rs
@@ -0,0 +1,69 @@
+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_from_endpoints(point![-10.5, 0.0], point![10.5, 0.0], radius)
+            .friction(friction);
+    colliders.insert(collider);
+    let collider =
+        ColliderBuilder::capsule_from_endpoints(point![-10.5, 0.0], point![-10.5, 20.5], radius)
+            .friction(friction);
+    colliders.insert(collider);
+    let collider =
+        ColliderBuilder::capsule_from_endpoints(point![10.5, 0.0], point![10.5, 20.5], radius)
+            .friction(friction);
+    colliders.insert(collider);
+    let collider =
+        ColliderBuilder::capsule_from_endpoints(point![-10.5, 20.5], point![10.5, 20.5], radius)
+            .friction(friction);
+    colliders.insert(collider);
+
+    /*
+     * Create the spheres
+     */
+    let mut row;
+    let mut count = 0;
+    let mut column = 0;
+
+    while count < max_count {
+        row = 0;
+        for _ 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);
+}
diff --git a/examples2d/s2d_far_pyramid.rs b/examples2d/s2d_far_pyramid.rs
new file mode 100644
index 0000000..7948731
--- /dev/null
+++ b/examples2d/s2d_far_pyramid.rs
@@ -0,0 +1,50 @@
+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 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);
+}
diff --git a/examples2d/s2d_high_mass_ratio_1.rs b/examples2d/s2d_high_mass_ratio_1.rs
new file mode 100644
index 0000000..b211a5e
--- /dev/null
+++ b/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 * ext