Complete the pfm/pfm contact generator.

9 files changed, 71 insertions, 155 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index af1fb4a..9fa5a8e 100644
--- a/src/dynamics/rigid_body.rs
+++ b/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>,
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs
index 782175a..d211cf1 100644
--- a/src/geometry/contact.rs
+++ b/src/geometry/contact.rs
@@ -429,17 +429,27 @@ impl ContactManifold {
     }
 
     #[inline]
-    pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>, early_stop: bool) -> bool {
+    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 early_stop && -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
+        if -self.local_n1.dot(&local_n2) < angle_dot_threshold {
             return false;
         }
 
@@ -448,15 +458,14 @@ impl ContactManifold {
             let dpt = local_p2 - pt.local_p1;
             let dist = dpt.dot(&self.local_n1);
 
-            if early_stop && dist * pt.dist < 0.0 {
+            if dist * pt.dist < 0.0 {
                 // We switched between penetrating/non-penetrating.
                 // The may result in other contacts to appear.
                 return false;
             }
             let new_local_p1 = local_p2 - self.local_n1 * dist;
 
-            let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
-            if early_stop && na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
+            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
                 return false;
             }
 
diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
index c94b300..a7857a1 100644
--- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
+++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
@@ -47,8 +47,8 @@ pub fn generate_contacts<'a>(
     let mut pos12 = pos1.inverse() * pos2;
     let mut pos21 = pos12.inverse();
 
-    if (!swapped && manifold.try_update_contacts(&pos12, true))
-        || (swapped && manifold.try_update_contacts(&pos21, true))
+    if (!swapped && manifold.try_update_contacts(&pos12))
+        || (swapped && manifold.try_update_contacts(&pos21))
     {
         return;
     }
diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
index 04ac43a..d879a22 100644
--- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
+++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
@@ -34,7 +34,7 @@ pub fn generate_contacts<'a>(
     let mut pos12 = pos1.inverse() * pos2;
     let mut pos21 = pos12.inverse();
 
-    if manifold.try_update_contacts(&pos12, true) {
+    if manifold.try_update_contacts(&pos12) {
         return;
     }
 
diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs
index d73e2eb..1a0358d 100644
--- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs
+++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs
@@ -48,8 +48,8 @@ pub fn generate_contacts<'a>(
     let mut pos12 = pos1.inverse() * pos2;
     let mut pos21 = pos12.inverse();
 
-    if (!swapped && manifold.try_update_contacts(&pos12, true))
-        || (swapped && manifold.try_update_contacts(&pos21, true))
+    if (!swapped && manifold.try_update_contacts(&pos12))
+        || (swapped && manifold.try_update_contacts(&pos21))
     {
         return;
     }
diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
index a5e8014..c3815dd 100644
--- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
+++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
@@ -30,8 +30,8 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace {
 
 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(),
+        ctxt.shape1.as_polygonal_feature_map(),
+        ctxt.shape2.as_polygonal_feature_map(),
     ) {
         do_generate_contacts(pfm1, pfm2, ctxt);
         ctxt.manifold.update_warmstart_multiplier();
@@ -47,9 +47,15 @@ fn do_generate_contacts(
     let pos12 = ctxt.position1.inverse() * ctxt.position2;
     let pos21 = pos12.inverse();
 
-    // if ctxt.manifold.try_update_contacts(&pos12, true) {
-    //     return;
-    // }
+    // 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
@@ -72,7 +78,7 @@ fn do_generate_contacts(
     ctxt.manifold.points.clear();
 
     match contact {
-        GJKResult::ClosestPoints(local_p1, local_p2, dir) => {
+        GJKResult::ClosestPoints(_, _, dir) => {
             workspace.last_gjk_dir = Some(dir);
             let normal1 = dir;
             let normal2 = pos21 * -dir;
@@ -89,24 +95,10 @@ fn do_generate_contacts(
                 ctxt.manifold,
             );
 
-            // 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.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate?
             ctxt.manifold.kinematics.radius1 = 0.0;
             ctxt.manifold.kinematics.radius2 = 0.0;
         }
@@ -115,116 +107,7 @@ fn do_generate_contacts(
         }
         _ => {}
     }
-}
-
-fn do_generate_contacts2(
-    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, true) {
-    //     return;
-    // }
-
-    let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
-        .workspace
-        .as_mut()
-        .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
-        .downcast_mut()
-        .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
-
-    fn generate_single_contact_pair(
-        pfm1: &dyn PolygonalFeatureMap,
-        pfm2: &dyn PolygonalFeatureMap,
-        pos12: &Isometry<f32>,
-        pos21: &Isometry<f32>,
-        prediction_distance: f32,
-        manifold: &mut ContactManifold,
-        workspace: &mut PfmPfmContactManifoldGeneratorWorkspace,
-    ) -> Option<Unit<Vector<f32>>> {
-        let contact = query::contact_support_map_support_map_with_params(
-            &Isometry::identity(),
-            pfm1,
-            &pos12,
-            pfm2,
-            prediction_distance,
-            &mut workspace.simplex,
-            workspace.last_gjk_dir,
-        );
-
-        match contact {
-            GJKResult::ClosestPoints(local_p1, local_p2, dir) => {
-                // Add at least the deepest contact.
-                let dist = (local_p2 - local_p1).dot(&dir);
-                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,
-                });
 
-                Some(dir)
-            }
-            GJKResult::NoIntersection(dir) => Some(dir),
-            _ => None,
-        }
-    }
-
-    let old_manifold_points = ctxt.manifold.points.clone();
-    ctxt.manifold.points.clear();
-
-    if let Some(local_n1) = generate_single_contact_pair(
-        pfm1,
-        pfm2,
-        &pos12,
-        &pos21,
-        ctxt.prediction_distance,
-        ctxt.manifold,
-        workspace,
-    ) {
-        workspace.last_gjk_dir = Some(local_n1);
-
-        if !ctxt.manifold.points.is_empty() {
-            use crate::utils::WBasis;
-            // Use perturbations to generate other contact points.
-            let basis = local_n1.orthonormal_basis();
-            let perturbation_angle = std::f32::consts::PI / 180.0 * 15.0; // FIXME: this should be a function of the shape size.
-            let perturbations = [
-                UnitQuaternion::new(basis[0] * perturbation_angle),
-                UnitQuaternion::new(basis[0] * -perturbation_angle),
-                UnitQuaternion::new(basis[1] * perturbation_angle),
-                UnitQuaternion::new(basis[1] * -perturbation_angle),
-            ];
-
-            for rot in &perturbations {
-                let new_pos12 = pos12 * rot;
-                let new_pos21 = new_pos12.inverse();
-                generate_single_contact_pair(
-                    pfm1,
-                    pfm2,
-                    &new_pos12,
-                    &new_pos21,
-                    ctxt.prediction_distance,
-                    ctxt.manifold,
-                    workspace,
-                );
-                println!("After perturbation: {}", ctxt.manifold.points.len());
-            }
-
-            // Set manifold normal.
-            ctxt.manifold.local_n1 = *local_n1;
-            ctxt.manifold.local_n2 = pos21 * -*local_n1;
-            ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME
-            ctxt.manifold.kinematics.radius1 = 0.0;
-            ctxt.manifold.kinematics.radius2 = 0.0;
-
-            ctxt.manifold.try_update_contacts(&pos12, false);
-        }
-    }
+    // 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 9fc1591..33b54e4 100644
--- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs
+++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs
@@ -31,7 +31,7 @@ fn generate_contacts<'a>(
     let mut m12 = m1.inverse() * m2;
     let mut m21 = m12.inverse();
 
-    if manifold.try_update_contacts(&m12, true) {
+    if manifold.try_update_contacts(&m12) {
         return;
     }
 
diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs
index 8b047fc..70be5d0 100644
--- a/src/geometry/polygonal_feature_map.rs
+++ b/src/geometry/polygonal_feature_map.rs
@@ -32,6 +32,20 @@ impl PolygonalFeatureMap for Cuboid {
 
 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());
@@ -45,10 +59,10 @@ impl PolygonalFeatureMap for Cylinder {
             );
             out_features.vertices[1] =
                 Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
-            out_features.eids = [0, 0, 0, 0]; // FIXME
-            out_features.fid = 1;
+            out_features.eids = [0, 0, 0, 0];
+            out_features.fid = 0;
             out_features.num_vertices = 2;
-            out_features.vids = [0, 1, 1, 1]; // FIXME
+            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);
@@ -56,10 +70,18 @@ impl PolygonalFeatureMap for Cylinder {
             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 = [0, 1, 2, 3]; // FIXME
-            out_features.fid = if dir.y < 0.0 { 0 } else { 2 };
-            out_features.num_vertices = 4;
-            out_features.vids = [0, 1, 2, 3]; // FIXME
+
+            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];
+            }
         }
     }
 }
diff --git a/src/utils.rs b/src/utils.rs
index 04e6a3a..bd972b8 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -19,8 +19,9 @@ 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;
+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;