Merge branch 'master' into infinite_fall_memory

Fix merge conflict resulting from "axii" spelling correction

.github/workflows/rapier-ci-bench.yml

@@ -18,15 +18,17 @@ jobs:
       BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }}
       BENCHBOT_SHA: ${{ github.sha }}
       BENCHBOT_HEAD_REF: ${{github.head_ref}}
+      BENCHBOT_OTHER_BACKENDS: false
     runs-on: ubuntu-latest
     steps:
       - name: Find commit SHA
         if: github.ref == 'refs/heads/master'
         run: |
           echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA"
+          echo "::set-env name=BENCHBOT_OTHER_BACKENDS::true"
       - name: Send 3D bench message
         shell: bash
         run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS
              -i -H "content-type:application/json" -X POST
              https://$BENCHBOT_AMQP_HOST/api/exchanges/$BENCHBOT_AMQP_VHOST//publish
-            -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\" }","payload_encoding":"string"}'
+            -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\", \"other_backends\":'$BENCHBOT_OTHER_BACKENDS' }","payload_encoding":"string"}'

CHANGELOG

@@ -1,4 +1,21 @@
-## v0.2.0 - WIP
+## v0.3.0
+- Collider shapes are now trait-objects instead of a `Shape` enum.
+- Add a user-defined `u128` to each colliders and rigid-bodies for storing user data.
+- Add the support for `Cylinder`, `RoundCylinder`, and `Cone` shapes.
+- Added the support for collision filtering based on bit masks (often known as collision groups, collision masks, or
+  collision layers in other physics engines). Each collider has two groups. Their `collision_groups` is used for filtering
+  what pair of colliders should have their contacts computed by the narrow-phase. Their `solver_groups` is used for filtering
+  what pair of colliders should have their contact forces computed by the constraints solver.
+- Collision groups can also be used to filter what collider should be hit by a ray-cast performed by the `QueryPipeline`.
+- Added collision filters based on user-defined trait-objects. This adds two traits `ContactPairFilter` and
+  `ProximityPairFilter` that allows user-defined logic for determining if two colliders/sensors are allowed to interact.
+- The `PhysicsPipeline::step` method now takes two additional arguments: the optional `&ContactPairFilter` and `&ProximityPairFilter`
+for filtering contact and proximity pairs.
+
+## v0.2.1
+- Fix panic in TriMesh construction and QueryPipeline update caused by a stack overflow or a subtraction underflow.
+
+## v0.2.0
 The most significant change on this version is the addition of the `QueryPipeline` responsible for performing
 scene-wide queries. So far only ray-casting has been implemented.
 

Cargo.toml

@@ -5,6 +5,11 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark
 [patch.crates-io]
 #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
 #simba = { path = "../simba" }
+#ncollide2d = { path = "../ncollide/build/ncollide2d" }
+#ncollide3d = { path = "../ncollide/build/ncollide3d" }
+#nphysics2d = { path = "../nphysics/build/nphysics2d" }
+#nphysics3d = { path = "../nphysics/build/nphysics3d" }
+#kiss3d = { path = "../kiss3d" }
 
 [profile.release]
 #debug = true

benchmarks2d/Cargo.toml

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.22"
+nalgebra = "0.23"
 
 [dependencies.rapier_testbed2d]
 path = "../build/rapier_testbed2d"

benchmarks3d/Cargo.toml

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.22"
+nalgebra = "0.23"
 
 [dependencies.rapier_testbed3d]
 path = "../build/rapier_testbed3d"

build/rapier2d/Cargo.toml

@@ -1,7 +1,6 @@
-# Name idea: bident for 2D and trident for 3D
 [package]
 name    = "rapier2d"
-version = "0.2.0"
+version = "0.3.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
 description = "2-dimensional physics engine in Rust."
 documentation = "http://docs.rs/rapier2d"
@@ -22,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
 # enabled with the "simd-stable" or "simd-nightly" feature.
 simd-is-enabled = [ ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "indexmap" ]
 
 [lib]
@@ -35,18 +34,22 @@ required-features = [ "dim2" ]
 vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.22"
+nalgebra = "0.23"
-ncollide2d = "0.24"
+ncollide2d = "0.26"
-simba = "^0.2.1"
+simba = "0.3"
-approx = "0.3"
+approx = "0.4"
 rayon = { version = "1", optional = true }
-crossbeam = "0.7"
+crossbeam = "0.8"
 generational-arena = "0.2"
 arrayvec = "0.5"
 bit-vec = "0.6"
 rustc-hash = "1"
 serde = { version = "1", features = [ "derive" ], optional = true }
+erased-serde = { version = "0.3", optional = true }
 indexmap = { version = "1", features = [ "serde-1" ], optional = true }
+downcast-rs = "1.2"
+num-derive = "0.3"
+bitflags = "1"
 
 [dev-dependencies]
 bincode = "1"

build/rapier3d/Cargo.toml

@@ -1,7 +1,6 @@
-# Name idea: bident for 2D and trident for 3D
 [package]
 name    = "rapier3d"
-version = "0.2.0"
+version = "0.3.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
 description = "3-dimensional physics engine in Rust."
 documentation = "http://docs.rs/rapier3d"
@@ -22,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
 # enabled with the "simd-stable" or "simd-nightly" feature.
 simd-is-enabled = [ ]
 wasm-bindgen = [ "instant/wasm-bindgen" ]
-serde-serialize = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
+serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
 enhanced-determinism = [ "simba/libm_force", "indexmap" ]
 
 [lib]
@@ -35,18 +34,22 @@ required-features = [ "dim3" ]
 vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
-nalgebra = "0.22"
+nalgebra = "0.23"
-ncollide3d = "0.24"
+ncollide3d = "0.26"
-simba = "^0.2.1"
+simba = "0.3"
-approx = "0.3"
+approx = "0.4"
 rayon = { version = "1", optional = true }
-crossbeam = "0.7"
+crossbeam = "0.8"
 generational-arena = "0.2"
 arrayvec = "0.5"
 bit-vec = "0.6"
 rustc-hash = "1"
 serde = { version = "1", features = [ "derive" ], optional = true }
+erased-serde = { version = "0.3", optional = true }
 indexmap = { version = "1", features = [ "serde-1" ], optional = true }
+downcast-rs = "1.2"
+num-derive = "0.3"
+bitflags = "1"
 
 [dev-dependencies]
 bincode = "1"

build/rapier_testbed2d/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name    = "rapier_testbed2d"
-version = "0.2.0"
+version = "0.3.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
 description = "Testbed for the 2-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
@@ -23,17 +23,17 @@ other-backends = [ "wrapped2d", "nphysics2d" ]
 
 
 [dependencies]
-nalgebra   = "0.22"
+nalgebra   = "0.23"
-kiss3d     = { version = "0.25", features = [ "conrod" ] }
+kiss3d     = { version = "0.27", features = [ "conrod" ] }
 rand       = "0.7"
 rand_pcg   = "0.2"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 num_cpus   = { version = "1", optional = true }
 wrapped2d  = { version = "0.4", optional = true }
-ncollide2d = "0.24"
+ncollide2d = "0.26"
-nphysics2d = { version = "0.17", optional = true }
+nphysics2d = { version = "0.18", optional = true }
-crossbeam = "0.7"
+crossbeam = "0.8"
 bincode = "1"
 flexbuffers = "0.1"
 Inflector  = "0.11"
@@ -42,5 +42,5 @@ md5 = "0.7"
 
 [dependencies.rapier2d]
 path = "../rapier2d"
-version = "0.2"
+version = "0.3"
 features = [ "serde-serialize" ]

build/rapier_testbed3d/Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name    = "rapier_testbed3d"
-version = "0.2.0"
+version = "0.3.0"
 authors = [ "Sébastien Crozet <developer@crozet.re>" ]
 description = "Testbed for the 3-dimensional physics engine in Rust."
 homepage = "http://rapier.org"
@@ -22,19 +22,19 @@ parallel = [ "rapier3d/parallel", "num_cpus" ]
 other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ]
 
 [dependencies]
-nalgebra   = "0.22"
+nalgebra   = "0.23"
-kiss3d     = { version = "0.25", features = [ "conrod" ] }
+kiss3d     = { version = "0.27", features = [ "conrod" ] }
 rand       = "0.7"
 rand_pcg   = "0.2"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
-glam       = { version = "0.8", optional = true }
+glam       = { version = "0.9", optional = true }
 num_cpus   = { version = "1", optional = true }
-ncollide3d = "0.24"
+ncollide3d = "0.26"
-nphysics3d = { version = "0.17", optional = true }
+nphysics3d = { version = "0.18", optional = true }
-physx      = { version = "0.6", optional = true }
+physx      = { version = "0.7", optional = true }
 physx-sys = { version = "0.4", optional = true }
-crossbeam = "0.7"
+crossbeam = "0.8"
 bincode = "1"
 flexbuffers = "0.1"
 serde_cbor = "0.11"
@@ -44,5 +44,5 @@ serde = { version = "1", features = [ "derive" ] }
 
 [dependencies.rapier3d]
 path = "../rapier3d"
-version = "0.2"
+version = "0.3"
 features = [ "serde-serialize" ]

examples2d/Cargo.toml

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.22"
+nalgebra = "0.23"
 
 [dependencies.rapier_testbed2d]
 path = "../build/rapier_testbed2d"

examples2d/all_examples2.rs

@@ -11,6 +11,7 @@ use rapier_testbed2d::Testbed;
 use std::cmp::Ordering;
 
 mod add_remove2;
+mod collision_groups2;
 mod debug_box_ball2;
 mod debug_infinite_fall;
 mod heightfield2;
@@ -53,6 +54,7 @@ pub fn main() {
 
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
         ("Add remove", add_remove2::init_world),
+        ("Collision groups", collision_groups2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Joints", joints2::init_world),
         ("Platform", platform2::init_world),

examples2d/collision_groups2.rs

@@ -0,0 +1,98 @@
+use na::{Point2, Point3};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet, InteractionGroups};
+use rapier_testbed2d::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 = 5.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height)
+        .build();
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height).build();
+    colliders.insert(collider, floor_handle, &mut bodies);
+
+    /*
+     * Setup groups
+     */
+    const GREEN_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01);
+    const BLUE_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10);
+
+    /*
+     * A green floor that will collide with the GREEN group only.
+     */
+    let green_floor = ColliderBuilder::cuboid(1.0, 0.1)
+        .translation(0.0, 1.0)
+        .collision_groups(GREEN_GROUP)
+        .build();
+    let green_collider_handle = colliders.insert(green_floor, floor_handle, &mut bodies);
+
+    testbed.set_collider_initial_color(green_collider_handle, Point3::new(0.0, 1.0, 0.0));
+
+    /*
+     * A blue floor that will collide with the BLUE group only.
+     */
+    let blue_floor = ColliderBuilder::cuboid(1.0, 0.1)
+        .translation(0.0, 2.0)
+        .collision_groups(BLUE_GROUP)
+        .build();
+    let blue_collider_handle = colliders.insert(blue_floor, floor_handle, &mut bodies);
+
+    testbed.set_collider_initial_color(blue_collider_handle, Point3::new(0.0, 0.0, 1.0));
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 0.1;
+
+    let shift = rad * 2.0;
+    let centerx = shift * (num / 2) as f32;
+    let centery = 2.5;
+
+    for j in 0usize..4 {
+        for i in 0..num {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift + centery;
+
+            // Alternate between the green and blue groups.
+            let (group, color) = if i % 2 == 0 {
+                (GREEN_GROUP, Point3::new(0.0, 1.0, 0.0))
+            } else {
+                (BLUE_GROUP, Point3::new(0.0, 0.0, 1.0))
+            };
+
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad)
+                .collision_groups(group)
+                .build();
+            colliders.insert(collider, handle, &mut bodies);
+
+            testbed.set_body_color(handle, color);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 1.0), 100.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/Cargo.toml

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
 [dependencies]
 rand       = "0.7"
 Inflector  = "0.11"
-nalgebra = "0.22"
+nalgebra = "0.23"
 
 [dependencies.rapier_testbed3d]
 path = "../build/rapier_testbed3d"

examples3d/all_examples3.rs

@@ -11,8 +11,10 @@ use rapier_testbed3d::Testbed;
 use std::cmp::Ordering;
 
 mod add_remove3;
+mod collision_groups3;
 mod compound3;
 mod debug_boxes3;
+mod debug_cylinder3;
 mod debug_triangle3;
 mod debug_trimesh3;
 mod domino3;
@@ -64,6 +66,7 @@ pub fn main() {
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![
         ("Add remove", add_remove3::init_world),
         ("Primitives", primitives3::init_world),
+        ("Collision groups", collision_groups3::init_world),
         ("Compound", compound3::init_world),
         ("Domino", domino3::init_world),
         ("Heightfield", heightfield3::init_world),
@@ -76,6 +79,7 @@ pub fn main() {
         ("(Debug) boxes", debug_boxes3::init_world),
         ("(Debug) triangle", debug_triangle3::init_world),
         ("(Debug) trimesh", debug_trimesh3::init_world),
+        ("(Debug) cylinder", debug_cylinder3::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.

examples3d/collision_groups3.rs

@@ -0,0 +1,102 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet, InteractionGroups};
+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 = 5.0;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let floor_handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, floor_handle, &mut bodies);
+
+    /*
+     * Setup groups
+     */
+    const GREEN_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01);
+    const BLUE_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10);
+
+    /*
+     * A green floor that will collide with the GREEN group only.
+     */
+    let green_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0)
+        .translation(0.0, 1.0, 0.0)
+        .collision_groups(GREEN_GROUP)
+        .build();
+    let green_collider_handle = colliders.insert(green_floor, floor_handle, &mut bodies);
+
+    testbed.set_collider_initial_color(green_collider_handle, Point3::new(0.0, 1.0, 0.0));
+
+    /*
+     * A blue floor that will collide with the BLUE group only.
+     */
+    let blue_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0)
+        .translation(0.0, 2.0, 0.0)
+        .collision_groups(BLUE_GROUP)
+        .build();
+    let blue_collider_handle = colliders.insert(blue_floor, floor_handle, &mut bodies);
+
+    testbed.set_collider_initial_color(blue_collider_handle, Point3::new(0.0, 0.0, 1.0));
+
+    /*
+     * Create the cubes
+     */
+    let num = 8;
+    let rad = 0.1;
+
+    let shift = rad * 2.0;
+    let centerx = shift * (num / 2) as f32;
+    let centery = 2.5;
+    let centerz = shift * (num / 2) as f32;
+
+    for j in 0usize..4 {
+        for i in 0..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;
+
+                // Alternate between the green and blue groups.
+                let (group, color) = if k % 2 == 0 {
+                    (GREEN_GROUP, Point3::new(0.0, 1.0, 0.0))
+                } else {
+                    (BLUE_GROUP, Point3::new(0.0, 0.0, 1.0))
+                };
+
+                let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+                let handle = bodies.insert(rigid_body);
+                let collider = ColliderBuilder::cuboid(rad, rad, rad)
+                    .collision_groups(group)
+                    .build();
+                colliders.insert(collider, handle, &mut bodies);
+
+                testbed.set_body_color(handle, color);
+            }
+        }
+    }
+
+    /*
+     * 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()
+}

examples3d/debug_boxes3.rs

@@ -17,22 +17,26 @@ pub fn init_world(testbed: &mut Testbed) {
     let ground_size = 100.1;
     let ground_height = 0.1;
 
-    let rigid_body = RigidBodyBuilder::new_static()
+    for _ in 0..6 {
-        .translation(0.0, -ground_height, 0.0)
+        let rigid_body = RigidBodyBuilder::new_static()
-        .build();
+            .translation(0.0, -ground_height, 0.0)
-    let handle = bodies.insert(rigid_body);
+            .build();
-    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+        let handle = bodies.insert(rigid_body);
-    colliders.insert(collider, handle, &mut bodies);
+        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()
+    for _ in 0..6 {
-        .translation(1.1, 0.0, 0.0)
+        let rigid_body = RigidBodyBuilder::new_dynamic()
-        .rotation(Vector3::new(0.8, 0.2, 0.1))
+            .translation(1.1, 0.0, 0.0)
-        .can_sleep(false)
+            .rotation(Vector3::new(0.8, 0.2, 0.1))
-        .build();
+            .can_sleep(false)
-    let handle = bodies.insert(rigid_body);
+            .build();
-    let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build();
+        let handle = bodies.insert(rigid_body);
-    colliders.insert(collider, handle, &mut bodies);
+        let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build();
+        colliders.insert(collider, handle, &mut bodies);
+    }
 
     /*
      * Set up the testbed.

examples3d/debug_cylinder3.rs

@@ -0,0 +1,65 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+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 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);
+
+    /*
+     * Create the cubes
+     */
+    let num = 1;
+    let rad = 1.0;
+
+    let shiftx = rad * 2.0 + rad;
+    let shifty = rad * 2.0 + rad;
+    let shiftz = rad * 2.0 + rad;
+    let centerx = shiftx * (num / 2) as f32;
+    let centery = shifty / 2.0;
+    let centerz = shiftz * (num / 2) as f32;
+
+    let offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    let x = -centerx + offset;
+    let y = centery + 3.0;
+    let z = -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::cylinder(rad, 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()
+}

examples3d/heightfield3.rs

@@ -54,13 +54,17 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                if j % 2 == 0 {
+                let collider = match j % 5 {
-                    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                    0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
-                    colliders.insert(collider, handle, &mut bodies);
+                    1 => ColliderBuilder::ball(rad).build(),
-                } else {
+                    // Rounded cylinders are much more efficient that cylinder, even if the
-                    let collider = ColliderBuilder::ball(rad).build();
+                    // rounding margin is small.
-                    colliders.insert(collider, handle, &mut bodies);
+                    2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
-                }
+                    3 => ColliderBuilder::cone(rad, rad).build(),
+                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
+                };
+
+                colliders.insert(collider, handle, &mut bodies);
             }
         }
     }

examples3d/primitives3.rs

@@ -15,7 +15,7 @@ pub fn init_world(testbed: &mut Testbed) {
      * Ground
      */
     let ground_size = 100.1;
-    let ground_height = 0.1;
+    let ground_height = 2.1;
 
     let rigid_body = RigidBodyBuilder::new_static()
         .translation(0.0, -ground_height, 0.0)
@@ -30,29 +30,34 @@ pub fn init_world(testbed: &mut Testbed) {
     let num = 8;
     let rad = 1.0;
 
-    let shift = rad * 2.0 + rad;
+    let shiftx = rad * 2.0 + rad;
-    let centerx = shift * (num / 2) as f32;
+    let shifty = rad * 2.0 + rad;
-    let centery = shift / 2.0;
+    let shiftz = rad * 2.0 + rad;
-    let centerz = shift * (num / 2) as f32;
+    let centerx = shiftx * (num / 2) as f32;
+    let centery = shifty / 2.0;
+    let centerz = shiftz * (num / 2) as f32;
 
     let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
 
     for j in 0usize..20 {
         for i in 0..num {
             for k in 0usize..num {
-                let x = i as f32 * shift - centerx + offset;
+                let x = i as f32 * shiftx - centerx + offset;
-                let y = j as f32 * shift + centery + 3.0;
+                let y = j as f32 * shifty + centery + 3.0;
-                let z = k as f32 * shift - centerz + offset;
+                let z = k as f32 * shiftz - 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 = match j % 2 {
+                let collider = match j % 5 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    // 2 => ColliderBuilder::capsule_y(rad, rad).build(),
+                    // Rounded cylinders are much more efficient that cylinder, even if the
-                    _ => unreachable!(),
+                    // rounding margin is small.
+                    2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
+                    3 => ColliderBuilder::cone(rad, rad).build(),
+                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);

examples3d/trimesh3.rs

@@ -64,13 +64,17 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                if j % 2 == 0 {
+                let collider = match j % 5 {
-                    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                    0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
-                    colliders.insert(collider, handle, &mut bodies);
+                    1 => ColliderBuilder::ball(rad).build(),
-                } else {
+                    // Rounded cylinders are much more efficient that cylinder, even if the
-                    let collider = ColliderBuilder::ball(rad).build();
+                    // rounding margin is small.
-                    colliders.insert(collider, handle, &mut bodies);
+                    2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
-                }
+                    3 => ColliderBuilder::cone(rad, rad).build(),
+                    _ => ColliderBuilder::capsule_y(rad, rad).build(),
+                };
+
+                colliders.insert(collider, handle, &mut bodies);
             }
         }
     }

src/dynamics/mass_properties.rs

@@ -91,7 +91,7 @@ impl MassProperties {
     }
 
     #[cfg(feature = "dim3")]
-    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
+    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes.
     pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
         let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
         self.principal_inertia_local_frame.to_rotation_matrix()
@@ -103,7 +103,7 @@ impl MassProperties {
     }
 
     #[cfg(feature = "dim3")]
-    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
+    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes.
     pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
         let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
         self.principal_inertia_local_frame.to_rotation_matrix()

src/dynamics/mass_properties_capsule.rs

@@ -1,30 +1,9 @@
 use crate::dynamics::MassProperties;
 #[cfg(feature = "dim3")]
 use crate::geometry::Capsule;
-use crate::math::{Point, PrincipalAngularInertia, Vector};
+use crate::math::Point;
 
 impl MassProperties {
-    fn cylinder_y_volume_unit_inertia(
-        half_height: f32,
-        radius: f32,
-    ) -> (f32, PrincipalAngularInertia<f32>) {
-        #[cfg(feature = "dim2")]
-        {
-            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
-            let sq_radius = radius * radius;
-            let sq_height = half_height * half_height * 4.0;
-            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
-
-            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
-            (volume, inertia)
-        }
-    }
-
     pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
         let half_height = (b - a).norm() / 2.0;
         let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);

src/dynamics/mass_properties_cone.rs

@@ -0,0 +1,29 @@
+use crate::dynamics::MassProperties;
+use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
+
+impl MassProperties {
+    pub(crate) fn cone_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0;
+        let sq_radius = radius * radius;
+        let sq_height = half_height * half_height * 4.0;
+        let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0;
+        let principal = sq_radius * 3.0 / 10.0;
+
+        (volume, Vector::new(off_principal, principal, off_principal))
+    }
+
+    pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self {
+        let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius);
+        let cyl_mass = cyl_vol * density;
+
+        Self::with_principal_inertia_frame(
+            Point::new(0.0, -half_height / 2.0, 0.0),
+            cyl_mass,
+            cyl_unit_i * cyl_mass,
+            Rotation::identity(),
+        )
+    }
+}

