Merge pull request #41 from dimforge/cylinder

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

12 files changed, 253 insertions, 133 deletions

diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs
index a187832..69bc5e3 100644
--- a/src/geometry/contact_generator/ball_convex_contact_generator.rs
+++ b/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();
-
-        match ctxt.shape2 {
-            Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true),
-            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!(),
-        }
+        do_generate_contacts(ctxt.shape2, ball1, ctxt, true);
+    } else if let Some(ball2) = ctxt.shape2.as_ball() {
+        do_generate_contacts(ctxt.shape1, ball2, ctxt, false);
     }
 
     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,
diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs
index 3800ce6..3104496 100644
--- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs
+++ b/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),
-        (&capsule2_1.a, &capsule2_1.b),
+        (&seg1.a, &seg1.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_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
+    let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[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);
-    let seg2 = Segment::new(capsule2_1.a, capsule2_1.b);
-
-    if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2.direction()) {
+    if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.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),
-                (capsule2_1.a, capsule2_1.b),
+                (seg1.a, seg1.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),
-        (&capsule2_1.a, &capsule2_1.b),
+        (&seg1.a, &seg1.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_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
+        let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[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());
diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs
index 8c846e0..1872c7b 100644
--- a/src/geometry/contact_generator/contact_dispatcher.rs
+++ b/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,
-        shape2: &Shape,
+        shape1: ShapeType,
+        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,
-        shape2: &Shape,
+        shape1: ShapeType,
+        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,
-        shape2: &Shape,
+        shape1: ShapeType,
+        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(_)) => (
-                PrimitiveContactGenerator {
-                    generate_contacts: super::generate_contacts_polygon_polygon,
-                    ..PrimitiveContactGenerator::default()
-                },
-                None,
-            ),
-            (Shape::Capsule(_), Shape::Capsule(_)) => (
+            // (ShapeType::Polygon, ShapeType::Polygon) => (
+            //     PrimitiveContactGenerator {
+            //         generate_contacts: super::generate_contacts_polygon_polygon,
+            //         ..PrimitiveContactGenerator::default()
+            //     },
+            //     None,
+            // ),
+            (ShapeType::Capsule, ShapeType::Capsule) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_capsule_capsule,
                     ..PrimitiveContactGenerator::default()
                 },
                 None,
             ),
-            (Shape::Cuboid(_), Shape::Ball(_))
-            | (Shape::Ball(_), Shape::Cuboid(_))
-            | (Shape::Triangle(_), Shape::Ball(_))
-            | (Shape::Ball(_), Shape::Triangle(_))
-            | (Shape::Capsule(_), Shape::Ball(_))
-            | (Shape::Ball(_), Shape::Capsule(_)) => (
+            (_, ShapeType::Ball) | (ShapeType::Ball, _) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_convex,
                     ..PrimitiveContactGenerator::default()
                 },
                 None,
             ),
-            (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => (
+            (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_cuboid_capsule,
                     ..PrimitiveContactGenerator::default()
                 },
                 None,
             ),
-            (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => (
+            (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => {
+                (
+                    PrimitiveContactGenerator {
+                        generate_contacts: super::generate_contacts_cuboid_triangle,
+                        ..PrimitiveContactGenerator::default()
+                    },
+                    None,
+                )
+            }
+            #[cfg(feature = "dim3")]
+            (ShapeType::Cylinder, _)
+            | (_, ShapeType::Cylinder)
+            | (ShapeType::Cone, _)
+            | (_, ShapeType::Cone)
+            | (ShapeType::RoundCylinder, _)
+            | (_, ShapeType::RoundCylinder)
+            | (ShapeType::Capsule, _)
+            | (_, ShapeType::Capsule) => (
                 PrimitiveContactGenerator {
-                    generate_contacts: super::generate_contacts_cuboid_triangle,
+                    generate_contacts: super::generate_contacts_pfm_pfm,
                     ..PrimitiveContactGenerator::default()
                 },
-                None,
+                Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())),
             ),
             _ => (PrimitiveContactGenerator::default(), None),
         }
@@ -100,18 +114,18 @@ impl ContactDispatcher for DefaultContactDispatcher {
 
     fn dispatch(
         &self,
-        shape1: &Shape,
-        shape2: &Shape,
+        shape1: ShapeType,
+        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()
diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs
index 9dd0050..b034760 100644
--- a/src/geometry/contact_generator/contact_generator.rs
+++ b/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 shape2: &'a Shape,
+    pub shape1: &'a dyn 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 shapes2: [&'a Shape; SIMD_WIDTH],
+    pub shapes1: [&'a dyn 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],
diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
index a7857a1..3fd4a17 100644
--- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
+++ b/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;
 
     /*
      *
diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
index d879a22..5be5af3 100644
--- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
+++ b/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,
diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs
index 1a0358d..562d7d6 100644
--- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs
+++ b/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,
diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
index 04afc65..9224d4e 100644
--- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
+++ b/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(),
diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs
index ecd2540..0549420 100644
--- a/src/geometry/contact_generator/mod.rs
+++ b/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::{
-    clip_segments, clip_segments_with_normal,
-};
+pub(crate) use self::polygon_polygon_contact_generator::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;
 
diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
new file mode 100644
index 0000000..1dcae33
--- /dev/null
+++ b/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);
+}
diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs
index 33b54e4..0e7543d 100644
--- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs
+++ b/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) {
-    if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
-        generate_contacts(
-            polygon1,
-            &ctxt.position1,
-            polygon2,
-            &ctxt.position2,
-            ctxt.manifold,
-        );
-        ctxt.manifold.update_warmstart_multiplier();
-    } else {
-        unreachable!()
-    }
-
-    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) {
+    unimplemented!()
+    // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
+    //     generate_contacts(
+    //         polygon1,
+    //         &ctxt.position1,
+    //         polygon2,
+    //         &ctxt.position2,
+    //         ctxt.manifold,
+    //     );
+    //     ctxt.manifold.update_warmstart_multiplier();
+    // } else {
+    //     unreachable!()
+    // }
+    //
+    // ctxt.manifold.sort_contacts(ctxt.prediction_d