20 files changed, 1885 insertions, 0 deletions

diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml
new file mode 100644
index 0000000..3176696
--- /dev/null
+++ b/examples3d/Cargo.toml
@@ -0,0 +1,27 @@
+[package]
+name    = "nphysics-examples-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_examples3"
+path = "./all_examples3.rs"
\ No newline at end of file
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
new file mode 100644
index 0000000..10b8f33
--- /dev/null
+++ b/examples3d/all_examples3.rs
@@ -0,0 +1,110 @@
+#![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 debug_boxes3;
+mod debug_triangle3;
+mod domino3;
+mod heightfield3;
+mod joints3;
+mod kinematic3;
+mod pyramid3;
+mod sensor3;
+mod stacks3;
+mod stress_joint_ball3;
+mod stress_joint_fixed3;
+mod stress_joint_prismatic3;
+mod stress_joint_revolute3;
+mod stress_keva3;
+mod trimesh3;
+
+fn demo_name_from_command_line() -> Option<String> {
+    let mut args = std::env::args();
+
+    while let Some(arg) = args.next() {
+        if &arg[..] == "--example" {
+            return args.next();
+        }
+    }
+
+    None
+}
+
+#[cfg(any(target_arch = "wasm32", target_arch = "asmjs"))]
+fn demo_name_from_url() -> Option<String> {
+    None
+    //    let window = stdweb::web::window();
+    //    let hash = window.location()?.search().ok()?;
+    //    if hash.len() > 0 {
+    //        Some(hash[1..].to_string())
+    //    } else {
+    //        None
+    //    }
+}
+
+#[cfg(not(any(target_arch = "wasm32", target_arch = "asmjs")))]
+fn demo_name_from_url() -> Option<String> {
+    None
+}
+
+#[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))]
+pub fn main() {
+    let demo = demo_name_from_command_line()
+        .or_else(|| demo_name_from_url())
+        .unwrap_or(String::new())
+        .to_camel_case();
+
+    let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
+        ("Balls", balls3::init_world),
+        ("Boxes", boxes3::init_world),
+        ("Capsules", capsules3::init_world),
+        ("Domino", domino3::init_world),
+        ("Heightfield", heightfield3::init_world),
+        ("Joints", joints3::init_world),
+        ("Kinematic", kinematic3::init_world),
+        ("Stacks", stacks3::init_world),
+        ("Pyramid", pyramid3::init_world),
+        ("Sensor", sensor3::init_world),
+        ("Trimesh", trimesh3::init_world),
+        ("(Debug) boxes", debug_boxes3::init_world),
+        ("(Debug) triangle", debug_triangle3::init_world),
+        ("(Stress test) joint ball", stress_joint_ball3::init_world),
+        ("(Stress test) joint fixed", stress_joint_fixed3::init_world),
+        (
+            "(Stress test) joint revolute",
+            stress_joint_revolute3::init_world,
+        ),
+        (
+            "(Stress test) joint prismatic",
+            stress_joint_prismatic3::init_world,
+        ),
+        ("(Stress test) keva tower", stress_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,
+    });
+
+    let i = builders
+        .iter()
+        .position(|builder| builder.0.to_camel_case().as_str() == demo.as_str())
+        .unwrap_or(0);
+
+    let testbed = Testbed::from_builders(i, builders);
+
+    testbed.run()
+}
diff --git a/examples3d/balls3.rs b/examples3d/balls3.rs
new file mode 100644
index 0000000..df0c21c
--- /dev/null
+++ b/examples3d/balls3.rs
@@ -0,0 +1,29 @@
+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();
+
+    let rb = RigidBodyBuilder::new_dynamic().build();
+    let co = ColliderBuilder::cuboid(0.5, 0.5, 0.5).build();
+    let h = bodies.insert(rb);
+    colliders.insert(co, h, &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/examples3d/boxes3.rs
new file mode 100644
index 0000000..eb8a472
--- /dev/null
+++ b/examples3d/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).density(1.0).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/examples3d/capsules3.rs b/examples3d/capsules3.rs
new file mode 100644
index 0000000..e6aad40
--- /dev/null
+++ b/examples3d/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).density(1.0).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/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs
new file mode 100644
index 0000000..bd7c344
--- /dev/null
+++ b/examples3d/debug_boxes3.rs
@@ -0,0 +1,47 @@
+use na::{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 = 100.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);
+
+    // Build the dynamic box rigid body.
+    let rigid_body = RigidBodyBuilder::new_dynamic()
+        .translation(1.1, 0.0, 0.0)
+        .rotation(Vector3::new(0.8, 0.2, 0.1))
+        .can_sleep(false)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).density(1.0).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/debug_triangle3.rs b/examples3d/debug_triangle3.rs
new file mode 100644
index 0000000..c9d7751
--- /dev/null
+++ b/examples3d/debug_triangle3.rs
@@ -0,0 +1,48 @@
+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();
+
+    // Triangle ground.
+    let vtx = [
+        Point3::new(-10.0, 0.0, -10.0),
+        Point3::new(10.0, 0.0, -10.0),
+        Point3::new(0.0, 0.0, 10.0),
+    ];
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, 0.0, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    // Dynamic box rigid body.
+    let rigid_body = RigidBodyBuilder::new_dynamic()
+        .translation(1.1, 0.01, 0.0)
+        // .rotation(Vector3::new(0.8, 0.2, 0.1))
+        .can_sleep(false)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0).density(1.0).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/domino3.rs b/examples3d/domino3.rs
new file mode 100644
index 0000000..a62ba41
--- /dev/null
+++ b/examples3d/domino3.rs
@@ -0,0 +1,87 @@
+use na::{Point3, Translation3, UnitQuaternion, 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 = 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 = 4000;
+    let width = 1.0;
+    let thickness = 0.1;
+
+    let colors = [Point3::new(0.7, 0.5, 0.9), Point3::new(0.6, 1.0, 0.6)];
+
+    let mut curr_angle = 0.0;
+    let mut curr_rad = 10.0;
+    let mut prev_angle;
+    let mut skip = 0;
+    for i in 0..num {
+        let perimeter = 2.0 * std::f32::consts::PI * curr_rad;
+        let spacing = thickness * 4.0;
+        prev_angle = curr_angle;
+        curr_angle += 2.0 * std::f32::consts::PI * spacing / perimeter;
+        let (x, z) = curr_angle.sin_cos();
+
+        // Build the rigid body.
+        let two_pi = 2.0 * std::f32::consts::PI;
+        let nudged = curr_angle % two_pi < prev_angle % two_pi;
+        let tilt = if nudged || i == num - 1 { 0.2 } else { 0.0 };
+
+        if skip == 0 {
+            let rot = UnitQuaternion::new(Vector3::y() * curr_angle);
+            let tilt = UnitQuaternion::new(rot * Vector3::z() * tilt);
+            let position =
+                Translation3::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad)
+                    * tilt
+                    * rot;
+            let rigid_body = RigidBodyBuilder::new_dynamic().position(position).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width)
+                .density(1.0)
+                .build();
+            colliders.insert(collider, handle, &mut bodies);
+            testbed.set_body_color(handle, colors[i % 2]);
+        } else {
+            skip -= 1;
+        }
+
+        if nudged {
+            skip = 5;
+        }
+
+        curr_rad += 1.5 / perimeter;
+    }
+
+    /*
+     * 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/heightfield3.rs b/examples3d/heightfield3.rs
new file mode 100644
index 0000000..f9d4e49
--- /dev/null
+++ b/examples3d/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).density(1.0).build();
+                    colliders.insert(collider, handle, &mut bodies);
+                } else {
+                    let collider = ColliderBuilder::ball(rad).density(1.0).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/joints3.rs b/examples3d/joints3.rs
new file mode 100644
index 0000000..c640770
--- /dev/null
+++ b/examples3d/joints3.rs
@@ -0,0 +1,272 @@
+use na::{Isometry3, Point3, Unit, Vector3};
+use rapier3d::dynamics::{
+    BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder,
+    RigidBodySet,
+};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+fn create_prismatic_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 1.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(origin.x, origin.y, origin.z)
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        let z = origin.z + (i + 1) as f32 * shift;
+        let density = 1.0;
+        let rigid_body = RigidBodyBuilder::new_dynamic()
+            .translation(origin.x, origin.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, 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(bodies, curr_parent, curr_child, prism);
+
+        curr_parent = curr_child;
+    }
+}
+
+fn create_revolute_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    let shift = 2.0;
+
+    let ground = RigidBodyBuilder::new_static()
+        .translation(origin.x, origin.y, 0.0)
+        .build();
+    let mut curr_parent = bodies.insert(ground);
+    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+    colliders.insert(collider, curr_parent, bodies);
+
+    for i in 0..num {
+        // Create four bodies.
+        let z = origin.z + i as f32 * shift * 2.0 + shift;
+        let positions = [
+            Isometry3::translation(origin.x, origin.y, z),
+            Isometry3::translation(origin.x + shift, origin.y, z),
+            Isometry3::translation(origin.x + shift, origin.y, z + shift),
+            Isometry3::translation(origin.x, origin.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], 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(bodies, curr_parent, handles[0], revs[0]);
+        joints.insert(bodies, handles[0], handles[1], revs[1]);
+        joints.insert(bodies, handles[1], handles[2], revs[2]);
+        joints.insert(bodies, handles[2], handles[3], revs[3]);
+
+        curr_parent = handles[3];
+    }
+}
+
+fn create_fixed_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    origin: Point3<f32>,
+    num: usize,
+) {
+    let rad = 0.4;
+    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;
+
+            // 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(origin.x + fk * shift, origin.y, origin.z + fi * shift)
+                .build();
+            let child_handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::ball(rad).density(1.0).build();
+            colliders.insert(collider, child_handle, 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(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(bodies, parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+}
+
+fn create_ball_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    joints: &mut JointSet,
+    num: usize,
+) {
+    let rad = 0.4;
+    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).density(1.0).build();
+            colliders.insert(collider, child_handle, 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(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(bodies, parent_handle, child_handle, joint);
+            }
+
+            body_handles.push(child_handle);
+        }
+    }
+}
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut joints = JointSet::new();
+
+    create_prismatic_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(20.0, 10.0, 0.0),
+        5,
+    );
+    create_revolute_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(20.0, 0.0, 0.0),
+        3,
+    );
+    create_fixed_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut joints,
+        Point3::new(0.0, 10.0, 0.0),
+        5,
+    );
+    create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(15.0, 5.0, 42.0), Point3::new(13.0, 1.0, 1.0));
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]);
+    testbed.run()
+}
diff --git a/examples3d/kinematic3.rs b/examples3d/kinematic3.rs
new file mode 100644
index 0000000..d6f2014
--- /dev/null
+++ b/examples3d/kinematic3.rs
@@ -0,0 +1,97 @@
+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 = 10.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 boxes
+     */
+    let num = 6;
+    let rad = 0.2;
+
+    let shift = rad * 2.0;
+    let centerx = shift * num as f32 / 2.0;
+    let centery = shift / 2.0 + 3.04;
+    let centerz = shift * num as f32 / 2.0;
+
+    for i in 0usize..num {
+        for j in 0usize..num {
+            for k in 0usize..num {
+                let x = i as f32 * shift - centerx;
+                let y = j as f32 * shift + centery;
+                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);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build();
+                colliders.insert(collider, handle, &mut bodies);
+            }
+        }
+    }
+
+    /*
+     * Setup a kinematic rigid body.
+     */
+    let platform_body = RigidBodyBuilder::new_kinematic()
+        .translation(0.0, 1.5 + 0.8, -10.0 * rad)
+        .build();
+    let platform_handle = bodies.insert(platform_body);
+    let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0)
+        .density(1.0)
+        .build();
+    colliders.insert(collider, platform_handle, &mut bodies);
+
+    /*
+     * Setup a callback to control the platform.
+     */
+    testbed.add_callback(move |bodies, _, _, _, time| {
+        let mut platform = bodies.get_mut(platform_handle).unwrap();
+        let mut next_pos = platform.position;
+
+        let dt = 0.016;
+        next_pos.translation.vector.y += (time * 5.0).sin() * dt;
+        next_pos.translation.vector.z += time.sin() * 5.0 * dt;
+
+        if next_pos.translation.vector.z >= rad * 10.0 {
+            next_pos.translation.vector.z -= dt;
+        }
+        if next_pos.translation.vector.z <= -rad * 10.0 {
+            next_pos.translation.vector.z += dt;
+        }
+
+        platform.set_next_kinemati