Move benchmark demos into their own directory.

44 files changed, 897 insertions, 148 deletions

diff --git a/Cargo.toml b/Cargo.toml
index a766033..100b8bb 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,12 +1,12 @@
 [workspace]
 members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d",
-            "build/rapier3d", "build/rapier_testbed3d", "examples3d" ]
+            "build/rapier3d", "build/rapier_testbed3d", "examples3d", "benchmarks3d" ]
 
 [patch.crates-io]
 #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
 
 [profile.release]
-debug = false
+debug = true
 codegen-units = 1
 #opt-level = 1
 #lto = true
diff --git a/benchmarks3d/Cargo.toml b/benchmarks3d/Cargo.toml
new file mode 100644
index 0000000..81c843a
--- /dev/null
+++ b/benchmarks3d/Cargo.toml
@@ -0,0 +1,27 @@
+[package]
+name    = "rapier-benchmarks-3d"
+version = "0.1.0"
+authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+edition = "2018"
+
+[features]
+parallel = [ "rapier3d/parallel", "rapier_testbed3d/parallel" ]
+simd-stable = [ "rapier3d/simd-stable" ]
+simd-nightly = [ "rapier3d/simd-nightly" ]
+other-backends = [ "rapier_testbed3d/other-backends" ]
+enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
+
+[dependencies]
+rand       = "0.7"
+Inflector  = "0.11"
+nalgebra = "0.22"
+
+[dependencies.rapier_testbed3d]
+path = "../build/rapier_testbed3d"
+
+[dependencies.rapier3d]
+path = "../build/rapier3d"
+
+[[bin]]
+name = "all_benchmarks3"
+path = "all_benchmarks3.rs"
diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs
new file mode 100644
index 0000000..dfbbc51
--- /dev/null
+++ b/benchmarks3d/all_benchmarks3.rs
@@ -0,0 +1,91 @@
+#![allow(dead_code)]
+
+extern crate nalgebra as na;
+
+#[cfg(target_arch = "wasm32")]
+use wasm_bindgen::prelude::*;
+
+use inflector::Inflector;
+
+use rapier_testbed3d::Testbed;
+use std::cmp::Ordering;
+
+mod balls3;
+mod boxes3;
+mod capsules3;
+mod compound3;
+mod heightfield3;
+mod joint_ball3;
+mod joint_fixed3;
+mod joint_prismatic3;
+mod joint_revolute3;
+mod keva3;
+mod pyramid3;
+mod stacks3;
+mod trimesh3;
+
+enum Command {
+    Run(String),
+    List,
+    RunAll,
+}
+
+fn parse_command_line() -> Command {
+    let mut args = std::env::args();
+
+    while let Some(arg) = args.next() {
+        if &arg[..] == "--example" {
+            return Command::Run(args.next().unwrap_or(String::new()));
+        } else if &arg[..] == "--list" {
+            return Command::List;
+        }
+    }
+
+    Command::RunAll
+}
+
+pub fn main() {
+    let command = parse_command_line();
+
+    let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
+        ("Balls", balls3::init_world),
+        ("Boxes", boxes3::init_world),
+        ("Capsules", capsules3::init_world),
+        ("Compound", compound3::init_world),
+        ("Heightfield", heightfield3::init_world),
+        ("Stacks", stacks3::init_world),
+        ("Pyramid", pyramid3::init_world),
+        ("Trimesh", trimesh3::init_world),
+        ("Joint ball", joint_ball3::init_world),
+        ("Joint fixed", joint_fixed3::init_world),
+        ("Joint revolute", joint_revolute3::init_world),
+        ("Joint prismatic", joint_prismatic3::init_world),
+        ("Keva tower", keva3::init_world),
+    ];
+
+    // Lexicographic sort, with stress tests moved at the end of the list.
+    builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) {
+        (true, true) | (false, false) => a.0.cmp(b.0),
+        (true, false) => Ordering::Greater,
+        (false, true) => Ordering::Less,
+    });
+
+    match command {
+        Command::Run(demo) => {
+            if let Some(i) = builders
+                .iter()
+                .position(|builder| builder.0.to_camel_case().as_str() == demo.as_str())
+            {
+                Testbed::from_builders(0, vec![builders[i]]).run()
+            } else {
+                eprintln!("Invalid example to run provided: '{}'", demo);
+            }
+        }
+        Command::RunAll => Testbed::from_builders(0, builders).run(),
+        Command::List => {
+            for builder in &builders {
+                println!("{}", builder.0.to_camel_case())
+            }
+        }
+    }
+}
diff --git a/benchmarks3d/balls3.rs b/benchmarks3d/balls3.rs
new file mode 100644
index 0000000..bdbe75e
--- /dev/null
+++ b/benchmarks3d/balls3.rs
@@ -0,0 +1,58 @@
+use na::Point3;
+use rapier3d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Create the balls
+     */
+    let num = 20;
+    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 {
+                    BodyStatus::Static
+                } else {
+                    BodyStatus::Dynamic
+                };
+                let density = 0.477;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new(status).translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::ball(rad).density(density).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/boxes3.rs b/benchmarks3d/boxes3.rs
index eb8a472..0eb31f5 100644
--- a/examples3d/boxes3.rs
+++ b/benchmarks3d/boxes3.rs
@@ -47,7 +47,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 // Build the rigid body.
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
-                let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
                 colliders.insert(collider, handle, &mut bodies);
             }
         }