src/dynamics/mass_properties_cylinder.rs

@@ -0,0 +1,40 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::math::{Point, Rotation};
+use crate::math::{PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+    pub(crate) fn cylinder_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
+            let sq_radius = radius * radius;
+            let sq_height = half_height * half_height * 4.0;
+            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
+
+            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
+            (volume, inertia)
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self {
+        let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
+        let cyl_mass = cyl_vol * density;
+
+        Self::with_principal_inertia_frame(
+            Point::origin(),
+            cyl_mass,
+            cyl_unit_i * cyl_mass,
+            Rotation::identity(),
+        )
+    }
+}

src/dynamics/mass_properties_polygon.rs

@@ -1,3 +1,5 @@
+#![allow(dead_code)] // TODO: remove this
+
 use crate::dynamics::MassProperties;
 use crate::math::Point;
 

src/dynamics/mod.rs

@@ -22,7 +22,10 @@ mod joint;
 mod mass_properties;
 mod mass_properties_ball;
 mod mass_properties_capsule;
+#[cfg(feature = "dim3")]
+mod mass_properties_cone;
 mod mass_properties_cuboid;
+mod mass_properties_cylinder;
 #[cfg(feature = "dim2")]
 mod mass_properties_polygon;
 mod rigid_body;

src/dynamics/rigid_body.rs

@@ -54,6 +54,8 @@ pub struct RigidBody {
     pub(crate) active_set_timestamp: u32,
     /// The status of the body, governing how it is affected by external forces.
     pub body_status: BodyStatus,
+    /// User-defined data associated to this rigid-body.
+    pub user_data: u128,
 }
 
 impl Clone for RigidBody {
@@ -90,6 +92,7 @@ impl RigidBody {
             active_set_offset: 0,
             active_set_timestamp: 0,
             body_status: BodyStatus::Dynamic,
+            user_data: 0,
         }
     }
 
@@ -218,6 +221,7 @@ impl RigidBody {
         let shift = Translation::from(com.coords);
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }
+
     pub(crate) fn integrate(&mut self, dt: f32) {
         self.position = self.integrate_velocity(dt) * self.position;
     }
@@ -334,13 +338,14 @@ impl RigidBody {
     }
 }
 
-/// A builder for rigid-bodies.
+/// A builder for rigid-bodies.
 pub struct RigidBodyBuilder {
     position: Isometry<f32>,
     linvel: Vector<f32>,
     angvel: AngVector<f32>,
     body_status: BodyStatus,
     can_sleep: bool,
+    user_data: u128,
 }
 
 impl RigidBodyBuilder {
@@ -352,6 +357,7 @@ impl RigidBodyBuilder {
             angvel: na::zero(),
             body_status,
             can_sleep: true,
+            user_data: 0,
         }
     }
 
