Complete the pfm/pfm contact generator.

5 files changed, 21 insertions, 138 deletions

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;
     }