Start adding cylinders.

4 files changed, 143 insertions, 2 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..0856029 100644
--- a/src/geometry/contact_generator/ball_convex_contact_generator.rs
+++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs
@@ -12,6 +12,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex
             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),
+            Shape::Cylinder(cylinder2) => do_generate_contacts(cylinder2, ball1, ctxt, true),
             _ => unimplemented!(),
         }
     } else if let Shape::Ball(ball2) = ctxt.shape2 {
@@ -19,6 +20,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex
             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),
+            Shape::Cylinder(cylinder1) => do_generate_contacts(cylinder1, ball2, ctxt, false),
             _ => unimplemented!(),
         }
     }
diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs
index 8c846e0..e925fd5 100644
--- a/src/geometry/contact_generator/contact_dispatcher.rs
+++ b/src/geometry/contact_generator/contact_dispatcher.rs
@@ -1,6 +1,7 @@
 use crate::geometry::contact_generator::{
     ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace,
-    PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace,
+    PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator,
+    TrimeshShapeContactGeneratorWorkspace,
 };
 use crate::geometry::Shape;
 use std::any::Any;
@@ -73,7 +74,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
             | (Shape::Triangle(_), Shape::Ball(_))
             | (Shape::Ball(_), Shape::Triangle(_))
             | (Shape::Capsule(_), Shape::Ball(_))
-            | (Shape::Ball(_), Shape::Capsule(_)) => (
+            | (Shape::Ball(_), Shape::Capsule(_))
+            | (Shape::Cylinder(_), Shape::Ball(_))
+            | (Shape::Ball(_), Shape::Cylinder(_)) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_convex,
                     ..PrimitiveContactGenerator::default()
@@ -94,6 +97,13 @@ impl ContactDispatcher for DefaultContactDispatcher {
                 },
                 None,
             ),
+            (Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_pfm_pfm,
+                    ..PrimitiveContactGenerator::default()
+                },
+                Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())),
+            ),
             _ => (PrimitiveContactGenerator::default(), None),
         }
     }
diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs
index ecd2540..a6bad05 100644
--- a/src/geometry/contact_generator/mod.rs
+++ b/src/geometry/contact_generator/mod.rs
@@ -18,6 +18,10 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang
 pub use self::heightfield_shape_contact_generator::{
     generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace,
 };
+#[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,
@@ -39,6 +43,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..cfb4472
--- /dev/null
+++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
@@ -0,0 +1,123 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{Contact, 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>>>,
+    last_optimal_dir: Option<Unit<Vector<f32>>>,
+    feature1: PolyhedronFace,
+    feature2: PolyhedronFace,
+}
+
+impl Default for PfmPfmContactManifoldGeneratorWorkspace {
+    fn default() -> Self {
+        Self {
+            simplex: VoronoiSimplex::new(),
+            last_gjk_dir: None,
+            last_optimal_dir: None,
+            feature1: PolyhedronFace::new(),
+            feature2: PolyhedronFace::new(),
+        }
+    }
+}
+
+pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Some(pfm1), Some(pfm2)) = (
+        ctxt.collider1.shape().as_polygonal_feature_map(),
+        ctxt.collider2.shape().as_polygonal_feature_map(),
+    ) {
+        do_generate_contacts(pfm1, pfm2, ctxt);
+        ctxt.manifold.update_warmstart_multiplier();
+        ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+    }
+}
+
+fn do_generate_contacts(
+    pfm1: &dyn PolygonalFeatureMap,
+    pfm2: &dyn PolygonalFeatureMap,
+    ctxt: &mut PrimitiveContactGenerationContext,
+) {
+    let pos12 = ctxt.position1.inverse() * ctxt.position2;
+    let pos21 = pos12.inverse();
+
+    // if ctxt.manifold.try_update_contacts(&pos12) {
+    //     return;
+    // }
+
+    let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
+        .workspace
+        .as_mut()
+        .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
+        .downcast_mut()
+        .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
+
+    let contact = query::contact_support_map_support_map_with_params(
+        &Isometry::identity(),
+        pfm1,
+        &pos12,
+        pfm2,
+        ctxt.prediction_distance,
+        &mut workspace.simplex,
+        workspace.last_gjk_dir,
+    );
+
+    let old_manifold_points = ctxt.manifold.points.clone();
+    ctxt.manifold.points.clear();
+
+    match contact {
+        GJKResult::ClosestPoints(local_p1, local_p2, 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(
+            //     ctxt.prediction_distance,
+            //     &workspace.feature1,
+            //     &normal1,
+            //     &workspace.feature2,
+            //     &pos21,
+            //     ctxt.manifold,
+            // );
+
+            println!(
+                "Contact patatrac: {:?}, {:?}, {}, {}",
+                ctxt.manifold.points.len(),
+                ctxt.position1 * dir,
+                workspace.feature1.num_vertices,
+                workspace.feature2.num_vertices
+            );
+
+            if ctxt.manifold.all_contacts().is_empty() {
+                // Add at least the deepest contact.
+                let dist = (local_p2 - local_p1).dot(&dir);
+                ctxt.manifold.points.push(Contact {
+                    local_p1,
+                    local_p2: pos21 * local_p2,
+                    impulse: 0.0,
+                    tangent_impulse: Contact::zero_tangent_impulse(),
+                    fid1: 0, // FIXME
+                    fid2: 0, // FIXME
+                    dist,
+                });
+            }
+
+            // Adjust points to take the radius into account.
+            ctxt.manifold.local_n1 = *normal1;
+            ctxt.manifold.local_n2 = *normal2;
+            ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME
+            ctxt.manifold.kinematics.radius1 = 0.0;
+            ctxt.manifold.kinematics.radius2 = 0.0;
+        }
+        GJKResult::NoIntersection(dir) => {
+            workspace.last_gjk_dir = Some(dir);
+        }
+        _ => {}
+    }
+}