Complete the pfm/pfm contact generator.

examples3d/all_examples3.rs

@@ -13,6 +13,7 @@ use std::cmp::Ordering;
 mod add_remove3;
 mod compound3;
 mod debug_boxes3;
+mod debug_cylinder3;
 mod debug_triangle3;
 mod debug_trimesh3;
 mod domino3;
@@ -76,6 +77,7 @@ pub fn main() {
         ("(Debug) boxes", debug_boxes3::init_world),
         ("(Debug) triangle", debug_triangle3::init_world),
         ("(Debug) trimesh", debug_trimesh3::init_world),
+        ("(Debug) cylinder", debug_cylinder3::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.

examples3d/debug_cylinder3.rs

@@ -0,0 +1,65 @@
+use na::{Point3, Vector3};
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+// This shows a bug when a cylinder is in contact with a very large
+// but very thin cuboid. In this case the EPA returns an incorrect
+// contact normal, resulting in the cylinder falling through the floor.
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 100.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, -ground_height, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the cubes
+     */
+    let num = 1;
+    let rad = 1.0;
+
+    let shiftx = rad * 2.0 + rad;
+    let shifty = rad * 2.0 + rad;
+    let shiftz = rad * 2.0 + rad;
+    let centerx = shiftx * (num / 2) as f32;
+    let centery = shifty / 2.0;
+    let centerz = shiftz * (num / 2) as f32;
+
+    let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
+
+    let x = -centerx + offset;
+    let y = centery + 3.0;
+    let z = -centerz + offset;
+
+    // Build the rigid body.
+    let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cylinder(rad, rad).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}

examples3d/heightfield3.rs

@@ -54,13 +54,13 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                if j % 2 == 0 {
+                let collider = match j % 3 {
-                    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                    0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
-                    colliders.insert(collider, handle, &mut bodies);
+                    1 => ColliderBuilder::ball(rad).build(),
-                } else {
+                    _ => ColliderBuilder::cylinder(rad, rad).build(),
-                    let collider = ColliderBuilder::ball(rad).build();
+                };
-                    colliders.insert(collider, handle, &mut bodies);
+
-                }
+                colliders.insert(collider, handle, &mut bodies);
             }
         }
     }

examples3d/primitives3.rs

@@ -1,4 +1,4 @@
-use na::Point3;
+use na::{Point3, Vector3};
 use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
 use rapier3d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed3d::Testbed;
@@ -15,7 +15,7 @@ pub fn init_world(testbed: &mut Testbed) {
      * Ground
      */
     let ground_size = 100.1;
-    let ground_height = 0.1;
+    let ground_height = 2.1;
 
     let rigid_body = RigidBodyBuilder::new_static()
         .translation(0.0, -ground_height, 0.0)
@@ -30,29 +30,30 @@ pub fn init_world(testbed: &mut Testbed) {
     let num = 8;
     let rad = 1.0;
 
-    let shift = rad * 2.0 + rad;
+    let shiftx = rad * 2.0 + rad;
-    let centerx = shift * (num / 2) as f32;
+    let shifty = rad * 2.0 + rad;
-    let centery = shift / 2.0;
+    let shiftz = rad * 2.0 + rad;
-    let centerz = shift * (num / 2) as f32;
+    let centerx = shiftx * (num / 2) as f32;
+    let centery = shifty / 2.0;
+    let centerz = shiftz * (num / 2) as f32;
 
     let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
 
     for j in 0usize..20 {
         for i in 0..num {
             for k in 0usize..num {
-                let x = i as f32 * shift - centerx + offset;
+                let x = i as f32 * shiftx - centerx + offset;
-                let y = j as f32 * shift + centery + 3.0;
+                let y = j as f32 * shifty + centery + 3.0;
-                let z = k as f32 * shift - centerz + offset;
+                let z = k as f32 * shiftz - centerz + offset;
 
                 // Build the rigid body.
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                let collider = match j % 2 {
+                let collider = match j % 3 {
                     0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
                     1 => ColliderBuilder::ball(rad).build(),
-                    // 2 => ColliderBuilder::capsule_y(rad, rad).build(),
+                    _ => ColliderBuilder::cylinder(rad, rad).build(),
-                    _ => unreachable!(),
                 };
 
                 colliders.insert(collider, handle, &mut bodies);

examples3d/trimesh3.rs

@@ -64,13 +64,13 @@ pub fn init_world(testbed: &mut Testbed) {
                 let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
                 let handle = bodies.insert(rigid_body);
 
-                if j % 2 == 0 {
+                let collider = match j % 3 {
-                    let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
+                    0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
-                    colliders.insert(collider, handle, &mut bodies);
+                    1 => ColliderBuilder::ball(rad).build(),
-                } else {
+                    _ => ColliderBuilder::cylinder(rad, rad).build(),
-                    let collider = ColliderBuilder::ball(rad).build();
+                };
-                    colliders.insert(collider, handle, &mut bodies);
+
-                }
+                colliders.insert(collider, handle, &mut bodies);
             }
         }
     }

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>,

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 na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
-            if early_stop && na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
                 return false;
             }
 

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))
+    if (!swapped && manifold.try_update_contacts(&pos12))
-        || (swapped && manifold.try_update_contacts(&pos21, true))
+        || (swapped && manifold.try_update_contacts(&pos21))
     {
         return;
     }

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

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))
+    if (!swapped && manifold.try_update_contacts(&pos12))
-        || (swapped && manifold.try_update_contacts(&pos21, true))
+        || (swapped && manifold.try_update_contacts(&pos21))
     {
         return;
     }

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.shape1.as_polygonal_feature_map(),
-        ctxt.collider2.shape().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) {
+    // We use very small thresholds for the manifold update because something to high would
-    //     return;
+    // 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(
         }
         _ => {}
     }
-}
+
-
+    // Transfer impulses.
-fn do_generate_contacts2(
+    super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false);
-    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);
-        }
-    }
 }

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

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.eids = [0, 0, 0, 0];
-            out_features.fid = 1;
+            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 };
+            if dir.y < 0.0 {
-            out_features.num_vertices = 4;
+                out_features.eids = [2, 4, 6, 8];
-            out_features.vids = [0, 1, 2, 3]; // FIXME
+                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];
+            }
         }
     }
 }

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;