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

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.

src/geometry/polyhedron_feature3d.rs

@@ -110,7 +110,9 @@ 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);
 
+            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];
@@ -128,7 +130,6 @@ impl PolyhedronFace {
 
                     // 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;
@@ -146,11 +147,14 @@ impl PolyhedronFace {
                     }
                 }
             }
+        }
 
         if face1.num_vertices > 2 {
             let normal1 = (face1.vertices[2] - face1.vertices[1])
                 .cross(&(face1.vertices[0] - face1.vertices[1]));
 
+            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];
@@ -169,7 +173,6 @@ impl PolyhedronFace {
 
                     // 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;
@@ -188,6 +191,7 @@ impl PolyhedronFace {
                     }
                 }
             }
+        }
 
         // Now we have to compute the intersection between all pairs of
         // edges from the face 1 and from the face2.

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;