Implement a special case for edge-edge 3D polygonal clipping.

build/rapier2d/Cargo.toml

@@ -36,7 +36,7 @@ vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
 nalgebra = "0.22"
-ncollide2d = "0.24"
+ncollide2d = "0.25"
 simba = "^0.2.1"
 approx = "0.3"
 rayon = { version = "1", optional = true }

build/rapier3d/Cargo.toml

@@ -36,7 +36,7 @@ vec_map = "0.8"
 instant = { version = "0.1", features = [ "now" ]}
 num-traits = "0.2"
 nalgebra = "0.22"
-ncollide3d = "0.24"
+ncollide3d = "0.25"
 simba = "^0.2.1"
 approx = "0.3"
 rayon = { version = "1", optional = true }

build/rapier_testbed2d/Cargo.toml

@@ -24,14 +24,14 @@ other-backends = [ "wrapped2d", "nphysics2d" ]
 
 [dependencies]
 nalgebra   = "0.22"
-kiss3d     = { version = "0.25", features = [ "conrod" ] }
+kiss3d     = { version = "0.26", features = [ "conrod" ] }
 rand       = "0.7"
 rand_pcg   = "0.2"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 num_cpus   = { version = "1", optional = true }
 wrapped2d  = { version = "0.4", optional = true }
-ncollide2d = "0.24"
+ncollide2d = "0.25"
 nphysics2d = { version = "0.17", optional = true }
 crossbeam = "0.7"
 bincode = "1"

build/rapier_testbed3d/Cargo.toml

@@ -23,14 +23,14 @@ other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ]
 
 [dependencies]
 nalgebra   = "0.22"
-kiss3d     = { version = "0.25", features = [ "conrod" ] }
+kiss3d     = { version = "0.26", features = [ "conrod" ] }
 rand       = "0.7"
 rand_pcg   = "0.2"
 instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 glam       = { version = "0.8", optional = true }
 num_cpus   = { version = "1", optional = true }
-ncollide3d = "0.24"
+ncollide3d = "0.25"
 nphysics3d = { version = "0.17", optional = true }
 physx      = { version = "0.6", optional = true }
 physx-sys = { version = "0.4", optional = true }

src/geometry/contact.rs

@@ -429,7 +429,7 @@ impl ContactManifold {
     }
 
     #[inline]
