Add rounded cylinder.

2 files changed, 19 insertions, 15 deletions

diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs
index 70ac84c..14b7f79 100644
--- a/src/geometry/contact_generator/contact_dispatcher.rs
+++ b/src/geometry/contact_generator/contact_dispatcher.rs
@@ -69,16 +69,7 @@ impl ContactDispatcher for DefaultContactDispatcher {
                 },
                 None,
             ),
-            (ShapeType::Cuboid, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Cuboid)
-            | (ShapeType::Triangle, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Triangle)
-            | (ShapeType::Capsule, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Capsule)
-            | (ShapeType::Cylinder, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Cylinder)
-            | (ShapeType::Cone, ShapeType::Ball)
-            | (ShapeType::Ball, ShapeType::Cone) => (
+            (_, ShapeType::Ball) | (ShapeType::Ball, _) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_convex,
                     ..PrimitiveContactGenerator::default()
@@ -104,7 +95,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
             (ShapeType::Cylinder, _)
             | (_, ShapeType::Cylinder)
             | (ShapeType::Cone, _)
-            | (_, ShapeType::Cone) => (
+            | (_, ShapeType::Cone)
+            | (ShapeType::RoundedCylinder, _)
+            | (_, ShapeType::RoundedCylinder) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_pfm_pfm,
                     ..PrimitiveContactGenerator::default()
diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
index c3815dd..37f8629 100644
--- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
+++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
@@ -29,11 +29,11 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace {
 }
 
 pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
-    if let (Some(pfm1), Some(pfm2)) = (
+    if let (Some((pfm1, round_radius1)), Some((pfm2, round_radius2))) = (
         ctxt.shape1.as_polygonal_feature_map(),
         ctxt.shape2.as_polygonal_feature_map(),
     ) {
-        do_generate_contacts(pfm1, pfm2, ctxt);
+        do_generate_contacts(pfm1, round_radius1, pfm2, round_radius2, ctxt);
         ctxt.manifold.update_warmstart_multiplier();
         ctxt.manifold.sort_contacts(ctxt.prediction_distance);
     }
@@ -41,7 +41,9 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
 
 fn do_generate_contacts(
     pfm1: &dyn PolygonalFeatureMap,
+    round_radius1: f32,
     pfm2: &dyn PolygonalFeatureMap,
+    round_radius2: f32,
     ctxt: &mut PrimitiveContactGenerationContext,
 ) {
     let pos12 = ctxt.position1.inverse() * ctxt.position2;
@@ -64,12 +66,13 @@ fn do_generate_contacts(
         .downcast_mut()
         .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
 
+    let total_prediction = ctxt.prediction_distance + round_radius1 + round_radius2;
     let contact = query::contact_support_map_support_map_with_params(
         &Isometry::identity(),
         pfm1,
         &pos12,
         pfm2,
-        ctxt.prediction_distance,
+        total_prediction,
         &mut workspace.simplex,
         workspace.last_gjk_dir,
     );
@@ -87,7 +90,7 @@ fn do_generate_contacts(
             workspace.feature2.transform_by(&pos12);
 
             PolyhedronFace::contacts(
-                ctxt.prediction_distance,
+                total_prediction,
                 &workspace.feature1,
                 &normal1,
                 &workspace.feature2,
@@ -95,6 +98,14 @@ fn do_generate_contacts(
                 ctxt.manifold,
             );
 
+            if round_radius1 != 0.0 || round_radius2 != 0.0 {
+                for contact in &mut ctxt.manifold.points {
+                    contact.local_p1 += *normal1 * round_radius1;
+                    contact.local_p2 += *normal2 * round_radius2;
+                    contact.dist -= round_radius1 + round_radius2;
+                }
+            }
+
             // Adjust points to take the radius into account.
             ctxt.manifold.local_n1 = *normal1;
             ctxt.manifold.local_n2 = *normal2;