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

1 files changed, 136 insertions, 1 deletions

diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs
index 5655f64..8f5de04 100644
--- a/src/geometry/polyhedron_feature3d.rs
+++ b/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;
@@ -74,6 +75,140 @@ impl 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
         // and contains the origin (this is numerically OK because