-    pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool {
+    pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>, early_stop: bool) -> bool {
         if self.points.len() == 0 {
             return false;
         }
@@ -439,7 +439,7 @@ impl ContactManifold {
 
         let local_n2 = pos12 * self.local_n2;
 
-        if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
+        if early_stop && -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
             return false;
         }
 
@@ -448,7 +448,7 @@ impl ContactManifold {
             let dpt = local_p2 - pt.local_p1;
             let dist = dpt.dot(&self.local_n1);
 
-            if dist * pt.dist < 0.0 {
+            if early_stop && dist * pt.dist < 0.0 {
                 // We switched between penetrating/non-penetrating.
                 // The may result in other contacts to appear.
                 return false;
@@ -456,7 +456,7 @@ impl ContactManifold {
             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_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))
+    if (!swapped && manifold.try_update_contacts(&pos12, true))
-        || (swapped && manifold.try_update_contacts(&pos21))
+        || (swapped && manifold.try_update_contacts(&pos21, true))
     {
         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) {
+    if manifold.try_update_contacts(&pos12, true) {
         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))
+    if (!swapped && manifold.try_update_contacts(&pos12, true))
-        || (swapped && manifold.try_update_contacts(&pos21))
+        || (swapped && manifold.try_update_contacts(&pos21, true))
     {
         return;
     }

src/geometry/contact_generator/mod.rs

@@ -27,10 +27,9 @@ pub use self::trimesh_shape_contact_generator::{
     generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
 };
 
+pub(crate) use self::polygon_polygon_contact_generator::clip_segments;
 #[cfg(feature = "dim2")]
-pub(crate) use self::polygon_polygon_contact_generator::{
+pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal;
-    clip_segments, clip_segments_with_normal,
-};
 
 mod ball_ball_contact_generator;
 mod ball_convex_contact_generator;

src/geometry/contact_generator/pfm_pfm_contact_generator.rs

@@ -1,6 +1,9 @@
 use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
-use crate::geometry::{Contact, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace};
+use crate::geometry::{
+    Contact, ContactManifold, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace,
+};
 use crate::math::{Isometry, Vector};
+use crate::na::UnitQuaternion;
 use na::Unit;
 use ncollide::query;
 use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex};
@@ -44,7 +47,7 @@ fn do_generate_contacts(
     let pos12 = ctxt.position1.inverse() * ctxt.position2;
     let pos21 = pos12.inverse();
 
-    // if ctxt.manifold.try_update_contacts(&pos12) {
+    // if ctxt.manifold.try_update_contacts(&pos12, true) {
     //     return;
     // }
 
@@ -77,36 +80,28 @@ fn do_generate_contacts(
             pfm2.local_support_feature(&normal2, &mut workspace.feature2);
             workspace.feature2.transform_by(&pos12);
 
-            // PolyhedronFace::contacts(
+            PolyhedronFace::contacts(
-            //     ctxt.prediction_distance,
+                ctxt.prediction_distance,
-            //     &workspace.feature1,
+                &workspace.feature1,
-            //     &normal1,
+                &normal1,
-            //     &workspace.feature2,
+                &workspace.feature2,
-            //     &pos21,
+                &pos21,
-            //     ctxt.manifold,
+                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() {
+            // if ctxt.manifold.all_contacts().is_empty() {
-                // Add at least the deepest contact.
+            //     // Add at least the deepest contact.
-                let dist = (local_p2 - local_p1).dot(&dir);
+            //     let dist = (local_p2 - local_p1).dot(&dir);
-                ctxt.manifold.points.push(Contact {
+            //     ctxt.manifold.points.push(Contact {
-                    local_p1,
+            //         local_p1,
-                    local_p2: pos21 * local_p2,
+            //         local_p2: pos21 * local_p2,
-                    impulse: 0.0,
+            //         impulse: 0.0,
-                    tangent_impulse: Contact::zero_tangent_impulse(),
+            //         tangent_impulse: Contact::zero_tangent_impulse(),
-                    fid1: 0, // FIXME
+            //         fid1: 0, // FIXME
-                    fid2: 0, // FIXME
+            //         fid2: 0, // FIXME
-                    dist,
+            //         dist,
-                });
+            //     });
-            }
+            // }
 
             // Adjust points to take the radius into account.
             ctxt.manifold.local_n1 = *normal1;
@@ -121,3 +116,115 @@ 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);
+        }
+    }
+}

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

src/geometry/mod.rs

@@ -50,8 +50,9 @@ pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
 pub(crate) use self::collider_set::RemovedCollider;
 #[cfg(feature = "simd-is-enabled")]
 pub(crate) use self::contact::WContact;
+pub(crate) use self::contact_generator::clip_segments;
 #[cfg(feature = "dim2")]
-pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal};
+pub(crate) use self::contact_generator::clip_segments_with_normal;
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 #[cfg(feature = "dim3")]
 pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap;

src/geometry/polyhedron_feature3d.rs

@@ -1,4 +1,5 @@
-use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle};
+use crate::approx::AbsDiffEq;
+use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle};
 use crate::math::{Isometry, Point, Vector};
 use crate::utils::WBasis;
 use na::Point2;
