feat: add a few more debug demos

11 files changed, 250 insertions, 3 deletions

diff --git a/benchmarks2d/all_benchmarks2.rs b/benchmarks2d/all_benchmarks2.rs
index fcc7b95..af71049 100644
--- a/benchmarks2d/all_benchmarks2.rs
+++ b/benchmarks2d/all_benchmarks2.rs
@@ -17,6 +17,7 @@ mod joint_ball2;
 mod joint_fixed2;
 mod joint_prismatic2;
 mod pyramid2;
+mod vertical_stacks2;
 
 fn demo_name_from_command_line() -> Option<String> {
     let mut args = std::env::args();
@@ -57,6 +58,7 @@ pub fn main() {
         ("Convex polygons", convex_polygons2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Pyramid", pyramid2::init_world),
+        ("Verticals tacks", vertical_stacks2::init_world),
         ("(Stress test) joint ball", joint_ball2::init_world),
         ("(Stress test) joint fixed", joint_fixed2::init_world),
         (
diff --git a/benchmarks2d/vertical_stacks2.rs b/benchmarks2d/vertical_stacks2.rs
new file mode 100644
index 0000000..12de83a
--- /dev/null
+++ b/benchmarks2d/vertical_stacks2.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();
+
+    /*
+     * Ground
+     */
+    let ground_size = 100.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);
+    colliders.insert_with_parent(collider, ground_handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 30;
+    let rad = 0.5;
+
+    let shift = rad * 2.0;
+    let centery = shift / 2.0 + ground_thickness;
+
+    for i in 0..num {
+        for j in 0usize..1 + i * 2 {
+            let fj = j as f32;
+            let fi = i as f32;
+            let x = (fj - fi) * shift; // (shift + rad / 2.0);
+            let y = (num as f32 - fi - 1.0) * shift + centery;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y]);
+            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, 2.5], 5.0);
+}
diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs
index 591886e..2f723d1 100644
--- a/benchmarks3d/all_benchmarks3.rs
+++ b/benchmarks3d/all_benchmarks3.rs
@@ -20,6 +20,7 @@ mod joint_fixed3;
 mod joint_prismatic3;
 mod joint_revolute3;
 mod keva3;
+mod many_sleep3;
 mod many_static3;
 mod pyramid3;
 mod stacks3;
@@ -56,6 +57,7 @@ pub fn main() {
         ("Compound", compound3::init_world),
         ("Convex polyhedron", convex_polyhedron3::init_world),
         ("Many static", many_static3::init_world),
+        ("Many sleep", many_sleep3::init_world),
         ("Heightfield", heightfield3::init_world),
         ("Stacks", stacks3::init_world),
         ("Pyramid", pyramid3::init_world),
diff --git a/benchmarks3d/ccd3.rs b/benchmarks3d/ccd3.rs
index f642739..4f7b2c1 100644
--- a/benchmarks3d/ccd3.rs
+++ b/benchmarks3d/ccd3.rs
@@ -56,8 +56,8 @@ pub fn init_world(testbed: &mut Testbed) {
                     1 => ColliderBuilder::ball(rad),
                     // Rounded cylinders are much more efficient that cylinder, even if the
                     // rounding margin is small.
-                    // 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
-                    // 3 => ColliderBuilder::cone(rad, rad),
+                    2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
+                    3 => ColliderBuilder::cone(rad, rad),
                     _ => ColliderBuilder::capsule_y(rad, rad),
                 };
 
diff --git a/benchmarks3d/many_sleep3.rs b/benchmarks3d/many_sleep3.rs
new file mode 100644
index 0000000..70f85cc
--- /dev/null
+++ b/benchmarks3d/many_sleep3.rs
@@ -0,0 +1,54 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::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();
+
+    /*
+     * Create the balls
+     */
+    let num = 50;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + 1.0;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+    let centerz = shift * (num as f32) / 2.0;
+
+    for i in 0..num {
+        for j in 0usize..num {
+            for k in 0..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery;
+                let z = k as f32 * shift - centerz;
+
+                let status = if j == 0 {
+                    RigidBodyType::Fixed
+                } else {
+                    RigidBodyType::Dynamic
+                };
+                let density = 0.477;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new(status)
+                    .translation(vector![x, y, z])
+                    .sleeping(true); // j < num - 1);
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::ball(rad).density(density);
+                colliders.insert_with_parent(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}
diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs
index 4cdbf98..6eff54f 100644
--- a/examples2d/all_examples2.rs
+++ b/examples2d/all_examples2.rs
@@ -15,6 +15,7 @@ mod collision_groups2;
 mod convex_polygons2;
 mod damping2;
 mod debug_box_ball2;
+mod debug_total_overlap2;
 mod drum2;
 mod heightfield2;
 mod joint_motor_position2;
@@ -82,6 +83,7 @@ pub fn main() {
         ("Trimesh", trimesh2::init_world),
         ("Joint motor position", joint_motor_position2::init_world),
         ("(Debug) box ball", debug_box_ball2::init_world),
+        ("(Debug) total overlap", debug_total_overlap2::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.
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/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index bac826e..1525c4c 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -23,9 +23,11 @@ mod debug_deserialize3;
 mod debug_dynamic_collider_add3;
 mod debug_friction3;
 mod debug_infinite_fall3;
+mod debug_pop3;
 mod debug_prismatic3;
 mod debug_rollback3;
 mod debug_shape_modification3;
+mod debug_thin_cube_on_mesh3;
 mod debug_triangle3;
 mod debug_trimesh3;
 mod domino3;
@@ -124,6 +126,7 @@ pub fn main() {
         ),
         ("(Debug) big colliders", debug_big_colliders3::init_world),
         ("(Debug) boxes", debug_boxes3::init_world),
+        ("(Debug) pop", debug_pop3::init_world),
         (
             "(Debug) dyn. coll. add",
             debug_dynamic_collider_add3::init_world,
@@ -141,6 +144,7 @@ pub fn main() {
         ),
         ("(Debug) triangle", debug_triangle3::init_world),
         ("(Debug) trimesh", debug_trimesh3::init_world),
+        ("(Debug) thin cube", debug_thin_cube_on_mesh3::init_world),
         ("(Debug) cylinder", debug_cylinder3::init_world),
         ("(Debug) infinite fall", debug_infinite_fall3::init_world),
         ("(Debug) prismatic", debug_prismatic3::init_world),
diff --git a/examples3d/debug_internal_edges3.rs b/examples3d/debug_internal_edges3.rs
index 7112fb0..86de1bd 100644
--- a/examples3d/debug_internal_edges3.rs
+++ b/examples3d/debug_internal_edges3.rs
@@ -11,7 +11,8 @@ pub fn init_world(testbed: &mut Testbed) {
     let multibody_joints = MultibodyJointSet::new();
 
     let heights = DMatrix::zeros(100, 100);
-    let heightfield = HeightField::new(heights, vector![60.0, 1.0, 60.0]);
+    let heightfield =
+        HeightField::with_flags(heights, vector![60.0, 1.0, 60.0], HeightFieldFlags::all());
     let rotation = vector![0.0, 0.0, 0.0]; // vector![-0.1, 0.0, 0.0];
     colliders
         .insert(ColliderBuilder::new(SharedShape::new(heightfield.clone())).rotation(rotation));
diff --git a/examples3d/debug_pop3.rs b/examples3d/debug_pop3.rs
new file mode 100644
index 0000000..93d58e0
--- /dev/null
+++ b/examples3d/debug_pop3.rs
@@ -0,0 +1,42 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::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 ground_size = 10.0;
+    let ground_height = 10.0;
+
+    for _ in 0..1 {
+        let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    // Build the dynamic box rigid body.
+    for _ in 0..1 {
+        let rigid_body = RigidBodyBuilder::dynamic()
+            // .translation(vector![0.0, 0.1, 0.0])
+            // .rotation(vector![0.8, 0.2, 0.1])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::cuboid(1.0, 1.0, 1.0);
+        colliders.insert_with_parent(collider.clone(), handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
+}
diff --git a/examples3d/debug_thin_cube_on_mesh3.rs b/examples3d/debug_thin_cube_on_mesh3.rs
new file mode 100644
index 0000000..801e0cd
--- /dev/null
+++ b/examples3d/debug_thin_cube_on_mesh3.rs
@@ -0,0 +1,59 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+// This shows a bug when a cylinder is in contact with a very large
+// but very thin cuboid. In this case the EPA returns an incorrect
+// contact normal, resulting in the cylinder falling through the floor.
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 100.1;
+    let ground_height = 0.1;
+
+    // let vertices = vec![
+    //     point![-50.0, 0.0, -50.0],
+    //     point![-50.0, 0.0, 50.0],
+    //     point![50.0, 0.0, 50.0],
+    //     point![50.0, 0.0, -50.0],
+    // ];
+    // let indices = vec![[0, 1, 2], [0, 2, 3]];
+    //
+    // let collider = ColliderBuilder::trimesh_with_flags(vertices, indices, TriMeshFlags::all());
+    // colliders.insert(collider);
+
+    let heights = DMatrix::repeat(2, 2, 0.0);
+    let collider = ColliderBuilder::heightfield_with_flags(
+        heights,
+        Vector::new(50.0, 1.0, 50.0),
+        HeightFieldFlags::FIX_INTERNAL_EDGES,
+    );
+    colliders.insert(collider);
+
+    /*
+     * Create the cubes
+     */
+    // Build the rigid body.
+    let rigid_body = RigidBodyBuilder::dynamic()
+        .translation(vector![0.0, 5.0, 0.0])
+        .rotation(vector![0.5, 0.0, 0.5])
+        .linvel(vector![0.0, -100.0, 0.0])
+        .soft_ccd_enabled(true);
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(0.01, 0.015, 5.0);
+    colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}