diff --git a/examples3d/capsules3.rs b/benchmarks3d/capsules3.rs
index e6aad40..0e8ad94 100644
--- a/examples3d/capsules3.rs
+++ b/benchmarks3d/capsules3.rs
@@ -48,7 +48,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 // Build the rigid body.
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
-                let collider = ColliderBuilder::capsule_y(rad, rad).density(1.0).build();
+                let collider = ColliderBuilder::capsule_y(rad, rad).build();
                 colliders.insert(collider, handle, &mut bodies);
             }
         }
diff --git a/benchmarks3d/compound3.rs b/benchmarks3d/compound3.rs
new file mode 100644
index 0000000..a8a9939
--- /dev/null
+++ b/benchmarks3d/compound3.rs
@@ -0,0 +1,76 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 0.2;
+
+    let shift = rad * 4.0 + rad;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    for j in 0usize..25 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift * 5.0 - centerx + offset;
+                let y = j as f32 * (shift * 5.0) + centery + 3.0;
+                let z = k as f32 * shift * 2.0 - centerz + offset;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
+                let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                    .translation(rad * 10.0, rad * 10.0, 0.0)
+                    .build();
+                let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
+                    .translation(-rad * 10.0, rad * 10.0, 0.0)
+                    .build();
+                colliders.insert(collider1, handle, &mut bodies);
+                colliders.insert(collider2, handle, &mut bodies);
+                colliders.insert(collider3, handle, &mut bodies);
+            }
+        }
+
+        offset -= 0.05 * rad * (num as f32 - 1.0);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/benchmarks3d/heightfield3.rs b/benchmarks3d/heightfield3.rs
new file mode 100644
index 0000000..4f8211c
--- /dev/null
+++ b/benchmarks3d/heightfield3.rs
@@ -0,0 +1,78 @@
+use na::{DMatrix, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = Vector3::new(200.0, 1.0, 200.0);
+    let nsubdivs = 20;
+
+    let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
+        if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
+            10.0
+        } else {
+            let x = i as f32 * ground_size.x / (nsubdivs as f32);
+            let z = j as f32 * ground_size.z / (nsubdivs as f32);
+            x.sin() + z.cos()
+        }
+    });
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::heightfield(heights, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let centerx = shift * (num / 2) as f32;
+    let centery = shift / 2.0;
+    let centerz = shift * (num / 2) as f32;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+
+                if j % 2 == 0 {
+                    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                } else {
+                    let collider = ColliderBuilder::ball(rad).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/stress_joint_ball3.rs b/benchmarks3d/joint_ball3.rs
index bbff217..fee32a2 100644
--- a/examples3d/stress_joint_ball3.rs
+++ b/benchmarks3d/joint_ball3.rs
@@ -32,7 +32,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 .translation(fk * shift, 0.0, fi * shift)
                 .build();
             let child_handle = bodies.insert(rigid_body);
-            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            let collider = ColliderBuilder::ball(rad).build();
             colliders.insert(collider, child_handle, &mut bodies);
 
             // Vertical joint.
diff --git a/examples3d/stress_joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs
index b85879d..c8912e7 100644
--- a/examples3d/stress_joint_fixed3.rs
+++ b/benchmarks3d/joint_fixed3.rs
@@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
                             .translation(x + fk * shift, y, z + fi * shift)
                             .build();
                         let child_handle = bodies.insert(rigid_body);
-                        let collider = ColliderBuilder::ball(rad).density(1.0).build();
+                        let collider = ColliderBuilder::ball(rad).build();
                         colliders.insert(collider, child_handle, &mut bodies);
 
                         // Vertical joint.
diff --git a/examples3d/stress_joint_prismatic3.rs b/benchmarks3d/joint_prismatic3.rs
index 3267ada..3267ada 100644
--- a/examples3d/stress_joint_prismatic3.rs
+++ b/benchmarks3d/joint_prismatic3.rs
diff --git a/examples3d/stress_joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs
index 040fc3e..040fc3e 100644
--- a/examples3d/stress_joint_revolute3.rs
+++ b/benchmarks3d/joint_revolute3.rs
diff --git a/examples3d/stress_keva3.rs b/benchmarks3d/keva3.rs
index 2a41e47..6862f25 100644
--- a/examples3d/stress_keva3.rs
+++ b/benchmarks3d/keva3.rs
@@ -48,9 +48,7 @@ pub fn build_block(
                     )
                     .build();
                 let handle = bodies.insert(rigid_body);
-                let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z)
-                    .density(1.0)
-                    .build();
+                let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build();
                 colliders.insert(collider, handle, bodies);
 
                 testbed.set_body_color(handle, color0);
@@ -73,9 +71,7 @@ pub fn build_block(
                 )
                 .build();
             let handle = bodies.insert(rigid_body);
-            let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z)
-                .density(1.0)
-                .build();
+            let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build();
             colliders.insert(collider, handle, bodies);
             testbed.set_body_color(handle, color0);
             std::mem::swap(&mut color0, &mut color1);