@@ -73,6 +74,140 @@ impl PolyhedronFace {
         face2: &PolyhedronFace,
         pos21: &Isometry<f32>,
         manifold: &mut ContactManifold,
+    ) {
+        match (face1.num_vertices, face2.num_vertices) {
+            (2, 2) => Self::contacts_edge_edge(
+                prediction_distance,
+                face1,
+                sep_axis1,
+                face2,
+                pos21,
+                manifold,
+            ),
+            _ => Self::contacts_face_face(
+                prediction_distance,
+                face1,
+                sep_axis1,
+                face2,
+                pos21,
+                manifold,
+            ),
+        }
+    }
+
+    fn contacts_edge_edge(
+        prediction_distance: f32,
+        face1: &PolyhedronFace,
+        sep_axis1: &Vector<f32>,
+        face2: &PolyhedronFace,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        // Project the faces to a 2D plane for contact clipping.
+        // The plane they are projected onto has normal sep_axis1
+        // and contains the origin (this is numerically OK because
+        // we are not working in world-space here).
+        let basis = sep_axis1.orthonormal_basis();
+        let projected_edge1 = [
+            Point2::new(
+                face1.vertices[0].coords.dot(&basis[0]),
+                face1.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[1].coords.dot(&basis[0]),
+                face1.vertices[1].coords.dot(&basis[1]),
+            ),
+        ];
+        let projected_edge2 = [
+            Point2::new(
+                face2.vertices[0].coords.dot(&basis[0]),
+                face2.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[1].coords.dot(&basis[0]),
+                face2.vertices[1].coords.dot(&basis[1]),
+            ),
+        ];
+
+        let tangent1 =
+            (projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon());
+        let tangent2 =
+            (projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon());
+
+        // TODO: not sure what the best value for eps is.
+        // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small.
+        if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) {
+            let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8;
+
+            if !parallel {
+                let seg1 = (&projected_edge1[0], &projected_edge1[1]);
+                let seg2 = (&projected_edge2[0], &projected_edge2[1]);
+                let (loc1, loc2) =
+                    ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2);
+
+                // Found a contact between the two edges.
+                let bcoords1 = loc1.barycentric_coordinates();
+                let bcoords2 = loc2.barycentric_coordinates();
+
+                let edge1 = (face1.vertices[0], face1.vertices[1]);
+                let edge2 = (face2.vertices[0], face2.vertices[1]);
+                let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1];
+                let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1];
+                let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+                if dist <= prediction_distance {
+                    manifold.points.push(Contact {
+                        local_p1,
+                        local_p2: pos21 * local_p2,
+                        impulse: 0.0,
+                        tangent_impulse: Contact::zero_tangent_impulse(),
+                        fid1: face1.eids[0],
+                        fid2: face2.eids[0],
+                        dist,
+                    });
+                }
+
+                return;
+            }
+        }
+
+        // The lines are parallel so we are having a conformal contact.
+        // Let's use a range-based clipping to extract two contact points.
+        // TODO: would it be better and/or more efficient to do the
+        //clipping in 2D?
+        if let Some(clips) = geometry::clip_segments(
+            (face1.vertices[0], face1.vertices[1]),
+            (face2.vertices[0], face2.vertices[1]),
+        ) {
+            manifold.points.push(Contact {
+                local_p1: (clips.0).0,
+                local_p2: pos21 * (clips.0).1,
+                impulse: 0.0,
+                tangent_impulse: Contact::zero_tangent_impulse(),
+                fid1: 0, // FIXME
+                fid2: 0, // FIXME
+                dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1),
+            });
+
+            manifold.points.push(Contact {
+                local_p1: (clips.1).0,
+                local_p2: pos21 * (clips.1).1,
+                impulse: 0.0,
+                tangent_impulse: Contact::zero_tangent_impulse(),
+                fid1: 0, // FIXME
+                fid2: 0, // FIXME
+                dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1),
+            });
+        }
+    }
+
+    fn contacts_face_face(
+        prediction_distance: f32,
+        face1: &PolyhedronFace,
+        sep_axis1: &Vector<f32>,
+        face2: &PolyhedronFace,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
     ) {
         // Project the faces to a 2D plane for contact clipping.
         // The plane they are projected onto has normal sep_axis1