@@ -399,6 +405,12 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
+    pub fn user_data(mut self, data: u128) -> Self {
+        self.user_data = data;
+        self
+    }
+
     /// Sets the initial linear velocity of the rigid-body to be created.
     #[cfg(feature = "dim2")]
     pub fn linvel(mut self, x: f32, y: f32) -> Self {
@@ -433,6 +445,7 @@ impl RigidBodyBuilder {
         rb.linvel = self.linvel;
         rb.angvel = self.angvel;
         rb.body_status = self.body_status;
+        rb.user_data = self.user_data;
 
         if !self.can_sleep {
             rb.activation.threshold = -1.0;

src/geometry/broad_phase_multi_sap.rs

@@ -337,7 +337,7 @@ impl SAPAxis {
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 struct SAPRegion {
-    axii: [SAPAxis; DIM],
+    axes: [SAPAxis; DIM],
     existing_proxies: BitVec,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     to_insert: Vec<usize>, // Workspace
@@ -347,14 +347,14 @@ struct SAPRegion {
 
 impl SAPRegion {
     pub fn new(bounds: AABB<f32>) -> Self {
-        let axii = [
+        let axes = [
             SAPAxis::new(bounds.mins.x, bounds.maxs.x),
             SAPAxis::new(bounds.mins.y, bounds.maxs.y),
             #[cfg(feature = "dim3")]
             SAPAxis::new(bounds.mins.z, bounds.maxs.z),
         ];
         SAPRegion {
-            axii,
+            axes,
             existing_proxies: BitVec::new(),
             to_insert: Vec::new(),
             update_count: 0,
@@ -394,14 +394,14 @@ impl SAPRegion {
             // Update endpoints.
             let mut deleted = 0;
             for dim in 0..DIM {
-                self.axii[dim].update_endpoints(dim, proxies, reporting);
+                self.axes[dim].update_endpoints(dim, proxies, reporting);
-                deleted += self.axii[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies);
+                deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies);
             }
 
             if deleted > 0 {
                 self.proxy_count -= deleted;
                 for dim in 0..DIM {
-                    self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
+                    self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
                 }
             }
 
@@ -411,9 +411,9 @@ impl SAPRegion {
         if !self.to_insert.is_empty() {
             // Insert new proxies.
             for dim in 1..DIM {
-                self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None);
+                self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None);
             }
-            self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
+            self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
             self.to_insert.clear();
 
             // In the rare event that all proxies leave this region in the next step, we need an

src/geometry/capsule.rs

@@ -1,18 +1,16 @@
-use crate::geometry::AABB;
+use crate::geometry::{Ray, RayIntersection, AABB};
 use crate::math::{Isometry, Point, Rotation, Vector};
 use approx::AbsDiffEq;
 use na::Unit;
-use ncollide::query::{PointProjection, PointQuery};
+use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast};
-use ncollide::shape::{FeatureId, Segment};
+use ncollide::shape::{FeatureId, Segment, SupportMap};
 
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A capsule shape defined as a segment with a radius.
+/// A capsule shape defined as a round segment.
 pub struct Capsule {
-    /// The first endpoint of the capsule.
+    /// The axis and endpoint of the capsule.
-    pub a: Point<f32>,
+    pub segment: Segment<f32>,
-    /// The second enpdoint of the capsule.
-    pub b: Point<f32>,
     /// The radius of the capsule.
     pub radius: f32,
 }
@@ -39,13 +37,14 @@ impl Capsule {
 
     /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`.
     pub fn new(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
-        Self { a, b, radius }
+        let segment = Segment::new(a, b);
+        Self { segment, radius }
     }
 
     /// The axis-aligned bounding box of this capsule.
     pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
-        let a = pos * self.a;
+        let a = pos * self.segment.a;
-        let b = pos * self.b;
+        let b = pos * self.segment.b;
         let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius);
         let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius);
         AABB::new(mins.into(), maxs.into())
@@ -53,7 +52,7 @@ impl Capsule {
 
     /// The height of this capsule.
     pub fn height(&self) -> f32 {
-        (self.b - self.a).norm()
+        (self.segment.b - self.segment.a).norm()
     }
 
     /// The half-height of this capsule.
@@ -63,17 +62,17 @@ impl Capsule {
 
     /// The center of this capsule.
     pub fn center(&self) -> Point<f32> {
-        na::center(&self.a, &self.b)
+        na::center(&self.segment.a, &self.segment.b)
     }
 
     /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`.
     pub fn transform_by(&self, pos: &Isometry<f32>) -> Self {
-        Self::new(pos * self.a, pos * self.b, self.radius)
+        Self::new(pos * self.segment.a, pos * self.segment.b, self.radius)
     }
 
     /// The rotation `r` such that `r * Y` is collinear with `b - a`.
     pub fn rotation_wrt_y(&self) -> Rotation<f32> {
-        let mut dir = self.b - self.a;
+        let mut dir = self.segment.b - self.segment.a;
         if dir.y < 0.0 {
             dir = -dir;
         }
@@ -96,24 +95,49 @@ impl Capsule {
     }
 }
 
-// impl SupportMap<f32> for Capsule {
+impl SupportMap<f32> for Capsule {
-//     fn local_support_point(&self, dir: &Vector) -> Point {
+    fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
-//         let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis());
+        let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis());
-//         self.local_support_point_toward(&dir)
+        self.local_support_point_toward(&dir)
-//     }
+    }
-//
+
-//     fn local_support_point_toward(&self, dir: &Unit<Vector>) -> Point {
+    fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
-//         if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) {
+        if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) {
-//             self.a + **dir * self.radius
+            self.segment.a + **dir * self.radius
-//         } else {
+        } else {
-//             self.b + **dir * self.radius
+            self.segment.b + **dir * self.radius
-//         }
+        }
-//     }
+    }
-// }
+}
+
+impl RayCast<f32> for Capsule {
+    fn toi_and_normal_with_ray(
+        &self,
+        m: &Isometry<f32>,
+        ray: &Ray,
+        max_toi: f32,
+        solid: bool,
+    ) -> Option<RayIntersection> {
+        let ls_ray = ray.inverse_transform_by(m);
+
+        ncollide::query::ray_intersection_with_support_map_with_params(
+            &Isometry::identity(),
+            self,
+            &mut VoronoiSimplex::new(),
+            &ls_ray,
+            max_toi,
+            solid,
+        )
+        .map(|mut res| {
+            res.normal = m * res.normal;
+            res
+        })
+    }
+}
 
 // TODO: this code has been extracted from ncollide and added here
 // so we can modify it to fit with our new definition of capsule.
-// Wa should find a way to avoid this code duplication.
+// We should find a way to avoid this code duplication.
 impl PointQuery<f32> for Capsule {
     #[inline]
     fn project_point(
@@ -122,7 +146,7 @@ impl PointQuery<f32> for Capsule {
         pt: &Point<f32>,
         solid: bool,
     ) -> PointProjection<f32> {
-        let seg = Segment::new(self.a, self.b);
+        let seg = Segment::new(self.segment.a, self.segment.b);
         let proj = seg.project_point(m, pt, solid);
         let dproj = *pt - proj.point;
 

src/geometry/collider.rs

@@ -1,145 +1,189 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
+    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph,
-    Proximity, Ray, RayIntersection, Triangle, Trimesh,
+    InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh,
 };
+#[cfg(feature = "dim3")]
+use crate::geometry::{Cone, Cylinder, RoundCylinder};
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use na::Point3;
-use ncollide::bounding_volume::{HasBoundingVolume, AABB};
+use ncollide::bounding_volume::AABB;
-use ncollide::query::RayCast;
+use std::ops::Deref;
-use num::Zero;
+use std::sync::Arc;
 
+/// The shape of a collider.
 #[derive(Clone)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderShape(pub Arc<dyn Shape>);
-/// An enum grouping all the possible shape of a collider.
+
-pub enum Shape {
+impl Deref for ColliderShape {
-    /// A ball shape.
+    type Target = dyn Shape;
-    Ball(Ball),
+    fn deref(&self) -> &dyn Shape {
-    /// A convex polygon shape.
+        &*self.0
-    Polygon(Polygon),
+    }
-    /// A cuboid shape.
-    Cuboid(Cuboid),
-    /// A capsule shape.
-    Capsule(Capsule),
-    /// A triangle shape.
-    Triangle(Triangle),
-    /// A triangle mesh shape.
-    Trimesh(Trimesh),
-    /// A heightfield shape.
-    HeightField(HeightField),
 }
 
-impl Shape {
+impl ColliderShape {
-    /// Gets a reference to the underlying ball shape, if `self` is one.
+    /// Initialize a ball shape defined by its radius.
-    pub fn as_ball(&self) -> Option<&Ball> {
+    pub fn ball(radius: f32) -> Self {
-        match self {
+        ColliderShape(Arc::new(Ball::new(radius)))
-            Shape::Ball(b) => Some(b),
-            _ => None,
-        }
     }
 
-    /// Gets a reference to the underlying polygon shape, if `self` is one.
+    /// Initialize a cylindrical shape defined by its half-height
-    pub fn as_polygon(&self) -> Option<&Polygon> {
+    /// (along along the y axis) and its radius.
-        match self {
+    #[cfg(feature = "dim3")]
-            Shape::Polygon(p) => Some(p),
+    pub fn cylinder(half_height: f32, radius: f32) -> Self {
-            _ => None,
+        ColliderShape(Arc::new(Cylinder::new(half_height, radius)))
-        }
     }
 
-    /// Gets a reference to the underlying cuboid shape, if `self` is one.
+    /// Initialize a rounded cylindrical shape defined by its half-height
-    pub fn as_cuboid(&self) -> Option<&Cuboid> {
+    /// (along along the y axis), its radius, and its roundedness (the
-        match self {
+    /// radius of the sphere used for dilating the cylinder).
-            Shape::Cuboid(c) => Some(c),
+    #[cfg(feature = "dim3")]
-            _ => None,
+    pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self {
-        }
+        ColliderShape(Arc::new(RoundCylinder::new(
+            half_height,
+            radius,
+            border_radius,
+        )))
     }
 
-    /// Gets a reference to the underlying capsule shape, if `self` is one.
+    /// Initialize a cone shape defined by its half-height
-    pub fn as_capsule(&self) -> Option<&Capsule> {
+    /// (along along the y axis) and its basis radius.
-        match self {
+    #[cfg(feature = "dim3")]
-            Shape::Capsule(c) => Some(c),
+    pub fn cone(half_height: f32, radius: f32) -> Self {
-            _ => None,
+        ColliderShape(Arc::new(Cone::new(half_height, radius)))
-        }
     }
 
-    /// Gets a reference to the underlying triangle mesh shape, if `self` is one.
+    /// Initialize a cuboid shape defined by its half-extents.
-    pub fn as_trimesh(&self) -> Option<&Trimesh> {
+    pub fn cuboid(half_extents: Vector<f32>) -> Self {
-        match self {
+        ColliderShape(Arc::new(Cuboid::new(half_extents)))
-            Shape::Trimesh(c) => Some(c),
-            _ => None,
-        }
     }
 
-    /// Gets a reference to the underlying heightfield shape, if `self` is one.
+    /// Initialize a capsule shape from its endpoints and radius.
-    pub fn as_heightfield(&self) -> Option<&HeightField> {
+    pub fn capsule(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
-        match self {
+        ColliderShape(Arc::new(Capsule::new(a, b, radius)))
-            Shape::HeightField(h) => Some(h),
-            _ => None,
-        }
     }
 
-    /// Gets a reference to the underlying triangle shape, if `self` is one.
+    /// Initialize a segment shape from its endpoints.
-    pub fn as_triangle(&self) -> Option<&Triangle> {
+    pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
-        match self {
+        ColliderShape(Arc::new(Segment::new(a, b)))
-            Shape::Triangle(c) => Some(c),
-            _ => None,
-        }
     }
 
-    /// Computes the axis-aligned bounding box of this shape.
+    /// Initializes a triangle shape.
-    pub fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+    pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self {
-        match self {
+        ColliderShape(Arc::new(Triangle::new(a, b, c)))
-            Shape::Ball(ball) => ball.bounding_volume(position),
-            Shape::Polygon(poly) => poly.aabb(position),
-            Shape::Capsule(caps) => caps.aabb(position),
-            Shape::Cuboid(cuboid) => cuboid.bounding_volume(position),
-            Shape::Triangle(triangle) => triangle.bounding_volume(position),
-            Shape::Trimesh(trimesh) => trimesh.aabb(position),
-            Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
-        }
     }
 
-    /// Computes the first intersection point between a ray in this collider.
+    /// Initializes a triangle mesh shape defined by its vertex and index buffers.
-    ///
+    pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
-    /// Some shapes are not supported yet and will always return `None`.
+        ColliderShape(Arc::new(Trimesh::new(vertices, indices)))
-    ///
+    }
-    /// # Parameters
+
-    /// - `position`: the position of this shape.
+    /// Initializes an heightfield shape defined by its set of height and a scale
-    /// - `ray`: the ray to cast.
+    /// factor along each coordinate axis.
-    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
+    #[cfg(feature = "dim2")]
-    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
+    pub fn heightfield(heights: na::DVector<f32>, scale: Vector<f32>) -> Self {
-    pub fn cast_ray(
+        ColliderShape(Arc::new(HeightField::new(heights, scale)))
-        &self,
+    }
-        position: &Isometry<f32>,
+
-        ray: &Ray,
+    /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale
-        max_toi: f32,
+    /// factor along each coordinate axis.
-    ) -> Option<RayIntersection> {
+    #[cfg(feature = "dim3")]
-        match self {
+    pub fn heightfield(heights: na::DMatrix<f32>, scale: Vector<f32>) -> Self {
-            Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true),
+        ColliderShape(Arc::new(HeightField::new(heights, scale)))
-            Shape::Polygon(_poly) => None,
+    }
-            Shape::Capsule(caps) => {
+}
-                let pos = position * caps.transform_wrt_y();
+
-                let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius);
+#[cfg(feature = "serde-serialize")]
-                caps.toi_and_normal_with_ray(&pos, ray, max_toi, true)
+impl serde::Serialize for ColliderShape {
+    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
+    where
+        S: serde::Serializer,
+    {
+        use crate::serde::ser::SerializeStruct;
+
+        if let Some(ser) = self.0.as_serialize() {
+            let typ = self.0.shape_type();
+            let mut state = serializer.serialize_struct("ColliderShape", 2)?;
+            state.serialize_field("tag", &(typ as i32))?;
+            state.serialize_field("inner", ser)?;
+            state.end()
+        } else {
+            Err(serde::ser::Error::custom(
+                "Found a non-serializable custom shape.",
+            ))
+        }
+    }
+}
+
+#[cfg(feature = "serde-serialize")]
+impl<'de> serde::Deserialize<'de> for ColliderShape {
+    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
+    where
+        D: serde::Deserializer<'de>,
+    {
+        struct Visitor {};
+        impl<'de> serde::de::Visitor<'de> for Visitor {
+            type Value = ColliderShape;
+            fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
+                write!(formatter, "one shape type tag and the inner shape data")
             }
-            Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true),
+
-            #[cfg(feature = "dim2")]
+            fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
-            Shape::Triangle(_) | Shape::Trimesh(_) => {
+            where
-                // This is not implemented yet in 2D.
+                A: serde::de::SeqAccess<'de>,
-                None
+            {
-            }
+                use num::cast::FromPrimitive;
-            #[cfg(feature = "dim3")]
+
-            Shape::Triangle(triangle) => {
+                let tag: i32 = seq
-                triangle.toi_and_normal_with_ray(position, ray, max_toi, true)
+                    .next_element()?
-            }
+                    .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
-            #[cfg(feature = "dim3")]
+
-            Shape::Trimesh(trimesh) => {
+                fn deser<'de, A, S: Shape + serde::Deserialize<'de>>(
-                trimesh.toi_and_normal_with_ray(position, ray, max_toi, true)
+                    seq: &mut A,
-            }
+                ) -> Result<Arc<dyn Shape>, A::Error>
-            Shape::HeightField(heightfield) => {
+                where
-                heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
+                    A: serde::de::SeqAccess<'de>,
+                {
+                    let shape: S = seq.next_element()?.ok_or_else(|| {
+                        serde::de::Error::custom("Failed to deserialize builtin shape.")
+                    })?;
+                    Ok(Arc::new(shape) as Arc<dyn Shape>)
+                }
+
+                let shape = match ShapeType::from_i32(tag) {
+                    Some(ShapeType::Ball) => deser::<A, Ball>(&mut seq)?,
+                    Some(ShapeType::Polygon) => {
+                        unimplemented!()
+                        // let shape: Polygon = seq
+                        //     .next_element()?
+                        //     .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
+                        // Arc::new(shape) as Arc<dyn Shape>
+                    }
+                    Some(ShapeType::Cuboid) => deser::<A, Cuboid>(&mut seq)?,
+                    Some(ShapeType::Capsule) => deser::<A, Capsule>(&mut seq)?,
+                    Some(ShapeType::Triangle) => deser::<A, Triangle>(&mut seq)?,
+                    Some(ShapeType::Segment) => deser::<A, Segment>(&mut seq)?,
+                    Some(ShapeType::Trimesh) => deser::<A, Trimesh>(&mut seq)?,
+                    Some(ShapeType::HeightField) => deser::<A, HeightField>(&mut seq)?,
+                    #[cfg(feature = "dim3")]
+                    Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
+                    #[cfg(feature = "dim3")]
+                    Some(ShapeType::Cone) => deser::<A, Cone>(&mut seq)?,
+                    #[cfg(feature = "dim3")]
+                    Some(ShapeType::RoundCylinder) => deser::<A, RoundCylinder>(&mut seq)?,
+                    None => {
+                        return Err(serde::de::Error::custom(
+                            "found invalid shape type to deserialize",
+                        ))
+                    }
+                };
+
+                Ok(ColliderShape(shape))
             }
         }
+
+        deserializer.deserialize_struct("ColliderShape", &["tag", "inner"], Visitor {})
     }
 }
 
@@ -148,7 +192,7 @@ impl Shape {
 ///
 /// To build a new collider, use the `ColliderBuilder` structure.
 pub struct Collider {
-    shape: Shape,
+    shape: ColliderShape,
     density: f32,
     is_sensor: bool,
     pub(crate) parent: RigidBodyHandle,
@@ -159,9 +203,13 @@ pub struct Collider {
     pub friction: f32,
     /// The restitution coefficient of this collider.
     pub restitution: f32,
+    pub(crate) collision_groups: InteractionGroups,
+    pub(crate) solver_groups: InteractionGroups,
     pub(crate) contact_graph_index: ColliderGraphIndex,
     pub(crate) proximity_graph_index: ColliderGraphIndex,
     pub(crate) proxy_index: usize,
+    /// User-defined data associated to this rigid-body.
+    pub user_data: u128,
 }
 
 impl Clone for Collider {
@@ -209,14 +257,24 @@ impl Collider {
         &self.delta
     }
 
+    /// The collision groups used by this collider.
+    pub fn collision_groups(&self) -> InteractionGroups {
+        self.collision_groups
+    }
+
+    /// The solver groups used by this collider.
+    pub fn solver_groups(&self) -> InteractionGroups {
+        self.solver_groups
+    }
+
     /// The density of this collider.
     pub fn density(&self) -> f32 {
         self.density
     }
 
     /// The geometric shape of this collider.
-    pub fn shape(&self) -> &Shape {
+    pub fn shape(&self) -> &dyn Shape {
-        &self.shape
+        &*self.shape.0
     }
 
     /// Compute the axis-aligned bounding box of this collider.
@@ -232,20 +290,7 @@ impl Collider {
 
     /// Compute the local-space mass properties of this collider.
     pub fn mass_properties(&self) -> MassProperties {
-        match &self.shape {
+        self.shape.mass_properties(self.density)
-            Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius),
-            #[cfg(feature = "dim2")]
-            Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()),
-            #[cfg(feature = "dim3")]
-            Shape::Polygon(_p) => unimplemented!(),
-            Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents),
-            Shape::Capsule(caps) => {
-                MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius)
-            }
-            Shape::Triangle(_) => MassProperties::zero(),
-            Shape::Trimesh(_) => MassProperties::zero(),
-            Shape::HeightField(_) => MassProperties::zero(),
-        }
     }
 }
 
@@ -254,7 +299,7 @@ impl Collider {
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct ColliderBuilder {
     /// The shape of the collider to be built.
-    pub shape: Shape,
+    pub shape: ColliderShape,
     /// The density of the collider to be built.
     density: Option<f32>,
     /// The friction coefficient of the collider to be built.
@@ -265,11 +310,17 @@ pub struct ColliderBuilder {
     pub delta: Isometry<f32>,
     /// Is this collider a sensor?
     pub is_sensor: bool,
+    /// The user-data of the collider being built.
+    pub user_data: u128,
+    /// The collision groups for the collider being built.
+    pub collision_groups: InteractionGroups,
+    /// The solver groups for the collider being built.
+    pub solver_groups: InteractionGroups,
 }
 
 impl ColliderBuilder {
     /// Initialize a new collider builder with the given shape.
-    pub fn new(shape: Shape) -> Self {
+    pub fn new(shape: ColliderShape) -> Self {
         Self {
             shape,
             density: None,
@@ -277,6 +328,9 @@ impl ColliderBuilder {
             restitution: 0.0,
             delta: Isometry::identity(),
             is_sensor: false,
+            user_data: 0,
+            collision_groups: InteractionGroups::all(),
+            solver_groups: InteractionGroups::all(),
         }
     }
 
@@ -288,96 +342,93 @@ impl ColliderBuilder {
 
     /// Initialize a new collider builder with a ball shape defined by its radius.
     pub fn ball(radius: f32) -> Self {
-        Self::new(Shape::Ball(Ball::new(radius)))
+        Self::new(ColliderShape::ball(radius))
+    }
+
+    /// Initialize a new collider builder with a cylindrical shape defined by its half-height
+    /// (along along the y axis) and its radius.
+    #[cfg(feature = "dim3")]
+    pub fn cylinder(half_height: f32, radius: f32) -> Self {
+        Self::new(ColliderShape::cylinder(half_height, radius))
+    }
+
+    /// Initialize a new collider builder with a rounded cylindrical shape defined by its half-height
+    /// (along along the y axis), its radius, and its roundedness (the
+    /// radius of the sphere used for dilating the cylinder).
+    #[cfg(feature = "dim3")]
+    pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self {
+        Self::new(ColliderShape::round_cylinder(
+            half_height,
+            radius,
+            border_radius,
+        ))
+    }
+
+    /// Initialize a new collider builder with a cone shape defined by its half-height
+    /// (along along the y axis) and its basis radius.
+    #[cfg(feature = "dim3")]
+    pub fn cone(half_height: f32, radius: f32) -> Self {
+        Self::new(ColliderShape::cone(half_height, radius))
     }
 
     /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
     #[cfg(feature = "dim2")]
     pub fn cuboid(hx: f32, hy: f32) -> Self {
-        let cuboid = Cuboid {
+        Self::new(ColliderShape::cuboid(Vector::new(hx, hy)))
-            half_extents: Vector::new(hx, hy),
-        };
-
-        Self::new(Shape::Cuboid(cuboid))
-
-        /*
-        use crate::math::Point;
-        let vertices = vec![
-            Point::new(hx, -hy),
-            Point::new(hx, hy),
-            Point::new(-hx, hy),
-            Point::new(-hx, -hy),
-        ];
-        let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()];
-        let polygon = Polygon::new(vertices, normals);
-
-        Self::new(Shape::Polygon(polygon))
-        */
     }
 
     /// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
     pub fn capsule_x(half_height: f32, radius: f32) -> Self {
-        let capsule = Capsule::new_x(half_height, radius);
+        let p = Point::from(Vector::x() * half_height);
-        Self::new(Shape::Capsule(capsule))
+        Self::new(ColliderShape::capsule(-p, p, radius))
     }
 
     /// Initialize a new collider builder with a capsule shape aligned with the `y` axis.
     pub fn capsule_y(half_height: f32, radius: f32) -> Self {
-        let capsule = Capsule::new_y(half_height, radius);
+        let p = Point::from(Vector::y() * half_height);
-        Self::new(Shape::Capsule(capsule))
+        Self::new(ColliderShape::capsule(-p, p, radius))
     }
 
     /// Initialize a new collider builder with a capsule shape aligned with the `z` axis.
     #[cfg(feature = "dim3")]
     pub fn capsule_z(half_height: f32, radius: f32) -> Self {
-        let capsule = Capsule::new_z(half_height, radius);
+        let p = Point::from(Vector::z() * half_height);
-        Self::new(Shape::Capsule(capsule))
+        Self::new(ColliderShape::capsule(-p, p, radius))
     }
 
     /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
     #[cfg(feature = "dim3")]
     pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self {
-        let cuboid = Cuboid {
+        Self::new(ColliderShape::cuboid(Vector::new(hx, hy, hz)))
-            half_extents: Vector::new(hx, hy, hz),
-        };
-
-        Self::new(Shape::Cuboid(cuboid))
     }
 
     /// Initializes a collider builder with a segment shape.
-    ///
-    /// A segment shape is modeled by a capsule with a 0 radius.
     pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
-        let capsule = Capsule::new(a, b, 0.0);
+        Self::new(ColliderShape::segment(a, b))
-        Self::new(Shape::Capsule(capsule))
     }
 
     /// Initializes a collider builder with a triangle shape.
     pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self {
-        let triangle = Triangle::new(a, b, c);
+        Self::new(ColliderShape::triangle(a, b, c))
-        Self::new(Shape::Triangle(triangle))
     }
 
     /// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers.
     pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
-        let trimesh = Trimesh::new(vertices, indices);
+        Self::new(ColliderShape::trimesh(vertices, indices))
-        Self::new(Shape::Trimesh(trimesh))
     }
 
     /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
     /// factor along each coordinate axis.
     #[cfg(feature = "dim2")]
     pub fn heightfield(heights: na::DVector<f32>, scale: Vector<f32>) -> Self {
-        let heightfield = HeightField::new(heights, scale);
+        Self::new(ColliderShape::heightfield(heights, scale))
-        Self::new(Shape::HeightField(heightfield))
     }
 
     /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
     /// factor along each coordinate axis.
     #[cfg(feature = "dim3")]
     pub fn heightfield(heights: na::DMatrix<f32>, scale: Vector<f32>) -> Self {
-        let heightfield = HeightField::new(heights, scale);
+        Self::new(ColliderShape::heightfield(heights, scale))
-        Self::new(Shape::HeightField(heightfield))
     }
 
     /// The default friction coefficient used by the collider builder.
@@ -385,6 +436,30 @@ impl ColliderBuilder {
         0.5
     }
 
+    /// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder.
+    pub fn user_data(mut self, data: u128) -> Self {
+        self.user_data = data;
+        self
+    }
+
+    /// Sets the collision groups used by this collider.
+    ///
+    /// Two colliders will interact iff. their collision groups are compatible.
+    /// See [InteractionGroups::test] for details.
+    pub fn collision_groups(mut self, groups: InteractionGroups) -> Self {
+        self.collision_groups = groups;
+        self
+    }
+
+    /// Sets the solver groups used by this collider.
+    ///
+    /// Forces between two colliders in contact will be computed iff their solver groups are
+    /// compatible. See [InteractionGroups::test] for details.
+    pub fn solver_groups(mut self, groups: InteractionGroups) -> Self {
+        self.solver_groups = groups;
+        self
+    }
+
     /// Sets whether or not the collider built by this builder is a sensor.
     pub fn sensor(mut self, is_sensor: bool) -> Self {
         self.is_sensor = is_sensor;
@@ -449,7 +524,7 @@ impl ColliderBuilder {
         self
     }
 
-    /// Buildes a new collider attached to the given rigid-body.
+    /// Builds a new collider attached to the given rigid-body.
     pub fn build(&self) -> Collider {
         let density = self.get_density();
 
@@ -466,6 +541,9 @@ impl ColliderBuilder {
             contact_graph_index: InteractionGraph::<Contact>::invalid_graph_index(),
             proximity_graph_index: InteractionGraph::<Proximity>::invalid_graph_index(),
             proxy_index: crate::INVALID_USIZE,
+            collision_groups: self.collision_groups,
+            solver_groups: self.solver_groups,
+            user_data: self.user_data,
         }
     }
 }

src/geometry/contact.rs

@@ -9,6 +9,16 @@ use {
     simba::simd::SimdValue,
 };
 
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
+    pub struct SolverFlags: u32 {
+        /// The constraint solver will take this contact manifold into
+        /// account for force computation.
+        const COMPUTE_IMPULSES = 0b01;
+    }
+}
+
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// The type local linear approximation of the neighborhood of a pair contact points on two shapes
@@ -206,6 +216,7 @@ impl ContactPair {
     pub(crate) fn single_manifold<'a, 'b>(
         &'a mut self,
         colliders: &'b ColliderSet,
+        flags: SolverFlags,
     ) -> (
         &'b Collider,
         &'b Collider,
@@ -216,7 +227,7 @@ impl ContactPair {
         let coll2 = &colliders[self.pair.collider2];
 
         if self.manifolds.len() == 0 {
-            let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2);
+            let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags);
             self.manifolds.push(manifold);
         }
 
@@ -288,6 +299,8 @@ pub struct ContactManifold {
     /// The relative position between the second collider and its parent at the time the
     /// contact points were generated.
     pub delta2: Isometry<f32>,
+    /// Flags used to control some aspects of the constraints solver for this contact manifold.
+    pub solver_flags: SolverFlags,
 }
 
 impl ContactManifold {
@@ -299,6 +312,7 @@ impl ContactManifold {
         delta2: Isometry<f32>,
         friction: f32,
         restitution: f32,
+        solver_flags: SolverFlags,
     ) -> ContactManifold {
         Self {
             #[cfg(feature = "dim2")]
@@ -319,6 +333,7 @@ impl ContactManifold {
             delta2,
             constraint_index: 0,
             position_constraint_index: 0,
+            solver_flags,
         }
     }
 
@@ -342,11 +357,17 @@ impl ContactManifold {
             delta2: self.delta2,
             constraint_index: self.constraint_index,
             position_constraint_index: self.position_constraint_index,
+            solver_flags: self.solver_flags,
         }
     }
 
-    pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self {
+    pub(crate) fn from_colliders(
-        Self::with_subshape_indices(pair, coll1, coll2, 0, 0)
+        pair: ColliderPair,
+        coll1: &Collider,
+        coll2: &Collider,
+        flags: SolverFlags,
+    ) -> Self {
+        Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags)
     }
 
     pub(crate) fn with_subshape_indices(
@@ -355,6 +376,7 @@ impl ContactManifold {
         coll2: &Collider,
         subshape1: usize,
         subshape2: usize,
+        solver_flags: SolverFlags,
     ) -> Self {
         Self::new(
             pair,
@@ -364,6 +386,7 @@ impl ContactManifold {
             *coll2.position_wrt_parent(),
             (coll1.friction + coll2.friction) * 0.5,
             (coll1.restitution + coll2.restitution) * 0.5,
+            solver_flags,
         )
     }
 
@@ -430,16 +453,26 @@ impl ContactManifold {
 
     #[inline]
     pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool {
+        //        const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
+        const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
+        const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded.
+        self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD)
+    }
+
+    #[inline]
+    pub(crate) fn try_update_contacts_eps(
+        &mut self,
+        pos12: &Isometry<f32>,
+        angle_dot_threshold: f32,
+        dist_sq_threshold: f32,
+    ) -> bool {
         if self.points.len() == 0 {
             return false;
         }
 
-        //        const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
-        const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
-
         let local_n2 = pos12 * self.local_n2;
 
-        if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
+        if -self.local_n1.dot(&local_n2) < angle_dot_threshold {
             return false;
         }
 
@@ -455,8 +488,7 @@ impl ContactManifold {
             }
             let new_local_p1 = local_p2 - self.local_n1 * dist;
 
-            let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
+            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
-            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
                 return false;
             }
 

src/geometry/contact_generator/ball_convex_contact_generator.rs

@@ -1,32 +1,21 @@
 use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{Ball, Contact, KinematicsCategory, Shape};
+use crate::geometry::{Ball, Contact, KinematicsCategory};
 use crate::math::Isometry;
 use na::Unit;
 use ncollide::query::PointQuery;
 
 pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let Shape::Ball(ball1) = ctxt.shape1 {
+    if let Some(ball1) = ctxt.shape1.as_ball() {
         ctxt.manifold.swap_identifiers();
-
+        do_generate_contacts(ctxt.shape2, ball1, ctxt, true);
-        match ctxt.shape2 {
+    } else if let Some(ball2) = ctxt.shape2.as_ball() {
-            Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true),
+        do_generate_contacts(ctxt.shape1, ball2, ctxt, false);
-            Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true),
-            Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true),
-            _ => unimplemented!(),
-        }
-    } else if let Shape::Ball(ball2) = ctxt.shape2 {
-        match ctxt.shape1 {
-            Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false),
-            Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false),
-            Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false),
-            _ => unimplemented!(),
-        }
     }
 
     ctxt.manifold.sort_contacts(ctxt.prediction_distance);
 }
 
-fn do_generate_contacts<P: PointQuery<f32>>(
+fn do_generate_contacts<P: ?Sized + PointQuery<f32>>(
     point_query1: &P,
     ball2: &Ball,
     ctxt: &mut PrimitiveContactGenerationContext,

src/geometry/contact_generator/capsule_capsule_contact_generator.rs

@@ -1,14 +1,14 @@
 use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory, Shape};
+use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory};
 use crate::math::Isometry;
 use crate::math::Vector;
 use approx::AbsDiffEq;
 use na::Unit;
 #[cfg(feature = "dim2")]
-use ncollide::shape::{Segment, SegmentPointLocation};
+use ncollide::shape::SegmentPointLocation;
 
 pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) {
+    if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) {
         generate_contacts(
             ctxt.prediction_distance,
             capsule1,
@@ -39,10 +39,11 @@ pub fn generate_contacts<'a>(
     let pos12 = pos1.inverse() * pos2;
     let pos21 = pos12.inverse();
 
-    let capsule2_1 = capsule2.transform_by(&pos12);
+    let seg1 = capsule1.segment;
+    let seg2_1 = capsule2.segment.transformed(&pos12);
     let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
-        (&capsule1.a, &capsule1.b),
+        (&seg1.a, &seg1.b),
-        (&capsule2_1.a, &capsule2_1.b),
+        (&seg2_1.a, &seg2_1.b),
     );
 
     // We do this clone to perform contact tracking and transfer impulses.
@@ -65,8 +66,8 @@ pub fn generate_contacts<'a>(
 
     let bcoords1 = loc1.barycentric_coordinates();
     let bcoords2 = loc2.barycentric_coordinates();
-    let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1];
+    let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
-    let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
+    let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
 
     let local_n1 =
         Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());
@@ -87,18 +88,15 @@ pub fn generate_contacts<'a>(
         return;
     }
 
-    let seg1 = Segment::new(capsule1.a, capsule1.b);
+    if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) {
-    let seg2 = Segment::new(capsule2_1.a, capsule2_1.b);
-
-    if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2.direction()) {
         if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8
             && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8
         {
-            // Capsules axii are almost parallel and are almost perpendicular to the normal.
+            // Capsules axes are almost parallel and are almost perpendicular to the normal.
             // Find a second contact point.
             if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal(
-                (capsule1.a, capsule1.b),
+                (seg1.a, seg1.b),
-                (capsule2_1.a, capsule2_1.b),
+                (seg2_1.a, seg2_1.b),
                 *local_n1,
             ) {
                 let contact =
@@ -156,17 +154,18 @@ pub fn generate_contacts<'a>(
     let pos12 = pos1.inverse() * pos2;
     let pos21 = pos12.inverse();
 
-    let capsule2_1 = capsule1.transform_by(&pos12);
+    let seg1 = capsule1.segment;
+    let seg2_1 = capsule2.segment.transformed(&pos12);
     let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
-        (&capsule1.a, &capsule1.b),
+        (&seg1.a, &seg1.b),
-        (&capsule2_1.a, &capsule2_1.b),
+        (&seg2_1.a, &seg2_1.b),
     );
 
     {
         let bcoords1 = loc1.barycentric_coordinates();
         let bcoords2 = loc2.barycentric_coordinates();
-        let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1];
+        let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
-        let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
+        let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
 
         let local_n1 =
             Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());

src/geometry/contact_generator/contact_dispatcher.rs

@@ -1,8 +1,10 @@
+#[cfg(feature = "dim3")]
+use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
 use crate::geometry::contact_generator::{
     ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace,
     PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace,
 };
-use crate::geometry::Shape;
+use crate::geometry::ShapeType;
 use std::any::Any;
 
 /// Trait implemented by structures responsible for selecting a collision-detection algorithm
@@ -11,8 +13,8 @@ pub trait ContactDispatcher {
     /// Select the collision-detection algorithm for the given pair of primitive shapes.
     fn dispatch_primitives(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (
         PrimitiveContactGenerator,
         Option<Box<dyn Any + Send + Sync>>,
@@ -20,8 +22,8 @@ pub trait ContactDispatcher {
     /// Select the collision-detection algorithm for the given pair of non-primitive shapes.
     fn dispatch(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>);
 }
 
@@ -31,14 +33,14 @@ pub struct DefaultContactDispatcher;
 impl ContactDispatcher for DefaultContactDispatcher {
     fn dispatch_primitives(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (
         PrimitiveContactGenerator,
         Option<Box<dyn Any + Send + Sync>>,
     ) {
         match (shape1, shape2) {
-            (Shape::Ball(_), Shape::Ball(_)) => (
+            (ShapeType::Ball, ShapeType::Ball) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_ball,
                     #[cfg(feature = "simd-is-enabled")]
@@ -47,52 +49,64 @@ impl ContactDispatcher for DefaultContactDispatcher {
                 },
                 None,
             ),
-            (Shape::Cuboid(_), Shape::Cuboid(_)) => (
+            (ShapeType::Cuboid, ShapeType::Cuboid) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_cuboid_cuboid,
                     ..PrimitiveContactGenerator::default()
                 },
                 None,
             ),
-            (Shape::Polygon(_), Shape::Polygon(_)) => (
+            // (ShapeType::Polygon, ShapeType::Polygon) => (
-                PrimitiveContactGenerator {
+            //     PrimitiveContactGenerator {
-                    generate_contacts: super::generate_contacts_polygon_polygon,
+            //         generate_contacts: super::generate_contacts_polygon_polygon,
-                    ..PrimitiveContactGenerator::default()
+            //         ..PrimitiveContactGenerator::default()
-                },
+            //     },
-                None,
+            //     None,
-            ),
+            // ),
-            (Shape::Capsule(_), Shape::Capsule(_)) => (
+            (ShapeType::Capsule, ShapeType::Capsule) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_capsule_capsule,
                     ..PrimitiveContactGenerator::default()
                 },
                 None,
             ),
-            (Shape::Cuboid(_), Shape::Ball(_))
+            (_, ShapeType::Ball) | (ShapeType::Ball, _) => (
-            | (Shape::Ball(_), Shape::Cuboid(_))
-            | (Shape::Triangle(_), Shape::Ball(_))
-            | (Shape::Ball(_), Shape::Triangle(_))
-            | (Shape::Capsule(_), Shape::Ball(_))
-            | (Shape::Ball(_), Shape::Capsule(_)) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_convex,
                     ..PrimitiveContactGenerator::default()
                 },
                 None,
             ),
-            (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => (
+            (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_cuboid_capsule,
                     ..PrimitiveContactGenerator::default()
                 },
                 None,
             ),
-            (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => (
+            (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => {
+                (
+                    PrimitiveContactGenerator {
+                        generate_contacts: super::generate_contacts_cuboid_triangle,
+                        ..PrimitiveContactGenerator::default()
+                    },
+                    None,
+                )
+            }
+            #[cfg(feature = "dim3")]
+            (ShapeType::Cylinder, _)
+            | (_, ShapeType::Cylinder)
+            | (ShapeType::Cone, _)
+            | (_, ShapeType::Cone)
+            | (ShapeType::RoundCylinder, _)
+            | (_, ShapeType::RoundCylinder)
+            | (ShapeType::Capsule, _)
+            | (_, ShapeType::Capsule) => (
                 PrimitiveContactGenerator {
-                    generate_contacts: super::generate_contacts_cuboid_triangle,
+                    generate_contacts: super::generate_contacts_pfm_pfm,
                     ..PrimitiveContactGenerator::default()
                 },
-                None,
+                Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())),
             ),
             _ => (PrimitiveContactGenerator::default(), None),
         }
@@ -100,18 +114,18 @@ impl ContactDispatcher for DefaultContactDispatcher {
 
     fn dispatch(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>) {
         match (shape1, shape2) {
-            (Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => (
+            (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => (
                 ContactPhase::NearPhase(ContactGenerator {
                     generate_contacts: super::generate_contacts_trimesh_shape,
                     ..ContactGenerator::default()
                 }),
                 Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())),
             ),
-            (Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => (
+            (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => (
                 ContactPhase::NearPhase(ContactGenerator {
                     generate_contacts: super::generate_contacts_heightfield_shape,
                     ..ContactGenerator::default()

src/geometry/contact_generator/contact_generator.rs

@@ -1,5 +1,6 @@
 use crate::geometry::{
     Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape,
+    SolverFlags,
 };
 use crate::math::Isometry;
 #[cfg(feature = "simd-is-enabled")]
@@ -26,8 +27,9 @@ impl ContactPhase {
             Self::NearPhase(gen) => (gen.generate_contacts)(&mut context),
             Self::ExactPhase(gen) => {
                 // Build the primitive context from the non-primitive context and dispatch.
-                let (collider1, collider2, manifold, workspace) =
+                let (collider1, collider2, manifold, workspace) = context
-                    context.pair.single_manifold(context.colliders);
+                    .pair
+                    .single_manifold(context.colliders, context.solver_flags);
                 let mut context2 = PrimitiveContactGenerationContext {
                     prediction_distance: context.prediction_distance,
                     collider1,
@@ -85,9 +87,11 @@ impl ContactPhase {
                     [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH],
                 > = ArrayVec::new();
 
-                for pair in context.pairs.iter_mut() {
+                for (pair, solver_flags) in
+                    context.pairs.iter_mut().zip(context.solver_flags.iter())
+                {
                     let (collider1, collider2, manifold, workspace) =
-                        pair.single_manifold(context.colliders);
+                        pair.single_manifold(context.colliders, *solver_flags);
                     colliders_arr.push((collider1, collider2));
                     manifold_arr.push(manifold);
                     workspace_arr.push(workspace);
@@ -139,8 +143,8 @@ pub struct PrimitiveContactGenerationContext<'a> {
     pub prediction_distance: f32,
     pub collider1: &'a Collider,
     pub collider2: &'a Collider,
-    pub shape1: &'a Shape,
+    pub shape1: &'a dyn Shape,
-    pub shape2: &'a Shape,
+    pub shape2: &'a dyn Shape,
     pub position1: &'a Isometry<f32>,
     pub position2: &'a Isometry<f32>,
     pub manifold: &'a mut ContactManifold,
@@ -152,8 +156,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> {
     pub prediction_distance: f32,
     pub colliders1: [&'a Collider; SIMD_WIDTH],
     pub colliders2: [&'a Collider; SIMD_WIDTH],
-    pub shapes1: [&'a Shape; SIMD_WIDTH],
+    pub shapes1: [&'a dyn Shape; SIMD_WIDTH],
-    pub shapes2: [&'a Shape; SIMD_WIDTH],
+    pub shapes2: [&'a dyn Shape; SIMD_WIDTH],
     pub positions1: &'a Isometry<SimdFloat>,
     pub positions2: &'a Isometry<SimdFloat>,
     pub manifolds: &'a mut [&'b mut ContactManifold],
@@ -188,6 +192,7 @@ pub struct ContactGenerationContext<'a> {
     pub prediction_distance: f32,
     pub colliders: &'a ColliderSet,
     pub pair: &'a mut ContactPair,
+    pub solver_flags: SolverFlags,
 }
 
 #[cfg(feature = "simd-is-enabled")]
@@ -196,6 +201,7 @@ pub struct ContactGenerationContextSimd<'a, 'b> {
     pub prediction_distance: f32,
     pub colliders: &'a ColliderSet,
     pub pairs: &'a mut [&'b mut ContactPair],
+    pub solver_flags: &'a [SolverFlags],
 }
 
 #[derive(Copy, Clone)]

src/geometry/contact_generator/cuboid_capsule_contact_generator.rs

@@ -1,15 +1,14 @@
 use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
 #[cfg(feature = "dim3")]
 use crate::geometry::PolyhedronFace;
-use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory, Shape};
+use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory};
 #[cfg(feature = "dim2")]
 use crate::geometry::{CuboidFeature, CuboidFeatureFace};
 use crate::math::Isometry;
 use crate::math::Vector;
-use ncollide::shape::Segment;
 
 pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) {
+    if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) {
         generate_contacts(
             ctxt.prediction_distance,
             cube1,
@@ -20,7 +19,9 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon
             false,
         );
         ctxt.manifold.update_warmstart_multiplier();
-    } else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+    } else if let (Some(capsule1), Some(cube2)) =
+        (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid())
+    {
         generate_contacts(
             ctxt.prediction_distance,
             cube2,
@@ -53,7 +54,7 @@ pub fn generate_contacts<'a>(
         return;
     }
 
-    let segment2 = Segment::new(capsule2.a, capsule2.b);
+    let segment2 = capsule2.segment;
 
     /*
      *

src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs

@@ -1,12 +1,12 @@
 use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory, Shape};
+use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory};
 use crate::math::Isometry;
 #[cfg(feature = "dim2")]
 use crate::math::Vector;
 use ncollide::shape::Cuboid;
 
 pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+    if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) {
         generate_contacts(
             ctxt.prediction_distance,
             cube1,

src/geometry/contact_generator/cuboid_triangle_contact_generator.rs

@@ -1,7 +1,7 @@
 use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
 #[cfg(feature = "dim3")]
 use crate::geometry::PolyhedronFace;
-use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Shape, Triangle};
+use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Triangle};
 use crate::math::Isometry;
 #[cfg(feature = "dim2")]
 use crate::{
@@ -10,7 +10,7 @@ use crate::{
 };
 
 pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) {
+    if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) {
         generate_contacts(
             ctxt.prediction_distance,
             cube1,
@@ -21,7 +21,9 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo
             false,
         );
         ctxt.manifold.update_warmstart_multiplier();
-    } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+    } else if let (Some(triangle1), Some(cube2)) =
+        (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid())
+    {
         generate_contacts(
             ctxt.prediction_distance,
             cube2,

src/geometry/contact_generator/heightfield_shape_contact_generator.rs

@@ -3,10 +3,8 @@ use crate::geometry::contact_generator::{
 };
 #[cfg(feature = "dim2")]
 use crate::geometry::Capsule;
-use crate::geometry::{Collider, ContactManifold, HeightField, Shape};
+use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType};
 use crate::ncollide::bounding_volume::BoundingVolume;
-#[cfg(feature = "dim3")]
-use crate::{geometry::Triangle, math::Point};
 use std::any::Any;
 use std::collections::hash_map::Entry;
 use std::collections::HashMap;
@@ -38,9 +36,9 @@ pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext)
     let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
     let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
 
-    if let Shape::HeightField(heightfield1) = collider1.shape() {
+    if let Some(heightfield1) = collider1.shape().as_heightfield() {
         do_generate_contacts(heightfield1, collider1, collider2, ctxt, false)
-    } else if let Shape::HeightField(heightfield2) = collider2.shape() {
+    } else if let Some(heightfield2) = collider2.shape().as_heightfield() {
         do_generate_contacts(heightfield2, collider2, collider1, ctxt, true)
     }
 }
@@ -59,6 +57,7 @@ fn do_generate_contacts(
         .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.")
         .downcast_mut()
         .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace.");
+    let shape_type2 = collider2.shape().shape_type();
 
     /*
      * Detect if the detector context has been reset.
@@ -71,24 +70,9 @@ fn do_generate_contacts(
             } else {
                 manifold.subshape_index_pair.1
             };
-            // println!(
-            //     "Restoring for {} [chosen with {:?}]",
-            //     subshape_id, manifold.subshape_index_pair
-            // );
-
-            // Use dummy shapes for the dispatch.
-            #[cfg(feature = "dim2")]
-            let sub_shape1 =
-                Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0));
-            #[cfg(feature = "dim3")]
-            let sub_shape1 = Shape::Triangle(Triangle::new(
-                Point::origin(),
-                Point::origin(),
-                Point::origin(),
-            ));
             let (generator, workspace2) = ctxt
                 .dispatcher
-                .dispatch_primitives(&sub_shape1, collider2.shape());
+                .dispatch_primitives(ShapeType::Capsule, shape_type2);
 
             let sub_detector = SubDetector {
                 generator,
@@ -120,12 +104,16 @@ fn do_generate_contacts(
     let manifolds = &mut ctxt.pair.manifolds;
     let prediction_distance = ctxt.prediction_distance;
     let dispatcher = ctxt.dispatcher;
+    let solver_flags = ctxt.solver_flags;
+    let shape_type2 = collider2.shape().shape_type();
 
     heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| {
+        let position1 = collider1.position();
         #[cfg(feature = "dim2")]
-        let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0));
+        let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead.
         #[cfg(feature = "dim3")]
-        let sub_shape1 = Shape::Triangle(*part1);
+        let sub_shape1 = *part1;
+
         let sub_detector = match workspace.sub_detectors.entry(i) {
             Entry::Occupied(entry) => {
                 let sub_detector = entry.into_mut();
@@ -137,15 +125,21 @@ fn do_generate_contacts(
             }
             Entry::Vacant(entry) => {
                 let (generator, workspace2) =
-                    dispatcher.dispatch_primitives(&sub_shape1, collider2.shape());
+                    dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2);
                 let sub_detector = SubDetector {
                     generator,
                     manifold_id: manifolds.len(),
                     timestamp: new_timestamp,
                     workspace: workspace2,
                 };
-                let manifold =
+                let manifold = ContactManifold::with_subshape_indices(
-                    ContactManifold::with_subshape_indices(coll_pair, collider1, collider2, i, 0);
+                    coll_pair,
+                    collider1,
+                    collider2,
+                    i,
+                    0,
+                    solver_flags,
+                );
                 manifolds.push(manifold);
 
                 entry.insert(sub_detector)
@@ -162,7 +156,7 @@ fn do_generate_contacts(
                 shape1: collider2.shape(),
                 shape2: &sub_shape1,
                 position1: collider2.position(),
-                position2: collider1.position(),
+                position2: position1,
                 manifold,
                 workspace: sub_detector.workspace.as_deref_mut(),
             }
@@ -173,7 +167,7 @@ fn do_generate_contacts(
                 collider2,
                 shape1: &sub_shape1,
                 shape2: collider2.shape(),
-                position1: collider1.position(),
+                position1,
                 position2: collider2.position(),
                 manifold,
                 workspace: sub_detector.workspace.as_deref_mut(),

src/geometry/contact_generator/mod.rs

@@ -18,15 +18,18 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang
 pub use self::heightfield_shape_contact_generator::{
     generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace,
 };
-pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
+#[cfg(feature = "dim3")]
+pub use self::pfm_pfm_contact_generator::{
+    generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace,
+};
+// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
 pub use self::trimesh_shape_contact_generator::{
     generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
 };
 
+pub(crate) use self::polygon_polygon_contact_generator::clip_segments;
 #[cfg(feature = "dim2")]
-pub(crate) use self::polygon_polygon_contact_generator::{
+pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal;
-    clip_segments, clip_segments_with_normal,
-};
 
 mod ball_ball_contact_generator;
 mod ball_convex_contact_generator;
@@ -39,6 +42,8 @@ mod cuboid_cuboid_contact_generator;
 mod cuboid_polygon_contact_generator;
 mod cuboid_triangle_contact_generator;
 mod heightfield_shape_contact_generator;
+#[cfg(feature = "dim3")]
+mod pfm_pfm_contact_generator;
 mod polygon_polygon_contact_generator;
 mod trimesh_shape_contact_generator;
 

src/geometry/contact_generator/pfm_pfm_contact_generator.rs

@@ -0,0 +1,119 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{KinematicsCategory, PolygonalFeatureMap, PolyhedronFace};
+use crate::math::{Isometry, Vector};
+use na::Unit;
+use ncollide::query;
+use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex};
+
+pub struct PfmPfmContactManifoldGeneratorWorkspace {
+    simplex: VoronoiSimplex<f32>,
+    last_gjk_dir: Option<Unit<Vector<f32>>>,
+    feature1: PolyhedronFace,
+    feature2: PolyhedronFace,
+}
+
+impl Default for PfmPfmContactManifoldGeneratorWorkspace {
+    fn default() -> Self {
+        Self {
+            simplex: VoronoiSimplex::new(),
+            last_gjk_dir: None,
+            feature1: PolyhedronFace::new(),
+            feature2: PolyhedronFace::new(),
+        }
+    }
+}
+
+pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = (
+        ctxt.shape1.as_polygonal_feature_map(),
+        ctxt.shape2.as_polygonal_feature_map(),
+    ) {
+        do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt);
+        ctxt.manifold.update_warmstart_multiplier();
+        ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+    }
+}
+
+fn do_generate_contacts(
+    pfm1: &dyn PolygonalFeatureMap,
+    border_radius1: f32,
+    pfm2: &dyn PolygonalFeatureMap,
+    border_radius2: f32,
+    ctxt: &mut PrimitiveContactGenerationContext,
+) {
+    let pos12 = ctxt.position1.inverse() * ctxt.position2;
+    let pos21 = pos12.inverse();
+
+    // We use very small thresholds for the manifold update because something to high would
+    // cause numerical drifts with the effect of introducing bumps in
+    // what should have been smooth rolling motions.
+    if ctxt
+        .manifold
+        .try_update_contacts_eps(&pos12, crate::utils::COS_1_DEGREES, 1.0e-6)
+    {
+        return;
+    }
+
+    let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
+        .workspace
+        .as_mut()
+        .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
+        .downcast_mut()
+        .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
+
+    let total_prediction = ctxt.prediction_distance + border_radius1 + border_radius2;
+    let contact = query::contact_support_map_support_map_with_params(
+        &Isometry::identity(),
+        pfm1,
+        &pos12,
+        pfm2,
+        total_prediction,
+        &mut workspace.simplex,
+        workspace.last_gjk_dir,
+    );
+
+    let old_manifold_points = ctxt.manifold.points.clone();
+    ctxt.manifold.points.clear();
+
+    match contact {
+        GJKResult::ClosestPoints(_, _, dir) => {
+            workspace.last_gjk_dir = Some(dir);
+            let normal1 = dir;
+            let normal2 = pos21 * -dir;
+            pfm1.local_support_feature(&normal1, &mut workspace.feature1);
+            pfm2.local_support_feature(&normal2, &mut workspace.feature2);
+            workspace.feature2.transform_by(&pos12);
+
+            PolyhedronFace::contacts(
+                total_prediction,
+                &workspace.feature1,
+                &normal1,
+                &workspace.feature2,
+                &pos21,
+                ctxt.manifold,
+            );
+
+            if border_radius1 != 0.0 || border_radius2 != 0.0 {
+                for contact in &mut ctxt.manifold.points {
+                    contact.local_p1 += *normal1 * border_radius1;
+                    contact.local_p2 += *normal2 * border_radius2;
+                    contact.dist -= border_radius1 + border_radius2;
+                }
+            }
+
+            // Adjust points to take the radius into account.
+            ctxt.manifold.local_n1 = *normal1;
+            ctxt.manifold.local_n2 = *normal2;
+            ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate?
+            ctxt.manifold.kinematics.radius1 = 0.0;
+            ctxt.manifold.kinematics.radius2 = 0.0;
+        }
+        GJKResult::NoIntersection(dir) => {
+            workspace.last_gjk_dir = Some(dir);
+        }
+        _ => {}
+    }
+
+    // Transfer impulses.
+    super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false);
+}

src/geometry/contact_generator/polygon_polygon_contact_generator.rs

@@ -1,24 +1,27 @@
+#![allow(dead_code)] // TODO: remove this once we support polygons.
+
 use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon, Shape};
+use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon};
 use crate::math::{Isometry, Point};
 #[cfg(feature = "dim2")]
 use crate::{math::Vector, utils};
 
-pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) {
+pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
+    unimplemented!()
-        generate_contacts(
+    // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
-            polygon1,
+    //     generate_contacts(
-            &ctxt.position1,
+    //         polygon1,
-            polygon2,
+    //         &ctxt.position1,
-            &ctxt.position2,
+    //         polygon2,
-            ctxt.manifold,
+    //         &ctxt.position2,
-        );
+    //         ctxt.manifold,
-        ctxt.manifold.update_warmstart_multiplier();
+    //     );
-    } else {
+    //     ctxt.manifold.update_warmstart_multiplier();
-        unreachable!()
+    // } else {
-    }
+    //     unreachable!()
-
+    // }
-    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+    //
+    // ctxt.manifold.sort_contacts(ctxt.prediction_distance);
 }
 
 fn generate_contacts<'a>(

src/geometry/contact_generator/trimesh_shape_contact_generator.rs

@@ -1,7 +1,7 @@
 use crate::geometry::contact_generator::{
     ContactGenerationContext, PrimitiveContactGenerationContext,
 };
-use crate::geometry::{Collider, ContactManifold, Shape, Trimesh};
+use crate::geometry::{Collider, ContactManifold, ShapeType, Trimesh};
 use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
 
 pub struct TrimeshShapeContactGeneratorWorkspace {
@@ -26,9 +26,9 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) {
     let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
     let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
 
-    if let Shape::Trimesh(trimesh1) = collider1.shape() {
+    if let Some(trimesh1) = collider1.shape().as_trimesh() {
         do_generate_contacts(trimesh1, collider1, collider2, ctxt, false)
-    } else if let Shape::Trimesh(trimesh2) = collider2.shape() {
+    } else if let Some(trimesh2) = collider2.shape().as_trimesh() {
         do_generate_contacts(trimesh2, collider2, collider1, ctxt, true)
     }
 }
@@ -121,6 +121,7 @@ fn do_generate_contacts(
     let new_interferences = &workspace.interferences;
     let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
     let mut old_manifolds_it = workspace.old_manifolds.drain(..);
+    let shape_type2 = collider2.shape().shape_type();
 
     for (i, triangle_id) in new_interferences.iter().enumerate() {
         if *triangle_id >= trimesh1.num_triangles() {
@@ -148,6 +149,7 @@ fn do_generate_contacts(
                     collider2,
                     *triangle_id,
                     0,
+                    ctxt.solver_flags,
                 )
             } else {
                 // We already have a manifold for this triangle.
@@ -159,10 +161,10 @@ fn do_generate_contacts(
         }
 
         let manifold = &mut ctxt.pair.manifolds[i];
-        let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id));
+        let triangle1 = trimesh1.triangle(*triangle_id);
         let (generator, mut workspace2) = ctxt
             .dispatcher
-            .dispatch_primitives(&triangle1, collider2.shape());
+            .dispatch_primitives(ShapeType::Triangle, shape_type2);
 
         let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 {
             PrimitiveContactGenerationContext {

src/geometry/interaction_groups.rs

@@ -0,0 +1,60 @@
+#[repr(transparent)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)]
+/// Pairwise filtering using bit masks.
+///
+/// This filtering method is based on two 16-bit values:
+/// - The interaction groups (the 16 left-most bits of `self.0`).
+/// - The interaction mask (the 16 right-most bits of `self.0`).
+///
+/// An interaction is allowed between two filters `a` and `b` two conditions
+/// are met simultaneously:
+/// - The interaction groups of `a` has at least one bit set to `1` in common with the interaction mask of `b`.
+/// - The interaction groups of `b` has at least one bit set to `1` in common with the interaction mask of `a`.
+/// In other words, interactions are allowed between two filter iff. the following condition is met:
+/// ```ignore
+/// ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0
+/// ```
+pub struct InteractionGroups(pub u32);
+
+impl InteractionGroups {
+    /// Initializes with the given interaction groups and interaction mask.
+    pub const fn new(groups: u16, masks: u16) -> Self {
+        Self::none().with_groups(groups).with_mask(masks)
+    }
+
+    /// Allow interaction with everything.
+    pub const fn all() -> Self {
+        Self(u32::MAX)
+    }
+
+    /// Prevent all interactions.
+    pub const fn none() -> Self {
+        Self(0)
+    }
+
+    /// Sets the group this filter is part of.
+    pub const fn with_groups(self, groups: u16) -> Self {
+        Self((self.0 & 0x0000ffff) | ((groups as u32) << 16))
+    }
+
+    /// Sets the interaction mask of this filter.
+    pub const fn with_mask(self, mask: u16) -> Self {
+        Self((self.0 & 0xffff0000) | (mask as u32))
+    }
+
+    /// Check if interactions should be allowed based on the interaction groups and mask.
+    ///
+    /// An interaction is allowed iff. the groups of `self` contain at least one bit set to 1 in common
+    /// with the mask of `rhs`, and vice-versa.
+    #[inline]
+    pub const fn test(self, rhs: Self) -> bool {
+        ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0
+    }
+}
+
+impl Default for InteractionGroups {
+    fn default() -> Self {
+        Self::all()
+    }
+}

src/geometry/mod.rs

@@ -2,10 +2,10 @@
 
 pub use self::broad_phase_multi_sap::BroadPhase;
 pub use self::capsule::Capsule;
-pub use self::collider::{Collider, ColliderBuilder, Shape};
+pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
 pub use self::collider_set::{ColliderHandle, ColliderSet};
 pub use self::contact::{
-    Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory,
+    Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, SolverFlags,
 };
 pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher};
 #[cfg(feature = "dim2")]
@@ -19,9 +19,14 @@ pub use self::narrow_phase::NarrowPhase;
 pub use self::polygon::Polygon;
 pub use self::proximity::ProximityPair;
 pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher};
+#[cfg(feature = "dim3")]
+pub use self::round_cylinder::RoundCylinder;
 pub use self::trimesh::Trimesh;
+pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter};
 pub use ncollide::query::Proximity;
 
+/// A segment shape.
+pub type Segment = ncollide::shape::Segment<f32>;
 /// A cuboid shape.
 pub type Cuboid = ncollide::shape::Cuboid<f32>;
 /// A triangle shape.
@@ -30,6 +35,12 @@ pub type Triangle = ncollide::shape::Triangle<f32>;
 pub type Ball = ncollide::shape::Ball<f32>;
 /// A heightfield shape.
 pub type HeightField = ncollide::shape::HeightField<f32>;
+/// A cylindrical shape.
+#[cfg(feature = "dim3")]
+pub type Cylinder = ncollide::shape::Cylinder<f32>;
+/// A cone shape.
+#[cfg(feature = "dim3")]
+pub type Cone = ncollide::shape::Cone<f32>;
 /// An axis-aligned bounding box.
 pub type AABB = ncollide::bounding_volume::AABB<f32>;
 /// Event triggered when two non-sensor colliders start or stop being in contact.
@@ -40,6 +51,8 @@ pub type ProximityEvent = ncollide::pipeline::ProximityEvent<ColliderHandle>;
 pub type Ray = ncollide::query::Ray<f32>;
 /// The intersection between a ray and a  collider.
 pub type RayIntersection = ncollide::query::RayIntersection<f32>;
+/// The the projection of a point on a collider.
+pub type PointProjection = ncollide::query::PointProjection<f32>;
 
 #[cfg(feature = "simd-is-enabled")]
 pub(crate) use self::ball::WBall;
@@ -47,18 +60,22 @@ pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
 pub(crate) use self::collider_set::RemovedCollider;
 #[cfg(feature = "simd-is-enabled")]
 pub(crate) use self::contact::WContact;
+pub(crate) use self::contact_generator::clip_segments;
 #[cfg(feature = "dim2")]
-pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal};
+pub(crate) use self::contact_generator::clip_segments_with_normal;
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 #[cfg(feature = "dim3")]
+pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap;
+#[cfg(feature = "dim3")]
 pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
 pub(crate) use self::waabb::{WRay, WAABB};
 pub(crate) use self::wquadtree::WQuadtree;
 //pub(crate) use self::z_order::z_cmp_floats;
+pub use self::interaction_groups::InteractionGroups;
+pub use self::shape::{Shape, ShapeType};
 
 mod ball;
 mod broad_phase_multi_sap;
-mod capsule;
 mod collider;
 mod collider_set;
 mod contact;
@@ -81,3 +98,11 @@ mod trimesh;
 mod waabb;
 mod wquadtree;
 //mod z_order;
+mod capsule;
+mod interaction_groups;
+#[cfg(feature = "dim3")]
+mod polygonal_feature_map;
+#[cfg(feature = "dim3")]
+mod round_cylinder;
+mod shape;
+mod user_callbacks;

src/geometry/narrow_phase.rs

@@ -14,8 +14,9 @@ use crate::geometry::proximity_detector::{
 //    proximity_detector::ProximityDetectionContextSimd, WBall,
 //};
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent,
+    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactPairFilter,
-    ProximityPair, RemovedCollider,
+    PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, RemovedCollider,
+    SolverFlags,
 };
 use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
 //#[cfg(feature = "simd-is-enabled")]
@@ -197,7 +198,8 @@ impl NarrowPhase {
 
                             if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() {
                                 let dispatcher = DefaultProximityDispatcher;
-                                let generator = dispatcher.dispatch(co1.shape(), co2.shape());
+                                let generator = dispatcher
+                                    .dispatch(co1.shape().shape_type(), co2.shape().shape_type());
                                 let interaction =
                                     ProximityPair::new(*pair, generator.0, generator.1);
                                 let _ = self.proximity_graph.add_edge(
@@ -226,7 +228,8 @@ impl NarrowPhase {
 
                             if self.contact_graph.graph.find_edge(gid1, gid2).is_none() {
                                 let dispatcher = DefaultContactDispatcher;
-                                let generator = dispatcher.dispatch(co1.shape(), co2.shape());
+                                let generator = dispatcher
+                                    .dispatch(co1.shape().shape_type(), co2.shape().shape_type());
                                 let interaction = ContactPair::new(*pair, generator.0, generator.1);
                                 let _ = self.contact_graph.add_edge(
                                     co1.contact_graph_index,
@@ -288,6 +291,7 @@ impl NarrowPhase {
         prediction_distance: f32,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        pair_filter: Option<&dyn ProximityPairFilter>,
         events: &dyn EventHandler,
     ) {
         par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| {
@@ -299,16 +303,44 @@ impl NarrowPhase {
             let rb1 = &bodies[co1.parent];
             let rb2 = &bodies[co2.parent];
 
-            if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) {
+            if (rb1.is_sleeping() && rb2.is_static())
-                // No need to update this contact because nothing moved.
+                || (rb2.is_sleeping() && rb1.is_static())
+                || (rb1.is_sleeping() && rb2.is_sleeping())
+            {
+                // No need to update this proximity because nothing moved.
                 return;
             }
 
+            if !co1.collision_groups.test(co2.collision_groups) {
+                // The proximity is not allowed.
+                return;
+            }
+
+            if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
+                // Default filtering rule: no proximity between two non-dynamic bodies.
+                return;
+            }
+
+            if let Some(filter) = pair_filter {
+                let context = PairFilterContext {
+                    rigid_body1: rb1,
+                    rigid_body2: rb2,
+                    collider1: co1,
+                    collider2: co2,
+                };
+
+                if !filter.filter_proximity_pair(&context) {
+                    // No proximity allowed.
+                    return;
+                }
+            }
+
             let dispatcher = DefaultProximityDispatcher;
             if pair.detector.is_none() {
                 // We need a redispatch for this detector.
                 // This can happen, e.g., after restoring a snapshot of the narrow-phase.
-                let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
+                let (detector, workspace) =
+                    dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
                 pair.detector = Some(detector);
                 pair.detector_workspace = workspace;
             }
@@ -326,69 +358,6 @@ impl NarrowPhase {
                 .unwrap()
                 .detect_proximity(context, events);
         });
-
-        /*
-        // First, group pairs.
-        // NOTE: the transmutes here are OK because the Vec are all cleared
-        // before we leave this method.
-        // We do this in order to avoid reallocating those vecs each time
-        // we compute the contacts. Unsafe is necessary because we can't just
-        // store a Vec<&mut ProximityPair> into the NarrowPhase struct without
-        // polluting the World with lifetimes.
-        let ball_ball_prox: &mut Vec<&mut ProximityPair> =
-            unsafe { std::mem::transmute(&mut self.ball_ball_prox) };
-        let shape_shape_prox: &mut Vec<&mut ProximityPair> =
-            unsafe { std::mem::transmute(&mut self.shape_shape_prox) };
-
-        let bodies = &bodies.bodies;
-
-        // FIXME: don't iterate through all the interactions.
-        for pair in &mut self.proximity_graph.interactions {
-            let co1 = &colliders[pair.pair.collider1];
-            let co2 = &colliders[pair.pair.collider2];
-
-            // FIXME: avoid lookup into bodies.
-            let rb1 = &bodies[co1.parent];
-            let rb2 = &bodies[co2.parent];
-
-            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
-            {
-                // No need to update this proximity because nothing moved.
-                continue;
-            }
-
-            match (co1.shape(), co2.shape()) {
-                (Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair),
-                _ => shape_shape_prox.push(pair),
-            }
-        }
-
-        par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| {
-            let context = ProximityDetectionContextSimd {
-                dispatcher: &DefaultProximityDispatcher,
-                prediction_distance,
-                colliders,
-                pairs,
-            };
-            context.pairs[0]
-                .detector
-                .detect_proximity_simd(context, events);
-        });
-
-        par_iter_mut!(shape_shape_prox).for_each(|pair| {
-            let context = ProximityDetectionContext {
-                dispatcher: &DefaultProximityDispatcher,
-                prediction_distance,
-                colliders,
-                pair,
-            };
-
-            context.pair.detector.detect_proximity(context, events);
-        });
-
-        ball_ball_prox.clear();
-        shape_shape_prox.clear();
-        */
     }
 
     pub(crate) fn compute_contacts(
@@ -396,6 +365,7 @@ impl NarrowPhase {
         prediction_distance: f32,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
+        pair_filter: Option<&dyn ContactPairFilter>,
         events: &dyn EventHandler,
     ) {
         par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
@@ -407,18 +377,52 @@ impl NarrowPhase {
             let rb1 = &bodies[co1.parent];
             let rb2 = &bodies[co2.parent];
 
-            if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()))
+            if (rb1.is_sleeping() && rb2.is_static())
-                || (!rb1.is_dynamic() && !rb2.is_dynamic())
+                || (rb2.is_sleeping() && rb1.is_static())
+                || (rb1.is_sleeping() && rb2.is_sleeping())
             {
                 // No need to update this contact because nothing moved.
                 return;
             }
 
+            if !co1.collision_groups.test(co2.collision_groups) {
+                // The collision is not allowed.
+                return;
+            }
+
+            if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
+                // Default filtering rule: no contact between two non-dynamic bodies.
+                return;
+            }
+
+            let mut solver_flags = if let Some(filter) = pair_filter {
+                let context = PairFilterContext {
+                    rigid_body1: rb1,
+                    rigid_body2: rb2,
+                    collider1: co1,
+                    collider2: co2,
+                };
+
+                if let Some(solver_flags) = filter.filter_contact_pair(&context) {
+                    solver_flags
+                } else {
+                    // No contact allowed.
+                    return;
+                }
+            } else {
+                SolverFlags::COMPUTE_IMPULSES
+            };
+
+            if !co1.solver_groups.test(co2.solver_groups) {
+                solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
+            }
+
             let dispatcher = DefaultContactDispatcher;
             if pair.generator.is_none() {
                 // We need a redispatch for this generator.
                 // This can happen, e.g., after restoring a snapshot of the narrow-phase.
-                let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
+                let (generator, workspace) =
+                    dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
                 pair.generator = Some(generator);
                 pair.generator_workspace = workspace;
             }
@@ -428,6 +432,7 @@ impl NarrowPhase {
                 prediction_distance,
                 colliders,
                 pair,
+                solver_flags,
             };
 
             context
@@ -436,69 +441,6 @@ impl NarrowPhase {
                 .unwrap()
                 .generate_contacts(context, events);
         });
-
-        /*
-        // First, group pairs.
-        // NOTE: the transmutes here are OK because the Vec are all cleared
-        // before we leave this method.
-        // We do this in order to avoid reallocating those vecs each time
-        // we compute the contacts. Unsafe is necessary because we can't just
-        // store a Vec<&mut ContactPair> into the NarrowPhase struct without
-        // polluting the World with lifetimes.
-        let ball_ball: &mut Vec<&mut ContactPair> =
-            unsafe { std::mem::transmute(&mut self.ball_ball) };
-        let shape_shape: &mut Vec<&mut ContactPair> =
-            unsafe { std::mem::transmute(&mut self.shape_shape) };
-
-        let bodies = &bodies.bodies;
-
-        // FIXME: don't iterate through all the interactions.
-        for pair in &mut self.contact_graph.interactions {
-            let co1 = &colliders[pair.pair.collider1];
-            let co2 = &colliders[pair.pair.collider2];
-
-            // FIXME: avoid lookup into bodies.
-            let rb1 = &bodies[co1.parent];
-            let rb2 = &bodies[co2.parent];
-
-            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
-            {
-                // No need to update this contact because nothing moved.
-                continue;
-            }
-
-            match (co1.shape(), co2.shape()) {
-                (Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair),
-                _ => shape_shape.push(pair),
-            }
-        }
-
-        par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| {
-            let context = ContactGenerationContextSimd {
-                dispatcher: &DefaultContactDispatcher,
-                prediction_distance,
-                colliders,
-                pairs,
-            };
-            context.pairs[0]
-                .generator
-                .generate_contacts_simd(context, events);
-        });
-
-        par_iter_mut!(shape_shape).for_each(|pair| {
-            let context = ContactGenerationContext {
-                dispatcher: &DefaultContactDispatcher,
-                prediction_distance,
-                colliders,
-                pair,
-            };
-
-            context.pair.generator.generate_contacts(context, events);
-        });
-
-        ball_ball.clear();
-        shape_shape.clear();
-        */
     }
 
     /// Retrieve all the interactions with at least one contact point, happening between two active bodies.
@@ -518,7 +460,11 @@ impl NarrowPhase {
             for manifold in &mut inter.weight.manifolds {
                 let rb1 = &bodies[manifold.body_pair.body1];
                 let rb2 = &bodies[manifold.body_pair.body2];
-                if manifold.num_active_contacts() != 0
+                if manifold
+                    .solver_flags
+                    .contains(SolverFlags::COMPUTE_IMPULSES)
+                    && manifold.num_active_contacts() != 0
+                    && (rb1.is_dynamic() || rb2.is_dynamic())
                     && (!rb1.is_dynamic() || !rb1.is_sleeping())
                     && (!rb2.is_dynamic() || !rb2.is_sleeping())
                 {

src/geometry/polygon.rs

@@ -1,3 +1,5 @@
+#![allow(dead_code)] // TODO: remove this once we support polygons.
+
 use crate::math::{Isometry, Point, Vector};
 use ncollide::bounding_volume::AABB;
 

src/geometry/polygonal_feature_map.rs

@@ -0,0 +1,132 @@
+use crate::geometry::PolyhedronFace;
+use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Segment, Triangle};
+use crate::math::{Point, Vector};
+use approx::AbsDiffEq;
+use na::{Unit, Vector2};
+use ncollide::shape::SupportMap;
+
+/// Trait implemented by convex shapes with features with polyhedral approximations.
+pub trait PolygonalFeatureMap: SupportMap<f32> {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace);
+}
+
+impl PolygonalFeatureMap for Segment {
+    fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
+        *out_feature = PolyhedronFace::from(*self);
+    }
+}
+
+impl PolygonalFeatureMap for Triangle {
+    fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
+        *out_feature = PolyhedronFace::from(*self);
+    }
+}
+
+impl PolygonalFeatureMap for Cuboid {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
+        let face = cuboid::support_face(self, **dir);
+        *out_feature = PolyhedronFace::from(face);
+    }
+}
+
+impl PolygonalFeatureMap for Cylinder {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
+        // About feature ids.
+        // At all times, we consider our cylinder to be approximated as follows:
+        // - The curved part is approximated by a single segment.
+        // - Each flat cap of the cylinder is approximated by a square.
+        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
+        //   `y` coordinate has an ID of 1.
+        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
+        //   when looking at the cap with an eye looking towards +y).
+        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
+        // - The bottom cap has its face feature ID of 9.
+        // - The feature IDs of the top cap are the same as the bottom cap to which we add 10.
+        //   So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19.
+        // - Note that at all times, one of each cap's vertices are the same as the curved-part
+        //   segment endpoints.
+        let dir2 = Vector2::new(dir.x, dir.z)
+            .try_normalize(f32::default_epsilon())
+            .unwrap_or(Vector2::x());
+
+        if dir.y.abs() < 0.5 {
+            // We return a segment lying on the cylinder's curved part.
+            out_features.vertices[0] = Point::new(
+                dir2.x * self.radius,
+                -self.half_height,
+                dir2.y * self.radius,
+            );
+            out_features.vertices[1] =
+                Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
+            out_features.eids = [0, 0, 0, 0];
+            out_features.fid = 0;
+            out_features.num_vertices = 2;
+            out_features.vids = [1, 11, 11, 11];
+        } else {
+            // We return a square approximation of the cylinder cap.
+            let y = self.half_height.copysign(dir.y);
+            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
+            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
+            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
+            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
+
+            if dir.y < 0.0 {
+                out_features.eids = [2, 4, 6, 8];
+                out_features.fid = 9;
+                out_features.num_vertices = 4;
+                out_features.vids = [1, 3, 5, 7];
+            } else {
+                out_features.eids = [12, 14, 16, 18];
+                out_features.fid = 19;
+                out_features.num_vertices = 4;
+                out_features.vids = [11, 13, 15, 17];
+            }
+        }
+    }
+}
+
+impl PolygonalFeatureMap for Cone {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
+        // About feature ids. It is very similar to the feature ids of cylinders.
+        // At all times, we consider our cone to be approximated as follows:
+        // - The curved part is approximated by a single segment.
+        // - The flat cap of the cone is approximated by a square.
+        // - The curved-part segment has a feature ID of 0, and its endpoint with negative
+        //   `y` coordinate has an ID of 1.
+        // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
+        //   when looking at the cap with an eye looking towards +y).
+        // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
+        // - The bottom cap has its face feature ID of 9.
+        // - Note that at all times, one of the cap's vertices are the same as the curved-part
+        //   segment endpoints.
+        let dir2 = Vector2::new(dir.x, dir.z)
+            .try_normalize(f32::default_epsilon())
+            .unwrap_or(Vector2::x());
+
+        if dir.y > 0.0 {
+            // We return a segment lying on the cone's curved part.
+            out_features.vertices[0] = Point::new(
+                dir2.x * self.radius,
+                -self.half_height,
+                dir2.y * self.radius,
+            );
+            out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0);
+            out_features.eids = [0, 0, 0, 0];
+            out_features.fid = 0;
+            out_features.num_vertices = 2;
+            out_features.vids = [1, 11, 11, 11];
+        } else {
+            // We return a square approximation of the cone cap.
+            let y = -self.half_height;
+            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
+            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
+            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
+            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
+
+            out_features.eids = [2, 4, 6, 8];
+            out_features.fid = 9;
+            out_features.num_vertices = 4;
+            out_features.vids = [1, 3, 5, 7];
+        }
+    }
+}

src/geometry/polyhedron_feature3d.rs

@@ -1,4 +1,5 @@
-use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle};
+use crate::approx::AbsDiffEq;
+use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle};
 use crate::math::{Isometry, Point, Vector};
 use crate::utils::WBasis;
 use na::Point2;
@@ -39,6 +40,8 @@ impl From<Triangle> for PolyhedronFace {
 
 impl From<Segment<f32>> for PolyhedronFace {
     fn from(seg: Segment<f32>) -> Self {
+        // Vertices have feature ids 0 and 2.
+        // The segment interior has feature id 1.
         Self {
             vertices: [seg.a, seg.b, seg.b, seg.b],
             vids: [0, 2, 2, 2],
@@ -50,9 +53,19 @@ impl From<Segment<f32>> for PolyhedronFace {
 }
 
 impl PolyhedronFace {
+    pub fn new() -> Self {
+        Self {
+            vertices: [Point::origin(); 4],
+            vids: [0; 4],
+            eids: [0; 4],
+            fid: 0,
+            num_vertices: 0,
+        }
+    }
+
     pub fn transform_by(&mut self, iso: &Isometry<f32>) {
-        for v in &mut self.vertices[0..self.num_vertices] {
+        for p in &mut self.vertices[0..self.num_vertices] {
-            *v = iso * *v;
+            *p = iso * *p;
         }
     }
 
@@ -63,6 +76,140 @@ impl PolyhedronFace {
         face2: &PolyhedronFace,
         pos21: &Isometry<f32>,
         manifold: &mut ContactManifold,
+    ) {
+        match (face1.num_vertices, face2.num_vertices) {
+            (2, 2) => Self::contacts_edge_edge(
+                prediction_distance,
+                face1,
+                sep_axis1,
+                face2,
+                pos21,
+                manifold,
+            ),
+            _ => Self::contacts_face_face(
+                prediction_distance,
+                face1,
+                sep_axis1,
+                face2,
+                pos21,
+                manifold,
+            ),
+        }
+    }
+
+    fn contacts_edge_edge(
+        prediction_distance: f32,
+        face1: &PolyhedronFace,
+        sep_axis1: &Vector<f32>,
+        face2: &PolyhedronFace,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        // Project the faces to a 2D plane for contact clipping.
+        // The plane they are projected onto has normal sep_axis1
+        // and contains the origin (this is numerically OK because
+        // we are not working in world-space here).
+        let basis = sep_axis1.orthonormal_basis();
+        let projected_edge1 = [
+            Point2::new(
+                face1.vertices[0].coords.dot(&basis[0]),
+                face1.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[1].coords.dot(&basis[0]),
+                face1.vertices[1].coords.dot(&basis[1]),
+            ),
+        ];
+        let projected_edge2 = [
+            Point2::new(
+                face2.vertices[0].coords.dot(&basis[0]),
+                face2.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[1].coords.dot(&basis[0]),
+                face2.vertices[1].coords.dot(&basis[1]),
+            ),
+        ];
+
+        let tangent1 =
+            (projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon());
+        let tangent2 =
+            (projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon());
+
+        // TODO: not sure what the best value for eps is.
+        // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small.
+        if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) {
+            let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8;
+
+            if !parallel {
+                let seg1 = (&projected_edge1[0], &projected_edge1[1]);
+                let seg2 = (&projected_edge2[0], &projected_edge2[1]);
+                let (loc1, loc2) =
+                    ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2);
+
+                // Found a contact between the two edges.
+                let bcoords1 = loc1.barycentric_coordinates();
+                let bcoords2 = loc2.barycentric_coordinates();
+
+                let edge1 = (face1.vertices[0], face1.vertices[1]);
+                let edge2 = (face2.vertices[0], face2.vertices[1]);
+                let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1];
+                let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1];
+                let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+                if dist <= prediction_distance {
+                    manifold.points.push(Contact {
+                        local_p1,
+                        local_p2: pos21 * local_p2,
+                        impulse: 0.0,
+                        tangent_impulse: Contact::zero_tangent_impulse(),
+                        fid1: face1.eids[0],
+                        fid2: face2.eids[0],
+                        dist,
+                    });
+                }
+
+                return;
+            }
+        }
+
+        // The lines are parallel so we are having a conformal contact.
+        // Let's use a range-based clipping to extract two contact points.
+        // TODO: would it be better and/or more efficient to do the
+        //clipping in 2D?
+        if let Some(clips) = geometry::clip_segments(
+            (face1.vertices[0], face1.vertices[1]),
+            (face2.vertices[0], face2.vertices[1]),
+        ) {
+            manifold.points.push(Contact {
+                local_p1: (clips.0).0,
+                local_p2: pos21 * (clips.0).1,
+                impulse: 0.0,
+                tangent_impulse: Contact::zero_tangent_impulse(),
+                fid1: 0, // FIXME
+                fid2: 0, // FIXME
+                dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1),
+            });
+
+            manifold.points.push(Contact {
+                local_p1: (clips.1).0,
+                local_p2: pos21 * (clips.1).1,
+                impulse: 0.0,
+                tangent_impulse: Contact::zero_tangent_impulse(),
+                fid1: 0, // FIXME
+                fid2: 0, // FIXME
+                dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1),
+            });
+        }
+    }
+
+    fn contacts_face_face(
+        prediction_distance: f32,
+        face1: &PolyhedronFace,
+        sep_axis1: &Vector<f32>,
+        face2: &PolyhedronFace,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
     ) {
         // Project the faces to a 2D plane for contact clipping.
         // The plane they are projected onto has normal sep_axis1
@@ -242,8 +389,6 @@ impl PolyhedronFace {
 /// Compute the barycentric coordinates of the intersection between the two given lines.
 /// Returns `None` if the lines are parallel.
 fn closest_points_line2d(edge1: [Point2<f32>; 2], edge2: [Point2<f32>; 2]) -> Option<(f32, f32)> {
-    use approx::AbsDiffEq;
-
     // Inspired by Real-time collision detection by Christer Ericson.
     let dir1 = edge1[1] - edge1[0];
     let dir2 = edge2[1] - edge2[0];

src/geometry/proximity_detector/ball_convex_proximity_detector.rs

@@ -1,27 +1,19 @@
 use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
-use crate::geometry::{Ball, Proximity, Shape};
+use crate::geometry::{Ball, Proximity};
 use crate::math::Isometry;
 use ncollide::query::PointQuery;
 
 pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
-    if let Shape::Ball(ball1) = ctxt.shape1 {
+    if let Some(ball1) = ctxt.shape1.as_ball() {
-        match ctxt.shape2 {
+        do_detect_proximity(ctxt.shape2, ball1, &ctxt)
-            Shape::Triangle(tri2) => do_detect_proximity(tri2, ball1, &ctxt),
+    } else if let Some(ball2) = ctxt.shape2.as_ball() {
-            Shape::Cuboid(cube2) => do_detect_proximity(cube2, ball1, &ctxt),
+        do_detect_proximity(ctxt.shape1, ball2, &ctxt)
-            _ => unimplemented!(),
-        }
-    } else if let Shape::Ball(ball2) = ctxt.shape2 {
-        match ctxt.shape1 {
-            Shape::Triangle(tri1) => do_detect_proximity(tri1, ball2, &ctxt),
-            Shape::Cuboid(cube1) => do_detect_proximity(cube1, ball2, &ctxt),
-            _ => unimplemented!(),
-        }
     } else {
         panic!("Invalid shape types provide.")
     }
 }
 
-fn do_detect_proximity<P: PointQuery<f32>>(
+fn do_detect_proximity<P: ?Sized + PointQuery<f32>>(
     point_query1: &P,
     ball2: &Ball,
     ctxt: &PrimitiveProximityDetectionContext,

src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs

@@ -1,10 +1,10 @@
 use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
-use crate::geometry::{sat, Proximity, Shape};
+use crate::geometry::{sat, Proximity};
 use crate::math::Isometry;
 use ncollide::shape::Cuboid;
 
 pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
-    if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+    if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) {
         detect_proximity(
             ctxt.prediction_distance,
             cube1,

src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs

@@ -1,11 +1,11 @@
 use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
-use crate::geometry::{sat, Cuboid, Proximity, Shape, Triangle};
+use crate::geometry::{sat, Cuboid, Proximity, Triangle};
 use crate::math::Isometry;
 
 pub fn detect_proximity_cuboid_triangle(
     ctxt: &mut PrimitiveProximityDetectionContext,
 ) -> Proximity {
-    if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) {
+    if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) {
         detect_proximity(
             ctxt.prediction_distance,
             cube1,
@@ -13,7 +13,9 @@ pub fn detect_proximity_cuboid_triangle(
             triangle2,
             ctxt.position2,
         )
-    } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+    } else if let (Some(triangle1), Some(cube2)) =
+        (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid())
+    {
         detect_proximity(
             ctxt.prediction_distance,
             cube2,

src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs

@@ -1,21 +1,24 @@
+#![allow(dead_code)]
+
 use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
-use crate::geometry::{sat, Polygon, Proximity, Shape};
+use crate::geometry::{sat, Polygon, Proximity};
 use crate::math::Isometry;
 
 pub fn detect_proximity_polygon_polygon(
-    ctxt: &mut PrimitiveProximityDetectionContext,
+    _ctxt: &mut PrimitiveProximityDetectionContext,
 ) -> Proximity {
-    if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
+    unimplemented!()
-        detect_proximity(
+    // if let (Some(polygon1), Some(polygon2)) = (ctxt.shape1.as_polygon(), ctxt.shape2.as_polygon()) {
-            ctxt.prediction_distance,
+    //     detect_proximity(
-            polygon1,
+    //         ctxt.prediction_distance,
-            &ctxt.position1,
+    //         polygon1,
-            polygon2,
+    //         &ctxt.position1,
-            &ctxt.position2,
+    //         polygon2,
-        )
+    //         &ctxt.position2,
-    } else {
+    //     )
-        unreachable!()
+    // } else {
-    }
+    //     unreachable!()
+    // }
 }
 
 fn detect_proximity<'a>(

src/geometry/proximity_detector/proximity_detector.rs

@@ -120,8 +120,8 @@ pub struct PrimitiveProximityDetectionContext<'a> {
     pub prediction_distance: f32,
     pub collider1: &'a Collider,
     pub collider2: &'a Collider,
-    pub shape1: &'a Shape,
+    pub shape1: &'a dyn Shape,
-    pub shape2: &'a Shape,
+    pub shape2: &'a dyn Shape,
     pub position1: &'a Isometry<f32>,
     pub position2: &'a Isometry<f32>,
     pub workspace: Option<&'a mut (dyn Any + Send + Sync)>,
@@ -132,8 +132,8 @@ pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> {
     pub prediction_distance: f32,
     pub colliders1: [&'a Collider; SIMD_WIDTH],
     pub colliders2: [&'a Collider; SIMD_WIDTH],
-    pub shapes1: [&'a Shape; SIMD_WIDTH],
+    pub shapes1: [&'a dyn Shape; SIMD_WIDTH],
-    pub shapes2: [&'a Shape; SIMD_WIDTH],
+    pub shapes2: [&'a dyn Shape; SIMD_WIDTH],
     pub positions1: &'a Isometry<SimdFloat>,
     pub positions2: &'a Isometry<SimdFloat>,
     pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>],

src/geometry/proximity_detector/proximity_dispatcher.rs

@@ -2,7 +2,7 @@ use crate::geometry::proximity_detector::{
     PrimitiveProximityDetector, ProximityDetector, ProximityPhase,
     TrimeshShapeProximityDetectorWorkspace,
 };
-use crate::geometry::Shape;
+use crate::geometry::ShapeType;
 use std::any::Any;
 
 /// Trait implemented by structures responsible for selecting a collision-detection algorithm
@@ -11,8 +11,8 @@ pub trait ProximityDispatcher {
     /// Select the proximity detection algorithm for the given pair of primitive shapes.
     fn dispatch_primitives(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (
         PrimitiveProximityDetector,
         Option<Box<dyn Any + Send + Sync>>,
@@ -20,8 +20,8 @@ pub trait ProximityDispatcher {
     /// Select the proximity detection algorithm for the given pair of non-primitive shapes.
     fn dispatch(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>);
 }
 
@@ -31,14 +31,14 @@ pub struct DefaultProximityDispatcher;
 impl ProximityDispatcher for DefaultProximityDispatcher {
     fn dispatch_primitives(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (
         PrimitiveProximityDetector,
         Option<Box<dyn Any + Send + Sync>>,
     ) {
         match (shape1, shape2) {
-            (Shape::Ball(_), Shape::Ball(_)) => (
+            (ShapeType::Ball, ShapeType::Ball) => (
                 PrimitiveProximityDetector {
                     #[cfg(feature = "simd-is-enabled")]
                     detect_proximity_simd: super::detect_proximity_ball_ball_simd,
@@ -47,56 +47,56 @@ impl ProximityDispatcher for DefaultProximityDispatcher {
                 },
                 None,
             ),
-            (Shape::Cuboid(_), Shape::Cuboid(_)) => (
+            (ShapeType::Cuboid, ShapeType::Cuboid) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_cuboid_cuboid,
                     ..PrimitiveProximityDetector::default()
                 },
                 None,
             ),
-            (Shape::Polygon(_), Shape::Polygon(_)) => (
+            (ShapeType::Polygon, ShapeType::Polygon) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_polygon_polygon,
                     ..PrimitiveProximityDetector::default()
                 },
                 None,
             ),
-            (Shape::Triangle(_), Shape::Ball(_)) => (
+            (ShapeType::Triangle, ShapeType::Ball) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_ball_convex,
                     ..PrimitiveProximityDetector::default()
                 },
                 None,
             ),
-            (Shape::Ball(_), Shape::Triangle(_)) => (
+            (ShapeType::Ball, ShapeType::Triangle) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_ball_convex,
                     ..PrimitiveProximityDetector::default()
                 },
                 None,
             ),
-            (Shape::Cuboid(_), Shape::Ball(_)) => (
+            (ShapeType::Cuboid, ShapeType::Ball) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_ball_convex,
                     ..PrimitiveProximityDetector::default()
                 },
                 None,
             ),
-            (Shape::Ball(_), Shape::Cuboid(_)) => (
+            (ShapeType::Ball, ShapeType::Cuboid) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_ball_convex,
                     ..PrimitiveProximityDetector::default()
                 },
                 None,
             ),
-            (Shape::Triangle(_), Shape::Cuboid(_)) => (
+            (ShapeType::Triangle, ShapeType::Cuboid) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_cuboid_triangle,
                     ..PrimitiveProximityDetector::default()
                 },
                 None,
             ),
-            (Shape::Cuboid(_), Shape::Triangle(_)) => (
+            (ShapeType::Cuboid, ShapeType::Triangle) => (
                 PrimitiveProximityDetector {
                     detect_proximity: super::detect_proximity_cuboid_triangle,
                     ..PrimitiveProximityDetector::default()
@@ -109,18 +109,18 @@ impl ProximityDispatcher for DefaultProximityDispatcher {
 
     fn dispatch(
         &self,
-        shape1: &Shape,
+        shape1: ShapeType,
-        shape2: &Shape,
+        shape2: ShapeType,
     ) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>) {
         match (shape1, shape2) {
-            (Shape::Trimesh(_), _) => (
+            (ShapeType::Trimesh, _) => (
                 ProximityPhase::NearPhase(ProximityDetector {
                     detect_proximity: super::detect_proximity_trimesh_shape,
                     ..ProximityDetector::default()
                 }),
                 Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())),
             ),
-            (_, Shape::Trimesh(_)) => (
+            (_, ShapeType::Trimesh) => (
                 ProximityPhase::NearPhase(ProximityDetector {
                     detect_proximity: super::detect_proximity_trimesh_shape,
                     ..ProximityDetector::default()

src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs

@@ -1,7 +1,7 @@
 use crate::geometry::proximity_detector::{
     PrimitiveProximityDetectionContext, ProximityDetectionContext,
 };
-use crate::geometry::{Collider, Proximity, Shape, Trimesh};
+use crate::geometry::{Collider, Proximity, ShapeType, Trimesh};
 use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
 
 pub struct TrimeshShapeProximityDetectorWorkspace {
@@ -24,9 +24,9 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P
     let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
     let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
 
-    if let Shape::Trimesh(trimesh1) = collider1.shape() {
+    if let Some(trimesh1) = collider1.shape().as_trimesh() {
         do_detect_proximity(trimesh1, collider1, collider2, ctxt)
-    } else if let Shape::Trimesh(trimesh2) = collider2.shape() {
+    } else if let Some(trimesh2) = collider2.shape().as_trimesh() {
         do_detect_proximity(trimesh2, collider2, collider1, ctxt)
     } else {
         panic!("Invalid shape types provided.")
@@ -83,6 +83,7 @@ fn do_detect_proximity(
     let new_interferences = &workspace.interferences;
     let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
     let mut best_proximity = Proximity::Disjoint;
+    let shape_type2 = collider2.shape().shape_type();
 
     for triangle_id in new_interferences.iter() {
         if *triangle_id >= trimesh1.num_triangles() {
@@ -107,10 +108,10 @@ fn do_detect_proximity(
             };
         }
 
-        let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id));
+        let triangle1 = trimesh1.triangle(*triangle_id);
         let (proximity_detector, mut workspace2) = ctxt
             .dispatcher
-            .dispatch_primitives(&triangle1, collider2.shape());
+            .dispatch_primitives(ShapeType::Triangle, shape_type2);
 
         let mut ctxt2 = PrimitiveProximityDetectionContext {
             prediction_distance: ctxt.prediction_distance,

src/geometry/round_cylinder.rs

@@ -0,0 +1,107 @@
+use crate::geometry::Cylinder;
+use crate::math::{Isometry, Point, Vector};
+use na::Unit;
+use ncollide::query::{
+    algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection,
+};
+use ncollide::shape::{FeatureId, SupportMap};
+
+/// A rounded cylinder.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug)]
+pub struct RoundCylinder {
+    /// The cylinder being rounded.
+    pub cylinder: Cylinder,
+    /// The rounding radius.
+    pub border_radius: f32,
+}
+
+impl RoundCylinder {
+    /// Create sa new cylinder where all its edges and vertices are rounded by a radius of `radius`.
+    ///
+    /// This is done by applying a dilation of the given radius to the cylinder.
+    pub fn new(half_height: f32, radius: f32, border_radius: f32) -> Self {
+        Self {
+            cylinder: Cylinder::new(half_height, radius),
+            border_radius,
+        }
+    }
+}
+
+impl SupportMap<f32> for RoundCylinder {
+    fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
+        self.local_support_point_toward(&Unit::new_normalize(*dir))
+    }
+
+    fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
+        self.cylinder.local_support_point_toward(dir) + **dir * self.border_radius
+    }
+
+    fn support_point(&self, transform: &Isometry<f32>, dir: &Vector<f32>) -> Point<f32> {
+        let local_dir = transform.inverse_transform_vector(dir);
+        transform * self.local_support_point(&local_dir)
+    }
+
+    fn support_point_toward(
+        &self,
+        transform: &Isometry<f32>,
+        dir: &Unit<Vector<f32>>,
+    ) -> Point<f32> {
+        let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir));
+        transform * self.local_support_point_toward(&local_dir)
+    }
+}
+
+impl RayCast<f32> for RoundCylinder {
+    fn toi_and_normal_with_ray(
+        &self,
+        m: &Isometry<f32>,
+        ray: &Ray<f32>,
+        max_toi: f32,
+        solid: bool,
+    ) -> Option<RayIntersection<f32>> {
+        let ls_ray = ray.inverse_transform_by(m);
+
+        ncollide::query::ray_intersection_with_support_map_with_params(
+            &Isometry::identity(),
+            self,
+            &mut VoronoiSimplex::new(),
+            &ls_ray,
+            max_toi,
+            solid,
+        )
+        .map(|mut res| {
+            res.normal = m * res.normal;
+            res
+        })
+    }
+}
+
+// TODO: if PointQuery had a `project_point_with_normal` method, we could just
+// call this and adjust the projected point accordingly.
+impl PointQuery<f32> for RoundCylinder {
+    #[inline]
+    fn project_point(
+        &self,
+        m: &Isometry<f32>,
+        point: &Point<f32>,
+        solid: bool,
+    ) -> PointProjection<f32> {
+        ncollide::query::point_projection_on_support_map(
+            m,
+            self,
+            &mut VoronoiSimplex::new(),
+            point,
+            solid,
+        )
+    }
+
+    #[inline]
+    fn project_point_with_feature(
+        &self,
+        m: &Isometry<f32>,
+        point: &Point<f32>,
+    ) -> (PointProjection<f32>, FeatureId) {
+        (self.project_point(m, point, false), FeatureId::Unknown)
+    }
+}

src/geometry/sat.rs

@@ -1,9 +1,22 @@
-use crate::geometry::{cuboid, Cuboid, Polygon, Triangle};
+use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle};
 use crate::math::{Isometry, Point, Vector, DIM};
 use crate::utils::WSign;
 use na::Unit;
-use ncollide::shape::{Segment, SupportMap};
+use ncollide::shape::SupportMap;
 
+#[allow(dead_code)]
+pub fn support_map_support_map_compute_separation(
+    sm1: &impl SupportMap<f32>,
+    sm2: &impl SupportMap<f32>,
+    m12: &Isometry<f32>,
+    dir1: &Unit<Vector<f32>>,
+) -> f32 {
+    let p1 = sm1.local_support_point_toward(dir1);
+    let p2 = sm2.support_point_toward(m12, &-*dir1);
+    (p2 - p1).dot(dir1)
+}
+
+#[allow(dead_code)]
 pub fn polygon_polygon_compute_separation_features(
     p1: &Polygon,
     p2: &Polygon,
@@ -58,8 +71,8 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway(
     let y2 = pos12 * Vector::y();
     let z2 = pos12 * Vector::z();
 
-    // We have 3 * 3 = 9 axii to test.
+    // We have 3 * 3 = 9 axes to test.
-    let axii = [
+    let axes = [
         // Vector::{x, y ,z}().cross(y2)
         Vector::new(0.0, -x2.z, x2.y),
         Vector::new(x2.z, 0.0, -x2.x),
@@ -74,7 +87,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway(
         Vector::new(-z2.y, z2.x, 0.0),
     ];
 
-    for axis1 in &axii {
+    for axis1 in &axes {
         let norm1 = axis1.norm();
         if norm1 > f32::default_epsilon() {
             let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line(
@@ -127,7 +140,6 @@ pub fn cuboid_cuboid_find_local_separating_normal_oneway(
  *
  *
  */
-
 #[cfg(feature = "dim3")]
 pub fn cube_support_map_compute_separation_wrt_local_line<S: SupportMap<f32>>(
     cube1: &Cuboid,
@@ -149,7 +161,7 @@ pub fn cube_support_map_compute_separation_wrt_local_line<S: SupportMap<f32>>(
 pub fn cube_support_map_find_local_separating_edge_twoway(
     cube1: &Cuboid,
     shape2: &impl SupportMap<f32>,
-    axii: &[Vector<f32>],
+    axes: &[Vector<f32>],
     pos12: &Isometry<f32>,
     pos21: &Isometry<f32>,
 ) -> (f32, Vector<f32>) {
@@ -157,7 +169,7 @@ pub fn cube_support_map_find_local_separating_edge_twoway(
     let mut best_separation = -std::f32::MAX;
     let mut best_dir = Vector::zeros();
 
-    for axis1 in axii {
+    for axis1 in axes {
         if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) {
             let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line(
                 cube1, shape2, pos12, pos21, &axis1,
@@ -184,8 +196,8 @@ pub fn cube_triangle_find_local_separating_edge_twoway(
     let y2 = pos12 * (triangle2.c - triangle2.b);
     let z2 = pos12 * (triangle2.a - triangle2.c);
 
-    // We have 3 * 3 = 3 axii to test.
+    // We have 3 * 3 = 3 axes to test.
-    let axii = [
+    let axes = [
         // Vector::{x, y ,z}().cross(y2)
         Vector::new(0.0, -x2.z, x2.y),
         Vector::new(x2.z, 0.0, -x2.x),
@@ -200,26 +212,26 @@ pub fn cube_triangle_find_local_separating_edge_twoway(
         Vector::new(-z2.y, z2.x, 0.0),
     ];
 
-    cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21)
+    cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axes, pos12, pos21)
 }
 
 #[cfg(feature = "dim3")]
 pub fn cube_segment_find_local_separating_edge_twoway(
     cube1: &Cuboid,
-    segment2: &Segment<f32>,
+    segment2: &Segment,
     pos12: &Isometry<f32>,
     pos21: &Isometry<f32>,
 ) -> (f32, Vector<f32>) {
     let x2 = pos12 * (segment2.b - segment2.a);
 
-    let axii = [
+    let axes = [
         // Vector::{x, y ,z}().cross(y2)
         Vector::new(0.0, -x2.z, x2.y),
         Vector::new(x2.z, 0.0, -x2.x),
         Vector::new(-x2.y, x2.x, 0.0),
     ];
 
-    cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21)
+    cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axes, pos12, pos21)
 }
 
 pub fn cube_support_map_find_local_separating_normal_oneway<S: SupportMap<f32>>(
@@ -286,9 +298,72 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway(
 
 #[cfg(feature = "dim2")]
 pub fn segment_cuboid_find_local_separating_normal_oneway(
-    segment1: &Segment<f32>,
+    segment1: &Segment,
     shape2: &Cuboid,
     pos12: &Isometry<f32>,
 ) -> (f32, Vector<f32>) {
     point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12)
 }
+
+/*
+ * Capsules
+ */
+#[cfg(feature = "dim3")]
+pub fn triangle_segment_find_local_separating_normal_oneway(
+    triangle1: &Triangle,
+    segment2: &Segment,
+    m12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    if let Some(dir) = triangle1.normal() {
+        let p2a = segment2.support_point_toward(m12, &-dir);
+        let p2b = segment2.support_point_toward(m12, &dir);
+        let sep_a = (p2a - triangle1.a).dot(&dir);
+        let sep_b = -(p2b - triangle1.a).dot(&dir);
+
+        if sep_a >= sep_b {
+            (sep_a, *dir)
+        } else {
+            (sep_b, -*dir)
+        }
+    } else {
+        (-f32::MAX, Vector::zeros())
+    }
+}
+
+#[cfg(feature = "dim3")]
+pub fn segment_triangle_find_local_separating_edge(
+    segment1: &Segment,
+    triangle2: &Triangle,
+    pos12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    let x2 = pos12 * (triangle2.b - triangle2.a);
+    let y2 = pos12 * (triangle2.c - triangle2.b);
+    let z2 = pos12 * (triangle2.a - triangle2.c);
+    let dir1 = segment1.scaled_direction();
+
+    let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)];
+    let axes1 = [
+        crosses1[0],
+        crosses1[1],
+        crosses1[2],
+        -crosses1[0],
+        -crosses1[1],
+        -crosses1[2],
+    ];
+    let mut max_separation = -f32::MAX;
+    let mut sep_dir = axes1[0];
+
+    for axis1 in &axes1 {
+        if let Some(axis1) = Unit::try_new(*axis1, 0.0) {
+            let sep =
+                support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1);
+
+            if sep > max_separation {
+                max_separation = sep;
+                sep_dir = *axis1;
+            }
+        }
+    }
+
+    (max_separation, sep_dir)
+}

src/geometry/shape.rs

@@ -0,0 +1,390 @@
+use crate::dynamics::MassProperties;
+use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh};
+use crate::math::Isometry;
+use downcast_rs::{impl_downcast, DowncastSync};
+#[cfg(feature = "serde-serialize")]
+use erased_serde::Serialize;
+use ncollide::bounding_volume::{HasBoundingVolume, AABB};
+use ncollide::query::{PointQuery, RayCast};
+use num::Zero;
+use num_derive::FromPrimitive;
+#[cfg(feature = "dim3")]
+use {
+    crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, RoundCylinder},
+    ncollide::bounding_volume::BoundingVolume,
+};
+
+#[derive(Copy, Clone, Debug, FromPrimitive)]
+/// Enum representing the type of a shape.
+pub enum ShapeType {
+    /// A ball shape.
+    Ball = 1,
+    /// A convex polygon shape.
+    Polygon,
+    /// A cuboid shape.
+    Cuboid,
+    /// A capsule shape.
+    Capsule,
+    /// A segment shape.
+    Segment,
+    /// A triangle shape.
+    Triangle,
+    /// A triangle mesh shape.
+    Trimesh,
+    /// A heightfield shape.
+    HeightField,
+    #[cfg(feature = "dim3")]
+    /// A cylindrical shape.
+    Cylinder,
+    #[cfg(feature = "dim3")]
+    /// A cylindrical shape.
+    Cone,
+    // /// A custom shape type.
+    // Custom(u8),
+    // /// A cuboid with rounded corners.
+    // RoundedCuboid,
+    // /// A triangle with rounded corners.
+    // RoundedTriangle,
+    // /// A triangle-mesh with rounded corners.
+    // RoundedTrimesh,
+    // /// An heightfield with rounded corners.
+    // RoundedHeightField,
+    /// A cylinder with rounded corners.
+    #[cfg(feature = "dim3")]
+    RoundCylinder,
+    // /// A cone with rounded corners.
+    // RoundedCone,
+}
+
+/// Trait implemented by shapes usable by Rapier.
+pub trait Shape: RayCast<f32> + PointQuery<f32> + DowncastSync {
+    /// Convert this shape as a serializable entity.
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        None
+    }
+
+    /// Computes the AABB of this shape.
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32>;
+
+    /// Compute the mass-properties of this shape given its uniform density.
+    fn mass_properties(&self, density: f32) -> MassProperties;
+
+    /// Gets the type tag of this shape.
+    fn shape_type(&self) -> ShapeType;
+
+    /// Converts this shape to a polygonal feature-map, if it is one.
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        None
+    }
+
+    // fn as_rounded(&self) -> Option<&Rounded<Box<AnyShape>>> {
+    //     None
+    // }
+}
+
+impl_downcast!(sync Shape);
+
+impl dyn Shape {
+    /// Converts this abstract shape to a ball, if it is one.
+    pub fn as_ball(&self) -> Option<&Ball> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a cuboid, if it is one.
+    pub fn as_cuboid(&self) -> Option<&Cuboid> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a capsule, if it is one.
+    pub fn as_capsule(&self) -> Option<&Capsule> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a triangle, if it is one.
+    pub fn as_triangle(&self) -> Option<&Triangle> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a triangle mesh, if it is one.
+    pub fn as_trimesh(&self) -> Option<&Trimesh> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a heightfield, if it is one.
+    pub fn as_heightfield(&self) -> Option<&HeightField> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a cylinder, if it is one.
+    #[cfg(feature = "dim3")]
+    pub fn as_cylinder(&self) -> Option<&Cylinder> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a cone, if it is one.
+    #[cfg(feature = "dim3")]
+    pub fn as_cone(&self) -> Option<&Cone> {
+        self.downcast_ref()
+    }
+
+    /// Converts this abstract shape to a cone, if it is one.
+    #[cfg(feature = "dim3")]
+    pub fn as_round_cylinder(&self) -> Option<&RoundCylinder> {
+        self.downcast_ref()
+    }
+}
+
+impl Shape for Ball {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, density: f32) -> MassProperties {
+        MassProperties::from_ball(density, self.radius)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Ball
+    }
+}
+
+// impl Shape for Polygon {
+//     #[cfg(feature = "serde-serialize")]
+//     fn as_serialize(&self) -> Option<&dyn Serialize> {
+//         Some(self as &dyn Serialize)
+//     }
+//
+//     fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+//         self.aabb(position)
+//     }
+//
+//     fn mass_properties(&self, _density: f32) -> MassProperties {
+//         unimplemented!()
+//     }
+//
+//     fn shape_type(&self) -> ShapeType {
+//         ShapeType::Polygon
+//     }
+// }
+
+impl Shape for Cuboid {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, density: f32) -> MassProperties {
+        MassProperties::from_cuboid(density, self.half_extents)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Cuboid
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+}
+
+impl Shape for Capsule {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.aabb(position)
+    }
+
+    fn mass_properties(&self, density: f32) -> MassProperties {
+        MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Capsule
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((&self.segment as &dyn PolygonalFeatureMap, self.radius))
+    }
+}
+
+impl Shape for Triangle {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, _density: f32) -> MassProperties {
+        MassProperties::zero()
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Triangle
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+}
+
+impl Shape for Segment {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, _density: f32) -> MassProperties {
+        MassProperties::zero()
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Segment
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+}
+
+impl Shape for Trimesh {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.aabb(position)
+    }
+
+    fn mass_properties(&self, _density: f32) -> MassProperties {
+        MassProperties::zero()
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Trimesh
+    }
+}
+
+impl Shape for HeightField {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, _density: f32) -> MassProperties {
+        MassProperties::zero()
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::HeightField
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl Shape for Cylinder {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, density: f32) -> MassProperties {
+        MassProperties::from_cylinder(density, self.half_height, self.radius)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Cylinder
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl Shape for Cone {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.bounding_volume(position)
+    }
+
+    fn mass_properties(&self, density: f32) -> MassProperties {
+        MassProperties::from_cone(density, self.half_height, self.radius)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::Cone
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((self as &dyn PolygonalFeatureMap, 0.0))
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl Shape for RoundCylinder {
+    #[cfg(feature = "serde-serialize")]
+    fn as_serialize(&self) -> Option<&dyn Serialize> {
+        Some(self as &dyn Serialize)
+    }
+
+    fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        self.cylinder
+            .compute_aabb(position)
+            .loosened(self.border_radius)
+    }
+
+    fn mass_properties(&self, density: f32) -> MassProperties {
+        // We ignore the margin here.
+        self.cylinder.mass_properties(density)
+    }
+
+    fn shape_type(&self) -> ShapeType {
+        ShapeType::RoundCylinder
+    }
+
+    #[cfg(feature = "dim3")]
+    fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> {
+        Some((
+            &self.cylinder as &dyn PolygonalFeatureMap,
+            self.border_radius,
+        ))
+    }
+}

src/geometry/trimesh.rs

@@ -1,13 +1,9 @@
-use crate::geometry::{Triangle, WQuadtree};
+use crate::geometry::{PointProjection, Ray, RayIntersection, Triangle, WQuadtree};
 use crate::math::{Isometry, Point};
 use na::Point3;
 use ncollide::bounding_volume::{HasBoundingVolume, AABB};
-
+use ncollide::query::{PointQuery, RayCast};
-#[cfg(feature = "dim3")]
+use ncollide::shape::FeatureId;
-use {
-    crate::geometry::{Ray, RayIntersection},
-    ncollide::query::RayCast,
-};
 
 #[derive(Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -110,6 +106,41 @@ impl Trimesh {
     }
 }
 
+impl PointQuery<f32> for Trimesh {
+    fn project_point(&self, _m: &Isometry<f32>, _pt: &Point<f32>, _solid: bool) -> PointProjection {
+        // TODO
+        unimplemented!()
+    }
+
+    fn project_point_with_feature(
+        &self,
+        _m: &Isometry<f32>,
+        _pt: &Point<f32>,
+    ) -> (PointProjection, FeatureId) {
+        // TODO
+        unimplemented!()
+    }
+}
+
+#[cfg(feature = "dim2")]
+impl RayCast<f32> for Trimesh {
+    fn toi_and_normal_with_ray(
+        &self,
+        _m: &Isometry<f32>,
+        _ray: &Ray,
+        _max_toi: f32,
+        _solid: bool,
+    ) -> Option<RayIntersection> {
+        // TODO
+        None
+    }
+
+    fn intersects_ray(&self, _m: &Isometry<f32>, _ray: &Ray, _max_toi: f32) -> bool {
+        // TODO
+        false
+    }
+}
+
 #[cfg(feature = "dim3")]
 impl RayCast<f32> for Trimesh {
     fn toi_and_normal_with_ray(

src/geometry/user_callbacks.rs

@@ -0,0 +1,57 @@
+use crate::dynamics::RigidBody;
+use crate::geometry::{Collider, SolverFlags};
+
+/// Context given to custom collision filters to filter-out collisions.
+pub struct PairFilterContext<'a> {
+    /// The first collider involved in the potential collision.
+    pub rigid_body1: &'a RigidBody,
+    /// The first collider involved in the potential collision.
+    pub rigid_body2: &'a RigidBody,
+    /// The first collider involved in the potential collision.
+    pub collider1: &'a Collider,
+    /// The first collider involved in the potential collision.
+    pub collider2: &'a Collider,
+}
+
+/// User-defined filter for potential contact pairs detected by the broad-phase.
+///
+/// This can be used to apply custom logic in order to decide whether two colliders
+/// should have their contact computed by the narrow-phase, and if these contact
+/// should be solved by the constraints solver
+pub trait ContactPairFilter: Send + Sync {
+    /// Applies the contact pair filter.
+    ///
+    /// Note that using a contact pair filter will replace the default contact filtering
+    /// which consists of preventing contact computation between two non-dynamic bodies.
+    ///
+    /// This filtering method is called after taking into account the colliders collision groups.
+    ///
+    /// If this returns `None`, then the narrow-phase will ignore this contact pair and
+    /// not compute any contact manifolds for it.
+    /// If this returns `Some`, then the narrow-phase will compute contact manifolds for
+    /// this pair of colliders, and configure them with the returned solver flags. For
+    /// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts
+    /// will be taken into account by the constraints solver. If this returns
+    /// `Some(SolverFlags::empty())` then the constraints solver will ignore these
+    /// contacts.
+    fn filter_contact_pair(&self, context: &PairFilterContext) -> Option<SolverFlags>;
+}
+
+/// User-defined filter for potential proximity pairs detected by the broad-phase.
+///
+/// This can be used to apply custom logic in order to decide whether two colliders
+/// should have their proximity computed by the narrow-phase.
+pub trait ProximityPairFilter: Send + Sync {
+    /// Applies the proximity pair filter.
+    ///
+    /// Note that using a proximity pair filter will replace the default proximity filtering
+    /// which consists of preventing proximity computation between two non-dynamic bodies.
+    ///
+    /// This filtering method is called after taking into account the colliders collision groups.
+    ///
+    /// If this returns `false`, then the narrow-phase will ignore this pair and
+    /// not compute any proximity information for it.
+    /// If this return `true` then the narrow-phase will compute proximity
+    /// information for this pair.
+    fn filter_proximity_pair(&self, context: &PairFilterContext) -> bool;
+}

src/geometry/wquadtree.rs

@@ -539,7 +539,7 @@ fn split_indices_wrt_dim<'a>(
     dim: usize,
 ) -> (&'a mut [usize], &'a mut [usize]) {
     let mut icurr = 0;
-    let mut ilast = indices.len() - 1;
+    let mut ilast = indices.len();
 
     // The loop condition we can just do 0..indices.len()
     // instead of the test icurr < ilast because we know
@@ -549,12 +549,39 @@ fn split_indices_wrt_dim<'a>(
         let center = aabbs[i].center();
 
         if center[dim] > split_point[dim] {
-            indices.swap(icurr, ilast);
             ilast -= 1;
+            indices.swap(icurr, ilast);
         } else {
             icurr += 1;
         }
     }
 
-    indices.split_at_mut(icurr)
+    if icurr == 0 || icurr == indices.len() {
+        // We don't want to return one empty set. But
+        // this can happen if all the coordinates along the
+        // given dimension are equal.
+        // In this is the case, we just split in the middle.
+        let half = indices.len() / 2;
+        indices.split_at_mut(half)
+    } else {
+        indices.split_at_mut(icurr)
+    }
+}
+
+#[cfg(test)]
+mod test {
+    use crate::geometry::{WQuadtree, AABB};
+    use crate::math::{Point, Vector};
+
+    #[test]
+    fn multiple_identical_AABB_stack_overflow() {
+        // A stack overflow was caused during the construction of the
+        // WAABB tree with more than four AABB with the same center.
+        let aabb = AABB::new(Point::origin(), Vector::repeat(1.0).into());
+
+        for k in 0..20 {
+            let mut tree = WQuadtree::new();
+            tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0);
+        }
+    }
 }

src/lib.rs

@@ -10,6 +10,7 @@
 
 #![deny(missing_docs)]
 
+pub extern crate crossbeam;
 pub extern crate nalgebra as na;
 #[cfg(feature = "dim2")]
 pub extern crate ncollide2d as ncollide;

src/pipeline/collision_pipeline.rs

@@ -1,7 +1,10 @@
 //! Physics pipeline structures.
 
 use crate::dynamics::{JointSet, RigidBodySet};
-use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase};
+use crate::geometry::{
+    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, NarrowPhase,
+    ProximityPairFilter,
+};
 use crate::pipeline::EventHandler;
 
 /// The collision pipeline, responsible for performing collision detection between colliders.
@@ -40,6 +43,8 @@ impl CollisionPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
+        contact_pair_filter: Option<&dyn ContactPairFilter>,
+        proximity_pair_filter: Option<&dyn ProximityPairFilter>,
         events: &dyn EventHandler,
     ) {
         bodies.maintain_active_set();
@@ -52,8 +57,20 @@ impl CollisionPipeline {
 
         narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
 
-        narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events);
+        narrow_phase.compute_contacts(
-        narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events);
+            prediction_distance,
+            bodies,
+            colliders,
+            contact_pair_filter,
+            events,
+        );
+        narrow_phase.compute_proximities(
+            prediction_distance,
+            bodies,
+            colliders,
+            proximity_pair_filter,
+            events,
+        );
 
         bodies.update_active_set_with_contacts(
             colliders,

src/pipeline/physics_pipeline.rs

@@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
-    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
+    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex,
+    ContactPairFilter, NarrowPhase, ProximityPairFilter,
 };
 use crate::math::Vector;
 use crate::pipeline::EventHandler;
@@ -68,6 +69,8 @@ impl PhysicsPipeline {
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
+        contact_pair_filter: Option<&dyn ContactPairFilter>,
+        proximity_pair_filter: Option<&dyn ProximityPairFilter>,
         events: &dyn EventHandler,
     ) {
         self.counters.step_started();
@@ -112,12 +115,14 @@ impl PhysicsPipeline {
             integration_parameters.prediction_distance,
             bodies,
             colliders,
+            contact_pair_filter,
             events,
         );
         narrow_phase.compute_proximities(
             integration_parameters.prediction_distance,
             bodies,
             colliders,
+            proximity_pair_filter,
             events,
         );
         //        println!("Compute contact time: {}", instant::now() - t);
@@ -285,6 +290,8 @@ mod test {
             &mut bodies,
             &mut colliders,
             &mut joints,
+            None,
+            None,
             &(),
         );
     }
@@ -327,6 +334,8 @@ mod test {
             &mut bodies,
             &mut colliders,
             &mut joints,
+            None,
+            None,
             &(),
         );
     }

src/pipeline/query_pipeline.rs

@@ -1,5 +1,7 @@
 use crate::dynamics::RigidBodySet;
-use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree};
+use crate::geometry::{
+    Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, WQuadtree,
+};
 
 /// A pipeline for performing queries on all the colliders of a scene.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -59,6 +61,7 @@ impl QueryPipeline {
         colliders: &'a ColliderSet,
         ray: &Ray,
         max_toi: f32,
+        groups: InteractionGroups,
     ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
         // TODO: avoid allocation?
         let mut inter = Vec::new();
@@ -69,10 +72,17 @@ impl QueryPipeline {
 
         for handle in inter {
             let collider = &colliders[handle];
-            if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
+            if collider.collision_groups.test(groups) {
-                if inter.toi < best {
+                if let Some(inter) = collider.shape().toi_and_normal_with_ray(
-                    best = inter.toi;
+                    collider.position(),
-                    result = Some((handle, collider, inter));
+                    ray,
+                    max_toi,
+                    true,
+                ) {
+                    if inter.toi < best {
+                        best = inter.toi;
+                        result = Some((handle, collider, inter));
+                    }
                 }
             }
         }
@@ -95,6 +105,7 @@ impl QueryPipeline {
         colliders: &'a ColliderSet,
         ray: &Ray,
         max_toi: f32,
+        groups: InteractionGroups,
         mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
     ) {
         // TODO: avoid allocation?
@@ -103,9 +114,17 @@ impl QueryPipeline {
 
         for handle in inter {
             let collider = &colliders[handle];
-            if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
+
-                if !callback(handle, collider, inter) {
+            if collider.collision_groups.test(groups) {
-                    return;
+                if let Some(inter) = collider.shape().toi_and_normal_with_ray(
+                    collider.position(),
+                    ray,
+                    max_toi,
+                    true,
+                ) {
+                    if !callback(handle, collider, inter) {
+                        return;
+                    }
                 }
             }
         }

src/utils.rs

@@ -19,8 +19,10 @@ use {
 // pub(crate) const COS_10_DEGREES: f32 = 0.98480775301;
 // pub(crate) const COS_45_DEGREES: f32 = 0.70710678118;
 // pub(crate) const SIN_45_DEGREES: f32 = COS_45_DEGREES;
+#[cfg(feature = "dim3")]
+pub(crate) const COS_1_DEGREES: f32 = 0.99984769515;
 pub(crate) const COS_5_DEGREES: f32 = 0.99619469809;
-#[cfg(feature = "dim2")]
+// #[cfg(feature = "dim2")]
 pub(crate) const COS_FRAC_PI_8: f32 = 0.92387953251;
 #[cfg(feature = "dim2")]
 pub(crate) const SIN_FRAC_PI_8: f32 = 0.38268343236;
@@ -91,7 +93,7 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
 
 impl WSign<SimdFloat> for SimdFloat {
     fn copy_sign_to(self, to: SimdFloat) -> SimdFloat {
-        self.simd_copysign(to)
+        to.simd_copysign(self)
     }
 }
 

src_testbed/box2d_backend.rs

@@ -5,7 +5,7 @@ use rapier::counters::Counters;
 use rapier::dynamics::{
     IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
 };
-use rapier::geometry::{Collider, ColliderSet, Shape};
+use rapier::geometry::{Collider, ColliderSet};
 use std::f32;
 
 use wrapped2d::b2;
@@ -167,44 +167,42 @@ impl Box2dWorld {
         fixture_def.is_sensor = collider.is_sensor();
         fixture_def.filter = b2::Filter::new();
 
-        match collider.shape() {
+        let shape = collider.shape();
-            Shape::Ball(b) => {
-                let mut b2_shape = b2::CircleShape::new();
-                b2_shape.set_radius(b.radius);
-                b2_shape.set_position(center);
-                body.create_fixture(&b2_shape, &mut fixture_def);
-            }
-            Shape::Cuboid(c) => {
-                let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
-                body.create_fixture(&b2_shape, &mut fixture_def);
-            }
-            Shape::Polygon(poly) => {
-                let points: Vec<_> = poly
-                    .vertices()
-                    .iter()
-                    .map(|p| collider.position_wrt_parent() * p)
-                    .map(|p| na_vec_to_b2_vec(p.coords))
-                    .collect();
-                let b2_shape = b2::PolygonShape::new_with(&points);
-                body.create_fixture(&b2_shape, &mut fixture_def);
-            }
-            Shape::HeightField(heightfield) => {
-                let mut segments = heightfield.segments();
-                let seg1 = segments.next().unwrap();
-                let mut vertices = vec![
-                    na_vec_to_b2_vec(seg1.a.coords),
-                    na_vec_to_b2_vec(seg1.b.coords),
-                ];
 
-                // TODO: this will not handle holes properly.
+        if let Some(b) = shape.as_ball() {
-                segments.for_each(|seg| {
+            let mut b2_shape = b2::CircleShape::new();
-                    vertices.push(na_vec_to_b2_vec(seg.b.coords));
+            b2_shape.set_radius(b.radius);
-                });
+            b2_shape.set_position(center);
+            body.create_fixture(&b2_shape, &mut fixture_def);
+        } else if let Some(c) = shape.as_cuboid() {
+            let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
+            body.create_fixture(&b2_shape, &mut fixture_def);
+        // } else if let Some(polygon) = shape.as_polygon() {
+        //     let points: Vec<_> = poly
+        //         .vertices()
+        //         .iter()
+        //         .map(|p| collider.position_wrt_parent() * p)
+        //         .map(|p| na_vec_to_b2_vec(p.coords))
+        //         .collect();
+        //     let b2_shape = b2::PolygonShape::new_with(&points);
+        //     body.create_fixture(&b2_shape, &mut fixture_def);
+        } else if let Some(heightfield) = shape.as_heightfield() {
+            let mut segments = heightfield.segments();
+            let seg1 = segments.next().unwrap();
+            let mut vertices = vec![
+                na_vec_to_b2_vec(seg1.a.coords),
+                na_vec_to_b2_vec(seg1.b.coords),
+            ];
 
-                let b2_shape = b2::ChainShape::new_chain(&vertices);
+            // TODO: this will not handle holes properly.
-                body.create_fixture(&b2_shape, &mut fixture_def);
+            segments.for_each(|seg| {
-            }
+                vertices.push(na_vec_to_b2_vec(seg.b.coords));
-            _ => eprintln!("Creating a shape unknown to the Box2d backend."),
+            });
+
+            let b2_shape = b2::ChainShape::new_chain(&vertices);
+            body.create_fixture(&b2_shape, &mut fixture_def);
+        } else {
+            eprintln!("Creating a shape unknown to the Box2d backend.");
         }
     }
 

src_testbed/engine.rs

@@ -9,11 +9,10 @@ use na::Point3;
 use crate::math::Point;
 use crate::objects::ball::Ball;
 use crate::objects::box_node::Box as BoxNode;
-use crate::objects::convex::Convex;
 use crate::objects::heightfield::HeightField;
 use crate::objects::node::{GraphicsNode, Node};
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
-use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
+use rapier::geometry::{Collider, ColliderHandle, ColliderSet};
 //use crate::objects::capsule::Capsule;
 //use crate::objects::convex::Convex;
 //#[cfg(feature = "fluids")]
@@ -26,6 +25,10 @@ use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
 //#[cfg(feature = "fluids")]
 //use crate::objects::FluidRenderingMode;
 use crate::objects::capsule::Capsule;
+#[cfg(feature = "dim3")]
+use crate::objects::cone::Cone;
+#[cfg(feature = "dim3")]
+use crate::objects::cylinder::Cylinder;
 use crate::objects::mesh::Mesh;
 use rand::{Rng, SeedableRng};
 use rand_pcg::Pcg32;
@@ -60,6 +63,7 @@ pub struct GraphicsManager {
     #[cfg(feature = "fluids")]
     boundary2sn: HashMap<BoundaryHandle, FluidNode>,
     b2color: HashMap<RigidBodyHandle, Point3<f32>>,
+    c2color: HashMap<ColliderHandle, Point3<f32>>,
     b2wireframe: HashMap<RigidBodyHandle, bool>,
     #[cfg(feature = "fluids")]
     f2color: HashMap<FluidHandle, Point3<f32>>,
@@ -97,6 +101,7 @@ impl GraphicsManager {
             #[cfg(feature = "fluids")]
             boundary2sn: HashMap::new(),
             b2color: HashMap::new(),
+            c2color: HashMap::new(),
             #[cfg(feature = "fluids")]
             f2color: HashMap::new(),
             ground_color: Point3::new(0.5, 0.5, 0.5),
@@ -183,6 +188,10 @@ impl GraphicsManager {
         }
     }
 
+    pub fn set_collider_initial_color(&mut self, c: ColliderHandle, color: Point3<f32>) {
+        self.c2color.insert(c, color);
+    }
+
     pub fn set_body_wireframe(&mut self, b: RigidBodyHandle, enabled: bool) {
         self.b2wireframe.insert(b, enabled);
 
@@ -321,6 +330,7 @@ impl GraphicsManager {
         let mut new_nodes = Vec::new();
 
         for collider_handle in bodies[handle].colliders() {
+            let color = self.c2color.get(collider_handle).copied().unwrap_or(color);
             let collider = &colliders[*collider_handle];
             self.add_collider(window, *collider_handle, collider, color, &mut new_nodes);
         }
@@ -349,33 +359,44 @@ impl GraphicsManager {
         color: Point3<f32>,
         out: &mut Vec<Node>,
     ) {
-        match collider.shape() {
+        let shape = collider.shape();
-            Shape::Ball(ball) => {
+
-                out.push(Node::Ball(Ball::new(handle, ball.radius, color, window)))
+        if let Some(ball) = shape.as_ball() {
-            }
+            out.push(Node::Ball(Ball::new(handle, ball.radius, color, window)))
-            Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
+        }
-                handle,
+
-                poly.vertices().to_vec(),
+        // Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
-                color,
+        //     handle,
-                window,
+        //     poly.vertices().to_vec(),
-            ))),
+        //     color,
-            Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new(
+        //     window,
+        // ))),
+
+        if let Some(cuboid) = shape.as_cuboid() {
+            out.push(Node::Box(BoxNode::new(
                 handle,
                 cuboid.half_extents,
                 color,
                 window,
-            ))),
+            )))
-            Shape::Capsule(capsule) => {
+        }
-                out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
+
-            }
+        if let Some(capsule) = shape.as_capsule() {
-            Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new(
+            out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
+        }
+
+        if let Some(triangle) = shape.as_triangle() {
+            out.push(Node::Mesh(Mesh::new(
                 handle,
                 vec![triangle.a, triangle.b, triangle.c],
                 vec![Point3::new(0, 1, 2)],
                 color,
                 window,
-            ))),
+            )))
-            Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new(
+        }
+
+        if let Some(trimesh) = shape.as_trimesh() {
+            out.push(Node::Mesh(Mesh::new(
                 handle,
                 trimesh.vertices().to_vec(),
                 trimesh
@@ -385,13 +406,41 @@ impl GraphicsManager {
                     .collect(),
                 color,
                 window,
-            ))),
+            )))
-            Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new(
+        }
+
+        if let Some(heightfield) = shape.as_heightfield() {
+            out.push(Node::HeightField(HeightField::new(
                 handle,
                 heightfield,
                 color,
                 window,
-            ))),
+            )))
+        }
+
+        #[cfg(feature = "dim3")]
+        if let Some(cylinder) = shape
+            .as_cylinder()
+            .or(shape.as_round_cylinder().map(|r| &r.cylinder))
+        {
+            out.push(Node::Cylinder(Cylinder::new(
+                handle,
+                cylinder.half_height,
+                cylinder.radius,
+                color,
+                window,
+            )))
+        }
+
+        #[cfg(feature = "dim3")]
+        if let Some(cone) = shape.as_cone() {
+            out.push(Node::Cone(Cone::new(
+                handle,
+                cone.half_height,
+                cone.radius,
+                color,
+                window,
+            )))
         }
     }
 

src_testbed/nphysics_backend.rs

@@ -12,7 +12,7 @@ use rapier::counters::Counters;
 use rapier::dynamics::{
     IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
 };
-use rapier::geometry::{Collider, ColliderSet, Shape};
+use rapier::geometry::{Collider, ColliderSet};
 use rapier::math::Vector;
 use std::collections::HashMap;
 #[cfg(feature = "dim3")]
@@ -177,28 +177,37 @@ fn nphysics_collider_from_rapier_collider(
 ) -> Option<ColliderDesc<f32>> {
     let margin = ColliderDesc::<f32>::default_margin();
     let mut pos = *collider.position_wrt_parent();
+    let shape = collider.shape();
 
-    let shape = match collider.shape() {
+    let shape = if let Some(cuboid) = shape.as_cuboid() {
-        Shape::Cuboid(cuboid) => {
+        ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
-            ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
+    } else if let Some(ball) = shape.as_ball() {
-        }
+        ShapeHandle::new(Ball::new(ball.radius - margin))
-        Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)),
+    } else if let Some(capsule) = shape.as_capsule() {
-        Shape::Capsule(capsule) => {
+        pos *= capsule.transform_wrt_y();
-            pos *= capsule.transform_wrt_y();
+        ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
-            ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
+    } else if let Some(heightfield) = shape.as_heightfield() {
-        }
+        ShapeHandle::new(heightfield.clone())
-        Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()),
+    } else {
         #[cfg(feature = "dim3")]
-        Shape::Trimesh(trimesh) => ShapeHandle::new(TriMesh::new(
+        if let Some(trimesh) = shape.as_trimesh() {
-            trimesh.vertices().to_vec(),
+            ShapeHandle::new(TriMesh::new(
-            trimesh
+                trimesh.vertices().to_vec(),
-                .indices()
+                trimesh
-                .iter()
+                    .indices()
-                .map(|idx| na::convert(*idx))
+                    .iter()
-                .collect(),
+                    .map(|idx| na::convert(*idx))
-            None,
+                    .collect(),
-        )),
+                None,
-        _ => return None,
+            ))
+        } else {
+            return None;
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            return None;
+        }
     };
 
     let density = if is_dynamic { collider.density() } else { 0.0 };

src_testbed/objects/cone.rs

@@ -0,0 +1,74 @@
+use crate::objects::node::{self, GraphicsNode};
+use kiss3d::window::Window;
+use na::Point3;
+use rapier::geometry::{ColliderHandle, ColliderSet};
+use rapier::math::Isometry;
+
+pub struct Cone {
+    color: Point3<f32>,
+    base_color: Point3<f32>,
+    gfx: GraphicsNode,
+    collider: ColliderHandle,
+}
+
+impl Cone {
+    pub fn new(
+        collider: ColliderHandle,
+        half_height: f32,
+        radius: f32,
+        color: Point3<f32>,
+        window: &mut Window,
+    ) -> Cone {
+        #[cfg(feature = "dim2")]
+        let node = window.add_rectangle(radius, half_height);
+        #[cfg(feature = "dim3")]
+        let node = window.add_cone(radius, half_height * 2.0);
+
+        let mut res = Cone {
+            color,
+            base_color: color,
+            gfx: node,
+            collider,
+        };
+
+        // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
+        res.gfx.set_color(color.x, color.y, color.z);
+        res
+    }
+
+    pub fn select(&mut self) {
+        self.color = Point3::new(1.0, 0.0, 0.0);
+    }
+
+    pub fn unselect(&mut self) {
+        self.color = self.base_color;
+    }
+
+    pub fn set_color(&mut self, color: Point3<f32>) {
+        self.gfx.set_color(color.x, color.y, color.z);
+        self.color = color;
+        self.base_color = color;
+    }
+
+    pub fn update(&mut self, colliders: &ColliderSet) {
+        node::update_scene_node(
+            &mut self.gfx,
+            colliders,
+            self.collider,
+            &self.color,
+            &Isometry::identity(),
+        );
+    }
+
+    pub fn scene_node(&self) -> &GraphicsNode {
+        &self.gfx
+    }
+
+    pub fn scene_node_mut(&mut self) -> &mut GraphicsNode {
+        &mut self.gfx
+    }
+
+    pub fn object(&self) -> ColliderHandle {
+        self.collider
+    }
+}

src_testbed/objects/cylinder.rs

@@ -0,0 +1,74 @@
+use crate::objects::node::{self, GraphicsNode};
+use kiss3d::window::Window;
+use na::Point3;
+use rapier::geometry::{ColliderHandle, ColliderSet};
+use rapier::math::Isometry;
+
+pub struct Cylinder {
+    color: Point3<f32>,
+    base_color: Point3<f32>,
+    gfx: GraphicsNode,
+    collider: ColliderHandle,
+}
+
+impl Cylinder {
+    pub fn new(
+        collider: ColliderHandle,
+        half_height: f32,
+        radius: f32,
+        color: Point3<f32>,
+        window: &mut Window,
+    ) -> Cylinder {
+        #[cfg(feature = "dim2")]
+        let node = window.add_rectangle(radius, half_height);
+        #[cfg(feature = "dim3")]
+        let node = window.add_cylinder(radius, half_height * 2.0);
+
+        let mut res = Cylinder {
+            color,
+            base_color: color,
+            gfx: node,
+            collider,
+        };
+
+        // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
+        res.gfx.set_color(color.x, color.y, color.z);
+        res
+    }
+
+    pub fn select(&mut self) {
+        self.color = Point3::new(1.0, 0.0, 0.0);
+    }
+
+    pub fn unselect(&mut self) {
+        self.color = self.base_color;
+    }
+
+    pub fn set_color(&mut self, color: Point3<f32>) {
+        self.gfx.set_color(color.x, color.y, color.z);
+        self.color = color;
+        self.base_color = color;
+    }
+
+    pub fn update(&mut self, colliders: &ColliderSet) {
+        node::update_scene_node(
+            &mut self.gfx,
+            colliders,
+            self.collider,
+            &self.color,
+            &Isometry::identity(),
+        );
+    }
+
+    pub fn scene_node(&self) -> &GraphicsNode {
+        &self.gfx
+    }
+
+    pub fn scene_node_mut(&mut self) -> &mut GraphicsNode {
+        &mut self.gfx
+    }
+
+    pub fn object(&self) -> ColliderHandle {
+        self.collider
+    }
+}

src_testbed/objects/mod.rs

@@ -1,7 +1,9 @@
 pub mod ball;
 pub mod box_node;
 pub mod capsule;
+pub mod cone;
 pub mod convex;
+pub mod cylinder;
 pub mod heightfield;
 pub mod mesh;
 pub mod node;

src_testbed/objects/node.rs

@@ -10,6 +10,8 @@ use crate::objects::mesh::Mesh;
 use kiss3d::window::Window;
 use na::Point3;
 
+use crate::objects::cone::Cone;
+use crate::objects::cylinder::Cylinder;
 use rapier::geometry::{ColliderHandle, ColliderSet};
 use rapier::math::Isometry;
 
@@ -28,6 +30,8 @@ pub enum Node {
     //    Polyline(Polyline),
     Mesh(Mesh),
     Convex(Convex),
+    Cylinder(Cylinder),
+    Cone(Cone),
 }
 
 impl Node {
@@ -42,6 +46,8 @@ impl Node {
             //            Node::Polyline(ref mut n) => n.select(),
             Node::Mesh(ref mut n) => n.select(),
             Node::Convex(ref mut n) => n.select(),
+            Node::Cylinder(ref mut n) => n.select(),
+            Node::Cone(ref mut n) => n.select(),
         }
     }
 
@@ -56,6 +62,8 @@ impl Node {
             //            Node::Polyline(ref mut n) => n.unselect(),
             Node::Mesh(ref mut n) => n.unselect(),
             Node::Convex(ref mut n) => n.unselect(),
+            Node::Cylinder(ref mut n) => n.unselect(),
+            Node::Cone(ref mut n) => n.unselect(),
         }
     }
 
@@ -70,6 +78,8 @@ impl Node {
             //            Node::Polyline(ref mut n) => n.update(colliders),
             Node::Mesh(ref mut n) => n.update(colliders),
             Node::Convex(ref mut n) => n.update(colliders),
+            Node::Cylinder(ref mut n) => n.update(colliders),
+            Node::Cone(ref mut n) => n.update(colliders),
         }
     }
 
@@ -97,6 +107,8 @@ impl Node {
             Node::HeightField(ref n) => Some(n.scene_node()),
             Node::Mesh(ref n) => Some(n.scene_node()),
             Node::Convex(ref n) => Some(n.scene_node()),
+            Node::Cylinder(ref n) => Some(n.scene_node()),
+            Node::Cone(ref n) => Some(n.scene_node()),
             #[cfg(feature = "dim2")]
             _ => None,
         }
@@ -113,6 +125,8 @@ impl Node {
             Node::HeightField(ref mut n) => Some(n.scene_node_mut()),
             Node::Mesh(ref mut n) => Some(n.scene_node_mut()),
             Node::Convex(ref mut n) => Some(n.scene_node_mut()),
+            Node::Cylinder(ref mut n) => Some(n.scene_node_mut()),
+            Node::Cone(ref mut n) => Some(n.scene_node_mut()),
             #[cfg(feature = "dim2")]
             _ => None,
         }
@@ -129,6 +143,8 @@ impl Node {
             //            Node::Polyline(ref n) => n.object(),
             Node::Mesh(ref n) => n.object(),
             Node::Convex(ref n) => n.object(),
+            Node::Cylinder(ref n) => n.object(),
+            Node::Cone(ref n) => n.object(),
         }
     }
 
@@ -143,6 +159,8 @@ impl Node {
             //            Node::Polyline(ref mut n) => n.set_color(color),
             Node::Mesh(ref mut n) => n.set_color(color),
             Node::Convex(ref mut n) => n.set_color(color),
+            Node::Cylinder(ref mut n) => n.set_color(color),
+            Node::Cone(ref mut n) => n.set_color(color),
         }
     }
 }

src_testbed/physx_backend.rs

@@ -6,7 +6,7 @@ use rapier::counters::Counters;
 use rapier::dynamics::{
     IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
 };
-use rapier::geometry::{Collider, ColliderSet, Shape};
+use rapier::geometry::{Collider, ColliderSet};
 use rapier::utils::WBasis;
 use std::collections::HashMap;
 
@@ -422,27 +422,29 @@ fn physx_collider_from_rapier_collider(
     collider: &Collider,
 ) -> Option<(ColliderDesc, Isometry3<f32>)> {
     let mut local_pose = *collider.position_wrt_parent();
-    let desc = match collider.shape() {
+    let shape = collider.shape();
-        Shape::Cuboid(cuboid) => ColliderDesc::Box(
+
+    let desc = if let Some(cuboid) = shape.as_cuboid() {
+        ColliderDesc::Box(
             cuboid.half_extents.x,
             cuboid.half_extents.y,
             cuboid.half_extents.z,
-        ),
+        )
-        Shape::Ball(ball) => ColliderDesc::Sphere(ball.radius),
+    } else if let Some(ball) = shape.as_ball() {
-        Shape::Capsule(capsule) => {
+        ColliderDesc::Sphere(ball.radius)
-            let center = capsule.center();
+    } else if let Some(capsule) = shape.as_capsule() {
-            let mut dir = capsule.b - capsule.a;
+        let center = capsule.center();
+        let mut dir = capsule.segment.b - capsule.segment.a;
 
-            if dir.x < 0.0 {
+        if dir.x < 0.0 {
-                dir = -dir;
+            dir = -dir;
-            }
-
-            let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
-            local_pose *=
-                Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
-            ColliderDesc::Capsule(capsule.radius, capsule.height())
         }
-        Shape::Trimesh(trimesh) => ColliderDesc::TriMesh {
+
+        let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
+        local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
+        ColliderDesc::Capsule(capsule.radius, capsule.height())
+    } else if let Some(trimesh) = shape.as_trimesh() {
+        ColliderDesc::TriMesh {
             vertices: trimesh
                 .vertices()
                 .iter()
@@ -450,11 +452,10 @@ fn physx_collider_from_rapier_collider(
                 .collect(),
             indices: trimesh.flat_indices().to_vec(),
             mesh_scale: Vector3::repeat(1.0).into_glam(),
-        },
-        _ => {
-            eprintln!("Creating a shape unknown to the PhysX backend.");
-            return None;
         }
+    } else {
+        eprintln!("Creating a shape unknown to the PhysX backend.");
+        return None;
     };
 
     Some((desc, local_pose))

src_testbed/testbed.rs

@@ -21,7 +21,12 @@ use na::{self, Point2, Point3, Vector3};
 use rapier::dynamics::{
     ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet,
 };
-use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray};
+#[cfg(feature = "dim3")]
+use rapier::geometry::Ray;
+use rapier::geometry::{
+    BroadPhase, ColliderHandle, ColliderSet, ContactEvent, InteractionGroups, NarrowPhase,
+    ProximityEvent,
+};
 use rapier::math::Vector;
 use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline};
 #[cfg(feature = "fluids")]
@@ -495,6 +500,10 @@ impl Testbed {
         self.graphics.set_body_color(body, color);
     }
 
+    pub fn set_collider_initial_color(&mut self, collider: ColliderHandle, color: Point3<f32>) {
+        self.graphics.set_collider_initial_color(collider, color);
+    }
+
     #[cfg(feature = "fluids")]
     pub fn set_fluid_color(&mut self, fluid: FluidHandle, color: Point3<f32>) {
         self.graphics.set_fluid_color(fluid, color);
@@ -672,6 +681,8 @@ impl Testbed {
                                     &mut self.physics.bodies,
                                     &mut self.physics.colliders,
                                     &mut self.physics.joints,
+                                    None,
+                                    None,
                                     &self.event_handler,
                                 );
 
@@ -1180,10 +1191,12 @@ impl Testbed {
             .camera()
             .unproject(&self.cursor_pos, &na::convert(size));
         let ray = Ray::new(pos, dir);
-        let hit = self
+        let hit = self.physics.query_pipeline.cast_ray(
-            .physics
+            &self.physics.colliders,
-            .query_pipeline
+            &ray,
-            .cast_ray(&self.physics.colliders, &ray, f32::MAX);
+            f32::MAX,
+            InteractionGroups::all(),
+        );
 
         if let Some((_, collider, _)) = hit {
             if self.physics.bodies[collider.parent()].is_dynamic() {
@@ -1446,6 +1459,8 @@ impl State for Testbed {
                                 &mut physics.bodies,
                                 &mut physics.colliders,
                                 &mut physics.joints,
+                                None,
+                                None,
                                 event_handler,
                             );
                         });
@@ -1460,6 +1475,8 @@ impl State for Testbed {
                         &mut self.physics.bodies,
                         &mut self.physics.colliders,
                         &mut self.physics.joints,
+                        None,
+                        None,
                         &self.event_handler,
                     );