diff --git a/examples3d/pyramid3.rs b/benchmarks3d/pyramid3.rs
index e2aae39..5b6f34e 100644
--- a/examples3d/pyramid3.rs
+++ b/benchmarks3d/pyramid3.rs
@@ -28,9 +28,7 @@ fn create_pyramid(
                 let rigid_body_handle = bodies.insert(rigid_body);
 
                 let collider =
-                    ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z)
-                        .density(1.0)
-                        .build();
+                    ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
                 colliders.insert(collider, rigid_body_handle, bodies);
             }
         }
diff --git a/benchmarks3d/stacks3.rs b/benchmarks3d/stacks3.rs
new file mode 100644
index 0000000..435dcb5
--- /dev/null
+++ b/benchmarks3d/stacks3.rs
@@ -0,0 +1,191 @@
+use na::{Point3, Translation3, UnitQuaternion, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+fn create_tower_circle(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    nsubdivs: usize,
+    half_extents: Vector3<f32>,
+) {
+    let ang_step = std::f32::consts::PI * 2.0 / nsubdivs as f32;
+    let radius = 1.3 * nsubdivs as f32 * half_extents.x / std::f32::consts::PI;
+
+    let shift = half_extents * 2.0;
+    for i in 0usize..stack_height {
+        for j in 0..nsubdivs {
+            let fj = j as f32;
+            let fi = i as f32;
+            let y = fi * shift.y;
+            let pos = Translation3::from(offset)
+                * UnitQuaternion::new(Vector3::y() * (fi / 2.0 + fj) * ang_step)
+                * Translation3::new(0.0, y, radius);
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().position(pos).build();
+            let handle = bodies.insert(rigid_body);
+            let collider =
+                ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
+            colliders.insert(collider, handle, bodies);
+        }
+    }
+}
+
+fn create_wall(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    half_extents: Vector3<f32>,
+) {
+    let shift = half_extents * 2.0;
+    for i in 0usize..stack_height {
+        for j in i..stack_height {
+            let fj = j as f32;
+            let fi = i as f32;
+            let x = offset.x;
+            let y = fi * shift.y + offset.y;
+            let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
+                - stack_height as f32 * half_extents.z;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+            let handle = bodies.insert(rigid_body);
+            let collider =
+                ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
+            colliders.insert(collider, handle, bodies);
+        }
+    }
+}
+
+fn create_pyramid(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    offset: Vector3<f32>,
+    stack_height: usize,
+    half_extents: Vector3<f32>,
+) {
+    let shift = half_extents * 2.0;
+
+    for i in 0usize..stack_height {
+        for j in i..stack_height {
+            for k in i..stack_height {
+                let fi = i as f32;
+                let fj = j as f32;
+                let fk = k as f32;
+                let x = (fi * shift.x / 2.0) + (fk - fi) * shift.x + offset.x
+                    - stack_height as f32 * half_extents.x;
+                let y = fi * shift.y + offset.y;
+                let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z
+                    - stack_height as f32 * half_extents.z;
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider =
+                    ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
+                colliders.insert(collider, handle, bodies);
+            }
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 200.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let cube_size = 1.0;
+    let hext = Vector3::repeat(cube_size);
+    let bottomy = cube_size * 50.0;
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-110.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-80.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-50.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_pyramid(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-20.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_wall(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(-2.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_wall(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(4.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_wall(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(10.0, bottomy, 0.0),
+        12,
+        hext,
+    );
+    create_tower_circle(
+        &mut bodies,
+        &mut colliders,
+        Vector3::new(25.0, bottomy, 0.0),
+        8,
+        24,
+        hext,
+    );
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/benchmarks3d/trimesh3.rs b/benchmarks3d/trimesh3.rs
new file mode 100644
index 0000000..ce1d1c5
--- /dev/null
+++ b/benchmarks3d/trimesh3.rs
@@ -0,0 +1,88 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */