Fix NaN when computing contacts between a cuboid and a perfectly vertical triangle.

3 files changed, 65 insertions, 57 deletions

diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 8b0a2fa..64eb5b6 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -14,6 +14,7 @@ mod add_remove3;
 mod compound3;
 mod debug_boxes3;
 mod debug_triangle3;
+mod debug_trimesh3;
 mod domino3;
 mod heightfield3;
 mod joints3;
@@ -74,6 +75,7 @@ pub fn main() {
         ("Keva tower", keva3::init_world),
         ("(Debug) boxes", debug_boxes3::init_world),
         ("(Debug) triangle", debug_triangle3::init_world),
+        ("(Debug) trimesh", debug_trimesh3::init_world),
     ];
 
     // Lexicographic sort, with stress tests moved at the end of the list.
diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs
index 94870a4..dfeee29 100644
--- a/src/geometry/polyhedron_feature3d.rs
+++ b/src/geometry/polyhedron_feature3d.rs
@@ -110,39 +110,41 @@ impl PolyhedronFace {
         if face2.num_vertices > 2 {
             let normal2 = (face2.vertices[2] - face2.vertices[1])
                 .cross(&(face2.vertices[0] - face2.vertices[1]));
+            let denom = normal2.dot(&sep_axis1);
 
-            let last_index2 = face2.num_vertices as usize - 1;
-            'point_loop1: for i in 0..face1.num_vertices as usize {
-                let p1 = projected_face1[i];
+            if !relative_eq!(denom, 0.0) {
+                let last_index2 = face2.num_vertices as usize - 1;
+                'point_loop1: for i in 0..face1.num_vertices as usize {
+                    let p1 = projected_face1[i];
 
-                let sign = (projected_face2[0] - projected_face2[last_index2])
-                    .perp(&(p1 - projected_face2[last_index2]));
-                for j in 0..last_index2 {
-                    let new_sign = (projected_face2[j + 1] - projected_face2[j])
-                        .perp(&(p1 - projected_face2[j]));
-                    if new_sign * sign < 0.0 {
-                        // The point lies outside.
-                        continue 'point_loop1;
+                    let sign = (projected_face2[0] - projected_face2[last_index2])
+                        .perp(&(p1 - projected_face2[last_index2]));
+                    for j in 0..last_index2 {
+                        let new_sign = (projected_face2[j + 1] - projected_face2[j])
+                            .perp(&(p1 - projected_face2[j]));
+                        if new_sign * sign < 0.0 {
+                            // The point lies outside.
+                            continue 'point_loop1;
+                        }
                     }
-                }
 
-                // All the perp had the same sign: the point is inside of the other shapes projection.
-                // Output the contact.
-                let denom = normal2.dot(&sep_axis1);
-                let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
-                let local_p1 = face1.vertices[i];
-                let local_p2 = face1.vertices[i] + dist * sep_axis1;
+                    // All the perp had the same sign: the point is inside of the other shapes projection.
+                    // Output the contact.
+                    let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
+                    let local_p1 = face1.vertices[i];
+                    let local_p2 = face1.vertices[i] + dist * 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.vids[i],
-                        fid2: face2.fid,
-                        dist,
-                    });
+                    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.vids[i],
+                            fid2: face2.fid,
+                            dist,
+                        });
+                    }
                 }
             }
         }
@@ -151,40 +153,42 @@ impl PolyhedronFace {
             let normal1 = (face1.vertices[2] - face1.vertices[1])
                 .cross(&(face1.vertices[0] - face1.vertices[1]));
 
-            let last_index1 = face1.num_vertices as usize - 1;
-            'point_loop2: for i in 0..face2.num_vertices as usize {
-                let p2 = projected_face2[i];
+            let denom = -normal1.dot(&sep_axis1);
+            if !relative_eq!(denom, 0.0) {
+                let last_index1 = face1.num_vertices as usize - 1;
+                'point_loop2: for i in 0..face2.num_vertices as usize {
+                    let p2 = projected_face2[i];
 
-                let sign = (projected_face1[0] - projected_face1[last_index1])
-                    .perp(&(p2 - projected_face1[last_index1]));
-                for j in 0..last_index1 {
-                    let new_sign = (projected_face1[j + 1] - projected_face1[j])
-                        .perp(&(p2 - projected_face1[j]));
+                    let sign = (projected_face1[0] - projected_face1[last_index1])
+                        .perp(&(p2 - projected_face1[last_index1]));
+                    for j in 0..last_index1 {
+                        let new_sign = (projected_face1[j + 1] - projected_face1[j])
+                            .perp(&(p2 - projected_face1[j]));
 
-                    if new_sign * sign < 0.0 {
-                        // The point lies outside.
-                        continue 'point_loop2;
+                        if new_sign * sign < 0.0 {
+                            // The point lies outside.
+                            continue 'point_loop2;
+                        }
                     }
-                }
 
-                // All the perp had the same sign: the point is inside of the other shapes projection.
-                // Output the contact.
-                let denom = -normal1.dot(&sep_axis1);
-                let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
-                let local_p2 = face2.vertices[i];
-                let local_p1 = face2.vertices[i] - dist * sep_axis1;
+                    // All the perp had the same sign: the point is inside of the other shapes projection.
+                    // Output the contact.
+                    let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
+                    let local_p2 = face2.vertices[i];
+                    let local_p1 = face2.vertices[i] - dist * sep_axis1;
 
-                if true {
-                    // 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.fid,
-                        fid2: face2.vids[i],
-                        dist,
-                    });
+                    if true {
+                        // 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.fid,
+                            fid2: face2.vids[i],
+                            dist,
+                        });
+                    }
                 }
             }
         }
diff --git a/src/lib.rs b/src/lib.rs
index aae0bd0..118ac23 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -18,6 +18,8 @@ pub extern crate ncollide3d as ncollide;
 #[cfg(feature = "serde")]
 #[macro_use]
 extern crate serde;
+#[macro_use]
+extern crate approx;
 extern crate num_traits as num;
 // #[macro_use]
 // extern crate array_macro;