Move benchmark demos into their own directory.

15 files changed, 1305 insertions, 0 deletions

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/benchmarks3d/boxes3.rs b/benchmarks3d/boxes3.rs
new file mode 100644
index 0000000..0eb31f5
--- /dev/null
+++ b/benchmarks3d/boxes3.rs
@@ -0,0 +1,68 @@
+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 = 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;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    for j in 0usize..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx + offset;
+                let y = j as f32 * shift + centery + 3.0;
+                let z = k as f32 * shift - centerz + offset;
+
+                // 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).build();
+                colliders.insert(collider, 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/capsules3.rs b/benchmarks3d/capsules3.rs
new file mode 100644
index 0000000..0e8ad94
--- /dev/null
+++ b/benchmarks3d/capsules3.rs
@@ -0,0 +1,69 @@
+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 = 1.0;
+
+    let shift = rad * 2.0 + rad;
+    let shifty = rad * 4.0;
+    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..47 {
+        for i in 0..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx + offset;
+                let y = j as f32 * shifty + centery + 3.0;
+                let z = k as f32 * shift - centerz + offset;
+
+                // 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).build();
+                colliders.insert(collider, 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/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/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs
new file mode 100644
index 0000000..fee32a2
--- /dev/null
+++ b/benchmarks3d/joint_ball3.rs
@@ -0,0 +1,70 @@
+use na::Point3;
+use rapier3d::dynamics::{BallJoint, 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 mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 100;
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for k in 0..num {
+        for i in 0..num {
+            let fk = k as f32;
+            let fi = i as f32;
+
+            let status = if i == 0 && (k % 4 == 0 || k == num - 1) {
+                BodyStatus::Static
+            } else {
+                BodyStatus::Dynamic
+            };
+
+            let rigid_body = RigidBodyBuilder::new(status)
+                .translation(fk * shift, 0.0, fi * shift)
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).build();
+            colliders.insert(collider, child_handle, &mut bodies);
+
+            // Vertical joint.
+            if i > 0 {
+                let parent_handle = *body_handles.last().unwrap();
+                let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift));
+                joints.insert(&mut bodies, parent_handle, child_handle, joint);
+            }
+
+            // Horizontal joint.
+            if k > 0 {
+                let parent_index = body_handles.len() - num;
+                let parent_handle = body_handles[parent_index];
+                let joint = BallJoint::new(Point3::origin(), Point3::new(-shift, 0.0, 0.0));
+                joints.insert(&mut bodies, parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(-110.0, -46.0, 170.0),
+        Point3::new(54.0, -38.0, 29.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}
diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs
new file mode 100644
index 0000000..c8912e7
--- /dev/null
+++ b/benchmarks3d/joint_fixed3.rs
@@ -0,0 +1,92 @@
+use na::{Isometry3, Point3};
+use rapier3d::dynamics::{BodyStatus, FixedJoint, 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 mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 5;
+    let shift = 1.0;
+
+    let mut body_handles = Vec::new();
+
+    for m in 0..10 {
+        let z = m as f32 * shift * (num as f32 + 2.0);
+
+        for l in 0..10 {
+            let y = l as f32 * shift * 3.0;
+
+            for j in 0..5 {
+                let x = j as f32 * shift * (num as f32) * 2.0;
+
+                for k in 0..num {
+                    for i in 0..num {
+                        let fk = k as f32;
+                        let fi = i as f32;
+
+                        // NOTE: the num - 2 test is to avoid two consecutive
+                        // fixed bodies. Because physx will crash if we add
+                        // a joint between these.
+
+                        let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) {
+                            BodyStatus::Static
+                        } else {
+                            BodyStatus::Dynamic
+                        };
+
+                        let rigid_body = RigidBodyBuilder::new(status)
+                            .translation(x + fk * shift, y, z + fi * shift)
+                            .build();
+                        let child_handle = bodies.insert(rigid_body);
+                        let collider = ColliderBuilder::ball(rad).build();
+                        colliders.insert(collider, child_handle, &mut bodies);
+
+                        // Vertical joint.
+                        if i > 0 {
+                            let parent_handle = *body_handles.last().unwrap();
+                            let joint = FixedJoint::new(
+                                Isometry3::identity(),
+                                Isometry3::translation(0.0, 0.0, -shift),
+                            );
+                            joints.insert(&mut bodies, parent_handle, child_handle, joint);
+                        }
+
+                        // Horizontal joint.
+                        if k > 0 {
+                            let parent_index = body_handles.len() - num;
+                            let parent_handle = body_handles[parent_index];
+                            let joint = FixedJoint::new(
+                                Isometry3::identity(),
+                                Isometry3::translation(-shift, 0.0, 0.0),
+                            );
+                            joints.insert(&mut bodies, parent_handle, child_handle, joint);
+                        }
+
+                        body_handles.push(child_handle);
+                    }
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(-38.0, 14.0, 108.0),
+        Point3::new(46.0, 12.0, 23.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}
diff --git a/benchmarks3d/joint_prismatic3.rs b/benchmarks3d/joint_prismatic3.rs
new file mode 100644
index 0000000..3267ada
--- /dev/null
+++ b/benchmarks3d/joint_prismatic3.rs
@@ -0,0 +1,81 @@
+use na::{Point3, Unit, Vector3};
+use rapier3d::dynamics::{JointSet, PrismaticJoint, 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 mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 5;
+    let shift = 1.0;
+
+    for m in 0..8 {
+        let z = m as f32 * shift * (num as f32 + 2.0);
+
+        for l in 0..8 {
+            let y = l as f32 * shift * (num as f32) * 2.0;
+
+            for j in 0..50 {
+                let x = j as f32 * shift * 4.0;
+
+                let ground = RigidBodyBuilder::new_static().translation(x, y, z).build();
+                let mut curr_parent = bodies.insert(ground);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                colliders.insert(collider, curr_parent, &mut bodies);
+
+                for i in 0..num {
+                    let z = z + (i + 1) as f32 * shift;
+                    let density = 1.0;
+                    let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                    let curr_child = bodies.insert(rigid_body);
+                    let collider = ColliderBuilder::cuboid(rad, rad, rad)
+                        .density(density)
+                        .build();
+                    colliders.insert(collider, curr_child, &mut bodies);
+
+                    let axis = if i % 2 == 0 {
+                        Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
+                    } else {
+                        Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
+                    };
+
+                    let z = Vector3::z();
+                    let mut prism = PrismaticJoint::new(
+                        Point3::origin(),
+                        axis,
+                        z,
+                        Point3::new(0.0, 0.0, -shift),
+                        axis,
+                        z,
+                    );
+                    prism.limits_enabled = true;
+                    prism.limits[0] = -2.0;
+                    prism.limits[1] = 2.0;
+                    joints.insert(&mut bodies, curr_parent, curr_child, prism);
+
+                    curr_parent = curr_child;
+                }
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(262.0, 63.0, 124.0),
+        Point3::new(101.0, 4.0, -3.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}
diff --git a/benchmarks3d/joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs
new file mode 100644
index 0000000..040fc3e
--- /dev/null
+++ b/benchmarks3d/joint_revolute3.rs
@@ -0,0 +1,89 @@
+use na::{Isometry3, Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RevoluteJoint, 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 mut joints = JointSet::new();
+
+    let rad = 0.4;
+    let num = 10;
+    let shift = 2.0;
+
+    for l in 0..4 {
+        let y = l as f32 * shift * (num as f32) * 3.0;
+
+        for j in 0..50 {
+            let x = j as f32 * shift * 4.0;
+
+            let ground = RigidBodyBuilder::new_static()
+                .translation(x, y, 0.0)
+                .build();
+            let mut curr_parent = bodies.insert(ground);
+            let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+            colliders.insert(collider, curr_parent, &mut bodies);
+
+            for i in 0..num {
+                // Create four bodies.
+                let z = i as f32 * shift * 2.0 + shift;
+                let positions = [
+                    Isometry3::translation(x, y, z),
+                    Isometry3::translation(x + shift, y, z),
+                    Isometry3::translation(x + shift, y, z + shift),
+                    Isometry3::translation(x, y, z + shift),
+                ];
+
+                let mut handles = [curr_parent; 4];
+                for k in 0..4 {
+                    let density = 1.0;
+                    let rigid_body = RigidBodyBuilder::new_dynamic()
+                        .position(positions[k])
+                        .build();
+                    handles[k] = bodies.insert(rigid_body);
+                    let collider = ColliderBuilder::cuboid(rad, rad, rad)
+                        .density(density)
+                        .build();
+                    colliders.insert(collider, handles[k], &mut bodies);
+                }
+
+                // Setup four joints.
+                let o = Point3::origin();
+                let x = Vector3::x_axis();
+                let z = Vector3::z_axis();
+
+                let revs = [
+                    RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
+                    RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x),
+                    RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
+                    RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x),
+                ];
+
+                joints.insert(&mut bodies, curr_parent, handles[0], revs[0]);
+                joints.insert(&mut bodies, handles[0], handles[1], revs[1]);
+                joints.insert(&mut bodies, handles[1], handles[2], revs[2]);
+                joints.insert(&mut bodies, handles[2], handles[3], revs[3]);
+
+                curr_parent = handles[3];
+            }
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(
+        Point3::new(478.0, 83.0, 228.0),
+        Point3::new(134.0, 83.0, -116.0),
+    );
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}
diff --git a/benchmarks3d/keva3.rs b/benchmarks3d/keva3.rs
new file mode 100644
index 0000000..6862f25
--- /dev/null
+++ b/benchmarks3d/keva3.rs
@@ -0,0 +1,144 @@
+use na::{Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn build_block(
+    testbed: &mut Testbed,
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    half_extents: Vector3<f32>,
+    shift: Vector3<f32>,
+    mut numx: usize,
+    numy: usize,
+    mut numz: usize,
+) {
+    let dimensions = [half_extents.xyz(), half_extents.zyx()];
+    let block_width = 2.0 * half_extents.z * numx as f32;
+    let block_height = 2.0 * half_extents.y * numy as f32;
+    let spacing = (half_extents.z * numx as f32 - half_extents.x) / (numz as f32 - 1.0);
+    let mut color0 = Point3::new(0.7, 0.5, 0.9);
+    let mut color1 = Point3::new(0.6, 1.0, 0.6);
+
+    for i in 0..numy {
+        std::mem::swap(&mut numx, &mut numz);
+        let dim = dimensions[i % 2];
+        let y = dim.y * i as f32 * 2.0;
+
+        for j in 0..numx {
+            let x = if i % 2 == 0 {
+                spacing * j as f32 * 2.0
+            } else {
+                dim.x * j as f32 * 2.0
+            };
+
+            for k in 0..numz {
+                let z = if i % 2 == 0 {
+                    dim.z * k as f32 * 2.0
+                } else {
+                    spacing * k as f32 * 2.0
+                };
+
+                // Build the rigid body.
+                let rigid_body = RigidBodyBuilder::new_dynamic()
+                    .translation(
+                        x + dim.x + shift.x,
+                        y + dim.y + shift.y,
+                        z + dim.z + shift.z,
+                    )
+                    .build();
+                let handle = bodies.insert(rigid_body);
+                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);
+            }
+        }
+    }
+
+    // Close the top.
+    let dim = half_extents.zxy();
+
+    for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize {
+        for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize {
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .translation(
+                    i as f32 * dim.x * 2.0 + dim.x + shift.x,
+                    dim.y + shift.y + block_height,
+                    j as f32 * dim.z * 2.0 + dim.z + shift.z,
+                )
+                .build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build();<