Merge pull request #41 from dimforge/cylinder

Add cylinder and cone support + use a trait-object for shapes.

.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"}'

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,4 +1,3 @@
-# Name idea: bident for 2D and trident for 3D
 [package]
 name    = "rapier2d"
 version = "0.2.1"
@@ -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,21 @@ 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"
 
 [dev-dependencies]
 bincode = "1"

build/rapier3d/Cargo.toml

@@ -1,4 +1,3 @@
-# Name idea: bident for 2D and trident for 3D
 [package]
 name    = "rapier3d"
 version = "0.2.1"
@@ -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"
+
 
 [dev-dependencies]
 bincode = "1"

build/rapier_testbed2d/Cargo.toml

@@ -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"

build/rapier_testbed3d/Cargo.toml

@@ -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"

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"

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

@@ -13,6 +13,7 @@ use std::cmp::Ordering;
 mod add_remove3;
 mod compound3;
 mod debug_boxes3;
+mod debug_cylinder3;
 mod debug_triangle3;
 mod debug_trimesh3;
 mod domino3;
@@ -76,6 +77,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/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(),
+                    1 => ColliderBuilder::ball(rad).build(),
+                    // Rounded cylinders are much more efficient that cylinder, even if the
+                    // 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);
-                } else {
-                    let collider = ColliderBuilder::ball(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(),
+                    1 => ColliderBuilder::ball(rad).build(),
+                    // Rounded cylinders are much more efficient that cylinder, even if the
+                    // 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);
-                } else {
-                    let collider = ColliderBuilder::ball(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

@@ -218,6 +218,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,7 +335,7 @@ impl RigidBody {
     }
 }
 
-/// A builder for rigid-bodies.
+/// A builder for rigid-bodies.
 pub struct RigidBodyBuilder {
     position: Isometry<f32>,
     linvel: Vector<f32>,

src/geometry/broad_phase_multi_sap.rs

@@ -335,7 +335,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
@@ -344,14 +344,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(),
             need_update: false,
@@ -386,15 +386,15 @@ impl SAPRegion {
             // Update endpoints.
             let mut deleted_any = false;
             for dim in 0..DIM {
-                self.axii[dim].update_endpoints(dim, proxies, reporting);
+                self.axes[dim].update_endpoints(dim, proxies, reporting);
-                deleted_any = self.axii[dim]
+                deleted_any = self.axes[dim]
                     .delete_out_of_bounds_proxies(&mut self.existing_proxies)
                     || deleted_any;
             }
 
             if deleted_any {
                 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);
                 }
             }
 
@@ -404,9 +404,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();
         }
     }

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,
-    Proximity, Ray, RayIntersection, Triangle, Trimesh,
+    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 {
-    /// A ball shape.
-    Ball(Ball),
-    /// A convex polygon shape.
-    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 Deref for ColliderShape {
-    /// Gets a reference to the underlying ball shape, if `self` is one.
+    type Target = dyn Shape;
-    pub fn as_ball(&self) -> Option<&Ball> {
+    fn deref(&self) -> &dyn Shape {
-        match self {
+        &*self.0
-            Shape::Ball(b) => Some(b),
-            _ => None,
     }
 }
 
-    /// Gets a reference to the underlying polygon shape, if `self` is one.
+impl ColliderShape {
-    pub fn as_polygon(&self) -> Option<&Polygon> {
+    /// Initialize a ball shape defined by its radius.
-        match self {
+    pub fn ball(radius: f32) -> Self {
-            Shape::Polygon(p) => Some(p),
+        ColliderShape(Arc::new(Ball::new(radius)))
-            _ => None,
-        }
     }
 
-    /// Gets a reference to the underlying cuboid shape, if `self` is one.
+    /// Initialize a cylindrical shape defined by its half-height
-    pub fn as_cuboid(&self) -> Option<&Cuboid> {
+    /// (along along the y axis) and its radius.
-        match self {
+    #[cfg(feature = "dim3")]
-            Shape::Cuboid(c) => Some(c),
+    pub fn cylinder(half_height: f32, radius: f32) -> Self {
-            _ => None,
+        ColliderShape(Arc::new(Cylinder::new(half_height, radius)))
-        }
     }
 
-    /// Gets a reference to the underlying capsule shape, if `self` is one.
+    /// Initialize a rounded cylindrical shape defined by its half-height
-    pub fn as_capsule(&self) -> Option<&Capsule> {
+    /// (along along the y axis), its radius, and its roundedness (the
-        match self {
+    /// radius of the sphere used for dilating the cylinder).
-            Shape::Capsule(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 triangle mesh shape, if `self` is one.
+    /// Initialize a cone shape defined by its half-height
-    pub fn as_trimesh(&self) -> Option<&Trimesh> {
+    /// (along along the y axis) and its basis radius.
-        match self {
+    #[cfg(feature = "dim3")]
-            Shape::Trimesh(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 heightfield shape, if `self` is one.
+    /// Initialize a cuboid shape defined by its half-extents.
-    pub fn as_heightfield(&self) -> Option<&HeightField> {
+    pub fn cuboid(half_extents: Vector<f32>) -> Self {
-        match self {
+        ColliderShape(Arc::new(Cuboid::new(half_extents)))
-            Shape::HeightField(h) => Some(h),
-            _ => None,
-        }
     }
 
-    /// Gets a reference to the underlying triangle shape, if `self` is one.
+    /// Initialize a capsule shape from its endpoints and radius.
-    pub fn as_triangle(&self) -> Option<&Triangle> {
+    pub fn capsule(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
-        match self {
+        ColliderShape(Arc::new(Capsule::new(a, b, radius)))
-            Shape::Triangle(c) => Some(c),
-            _ => None,
-        }
     }
 
-    /// Computes the axis-aligned bounding box of this shape.
+    /// Initialize a segment shape from its endpoints.
-    pub fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+    pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
-        match self {
+        ColliderShape(Arc::new(Segment::new(a, b)))
-            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 shape.
-    ///
+    pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self {
-    /// Some shapes are not supported yet and will always return `None`.
+        ColliderShape(Arc::new(Triangle::new(a, b, c)))
-    ///
-    /// # Parameters
-    /// - `position`: the position of this shape.
-    /// - `ray`: the ray to cast.
-    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
-    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
-    pub fn cast_ray(
-        &self,
-        position: &Isometry<f32>,
-        ray: &Ray,
-        max_toi: f32,
-    ) -> Option<RayIntersection> {
-        match self {
-            Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true),
-            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);
-                caps.toi_and_normal_with_ray(&pos, ray, max_toi, true)
     }
-            Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true),
+
+    /// Initializes a triangle mesh shape defined by its vertex and index buffers.
+    pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
+        ColliderShape(Arc::new(Trimesh::new(vertices, indices)))
+    }
+
+    /// Initializes an heightfield shape defined by its set of height and a scale
+    /// factor along each coordinate axis.
     #[cfg(feature = "dim2")]
-            Shape::Triangle(_) | Shape::Trimesh(_) => {
+    pub fn heightfield(heights: na::DVector<f32>, scale: Vector<f32>) -> Self {
-                // This is not implemented yet in 2D.
+        ColliderShape(Arc::new(HeightField::new(heights, scale)))
-                None
     }
+
+    /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale
+    /// factor along each coordinate axis.
     #[cfg(feature = "dim3")]
-            Shape::Triangle(triangle) => {
+    pub fn heightfield(heights: na::DMatrix<f32>, scale: Vector<f32>) -> Self {
-                triangle.toi_and_normal_with_ray(position, ray, max_toi, true)
+        ColliderShape(Arc::new(HeightField::new(heights, scale)))
     }
+}
+
+#[cfg(feature = "serde-serialize")]
+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")
+            }
+
+            fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
+            where
+                A: serde::de::SeqAccess<'de>,
+            {
+                use num::cast::FromPrimitive;
+
+                let tag: i32 = seq
+                    .next_element()?
+                    .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
+
+                fn deser<'de, A, S: Shape + serde::Deserialize<'de>>(
+                    seq: &mut A,
+                ) -> Result<Arc<dyn Shape>, A::Error>
+                where
+                    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")]
-            Shape::Trimesh(trimesh) => {
+                    Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
-                trimesh.toi_and_normal_with_ray(position, ray, max_toi, true)
+                    #[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",
+                        ))
                     }
-            Shape::HeightField(heightfield) => {
+                };
-                heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
+
+                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,
@@ -215,8 +259,8 @@ impl Collider {
     }
 
     /// 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 +276,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 +285,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.
@@ -269,7 +300,7 @@ pub struct ColliderBuilder {
 
 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,
@@ -288,96 +319,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.

src/geometry/contact.rs

@@ -430,16 +430,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 +465,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_pfm_pfm,
+                    ..PrimitiveContactGenerator::default()
+                },
+                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

@@ -139,8 +139,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 +152,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],

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,15 @@ fn do_generate_contacts(
     let manifolds = &mut ctxt.pair.manifolds;
     let prediction_distance = ctxt.prediction_distance;
     let dispatcher = ctxt.dispatcher;
+    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,7 +124,7 @@ 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(),
@@ -162,7 +149,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 +160,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() {
@@ -159,10 +160,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/mod.rs

@@ -2,7 +2,7 @@
 
 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,
@@ -19,9 +19,13 @@ 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 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 +34,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 +50,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 +59,21 @@ 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::shape::{Shape, ShapeType};
 
 mod ball;
 mod broad_phase_multi_sap;
-mod capsule;
 mod collider;
 mod collider_set;
 mod contact;
@@ -81,3 +96,9 @@ mod trimesh;
 mod waabb;
 mod wquadtree;
 //mod z_order;
+mod capsule;
+#[cfg(feature = "dim3")]
+mod polygonal_feature_map;
+#[cfg(feature = "dim3")]
+mod round_cylinder;
+mod shape;

src/geometry/narrow_phase.rs

@@ -197,7 +197,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 +227,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,
@@ -308,7 +310,8 @@ impl NarrowPhase {
             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;
             }
@@ -418,7 +421,8 @@ impl NarrowPhase {
             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;
             }

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/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/query_pipeline.rs

@@ -69,7 +69,11 @@ impl QueryPipeline {
 
         for handle in inter {
             let collider = &colliders[handle];
-            if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
+            if let Some(inter) =
+                collider
+                    .shape()
+                    .toi_and_normal_with_ray(collider.position(), ray, max_toi, true)
+            {
                 if inter.toi < best {
                     best = inter.toi;
                     result = Some((handle, collider, inter));
@@ -103,7 +107,11 @@ impl QueryPipeline {
 
         for handle in inter {
             let collider = &colliders[handle];
-            if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
+            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,28 +167,26 @@ 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) => {
+
+        if let Some(b) = shape.as_ball() {
             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);
-            }
+        } else if let Some(c) = shape.as_cuboid() {
-            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);
-            }
+        // } else if let Some(polygon) = shape.as_polygon() {
-            Shape::Polygon(poly) => {
+        //     let points: Vec<_> = poly
-                let points: Vec<_> = poly
+        //         .vertices()
-                    .vertices()
+        //         .iter()
-                    .iter()
+        //         .map(|p| collider.position_wrt_parent() * p)
-                    .map(|p| collider.position_wrt_parent() * p)
+        //         .map(|p| na_vec_to_b2_vec(p.coords))
-                    .map(|p| na_vec_to_b2_vec(p.coords))
+        //         .collect();
-                    .collect();
+        //     let b2_shape = b2::PolygonShape::new_with(&points);
-                let b2_shape = b2::PolygonShape::new_with(&points);
+        //     body.create_fixture(&b2_shape, &mut fixture_def);
-                body.create_fixture(&b2_shape, &mut fixture_def);
+        } else if let Some(heightfield) = shape.as_heightfield() {
-            }
-            Shape::HeightField(heightfield) => {
             let mut segments = heightfield.segments();
             let seg1 = segments.next().unwrap();
             let mut vertices = vec![
@@ -203,8 +201,8 @@ impl Box2dWorld {
 
             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."),
+            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;
@@ -349,33 +352,44 @@ impl GraphicsManager {
         color: Point3<f32>,
         out: &mut Vec<Node>,
     ) {
-        match collider.shape() {
+        let shape = collider.shape();
-            Shape::Ball(ball) => {
+
+        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,
+        // Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
-                poly.vertices().to_vec(),
+        //     handle,
-                color,
+        //     poly.vertices().to_vec(),
-                window,
+        //     color,
-            ))),
+        //     window,
-            Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new(
+        // ))),
+
+        if let Some(cuboid) = shape.as_cuboid() {
+            out.push(Node::Box(BoxNode::new(
                 handle,
                 cuboid.half_extents,
                 color,
                 window,
-            ))),
+            )))
-            Shape::Capsule(capsule) => {
+        }
+
+        if let Some(capsule) = shape.as_capsule() {
             out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
         }
-            Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new(
+
+        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 +399,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,19 +177,21 @@ 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)))
-        }
+    } else if let Some(ball) = shape.as_ball() {
-        Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)),
+        ShapeHandle::new(Ball::new(ball.radius - margin))
-        Shape::Capsule(capsule) => {
+    } else if let Some(capsule) = shape.as_capsule() {
         pos *= capsule.transform_wrt_y();
         ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
-        }
+    } else if let Some(heightfield) = shape.as_heightfield() {
-        Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()),
+        ShapeHandle::new(heightfield.clone())
+    } else {
         #[cfg(feature = "dim3")]
-        Shape::Trimesh(trimesh) => ShapeHandle::new(TriMesh::new(
+        if let Some(trimesh) = shape.as_trimesh() {
+            ShapeHandle::new(TriMesh::new(
                 trimesh.vertices().to_vec(),
                 trimesh
                     .indices()
@@ -197,8 +199,15 @@ fn nphysics_collider_from_rapier_collider(
                     .map(|idx| na::convert(*idx))
                     .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)
+    } else if let Some(capsule) = shape.as_capsule() {
         let center = capsule.center();
-            let mut dir = capsule.b - capsule.a;
+        let mut dir = capsule.segment.b - capsule.segment.a;
 
         if dir.x < 0.0 {
             dir = -dir;
         }
 
         let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
-            local_pose *=
+        local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
-                Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
         ColliderDesc::Capsule(capsule.radius, capsule.height())
-        }
+    } else if let Some(trimesh) = shape.as_trimesh() {
-        Shape::Trimesh(trimesh) => ColliderDesc::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(),
-        },
+        }
-        _ => {
+    } else {
         eprintln!("Creating a shape unknown to the PhysX backend.");
         return None;
-        }
     };
 
     Some((desc, local_pose))

src_testbed/testbed.rs

@@ -21,7 +21,9 @@ 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, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent};
 use rapier::math::Vector;
 use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline};
 #[cfg(feature = "fluids")]