Merge pull request #21 from dimforge/trimesh_cd_bug

Fix NaN caused by the collision-detection between a vertical triangle and a cuboid.

3 files changed, 89 insertions, 83 deletions

diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
index 2d2309b..78f26bb 100644
--- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
+++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
@@ -40,6 +40,12 @@ fn do_generate_contacts(
     ctxt: &mut ContactGenerationContext,
     flipped: bool,
 ) {
+    let ctxt_pair_pair = if flipped {
+        ctxt.pair.pair.swap()
+    } else {
+        ctxt.pair.pair
+    };
+
     let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt
         .pair
         .generator_workspace
@@ -78,7 +84,7 @@ fn do_generate_contacts(
             // and rebuilt. In this case, we hate to reconstruct the `old_interferences`
             // array using the subshape ids from the contact manifolds.
             // TODO: always rely on the subshape ids instead of maintaining `.ord_interferences` ?
-            let ctxt_collider1 = ctxt.pair.pair.collider1;
+            let ctxt_collider1 = ctxt_pair_pair.collider1;
             workspace.old_interferences = workspace
                 .old_manifolds
                 .iter()
@@ -91,13 +97,16 @@ fn do_generate_contacts(
                 })
                 .collect();
         }
-        assert_eq!(
-            workspace
-                .old_interferences
-                .len()
-                .min(trimesh1.num_triangles()),
-            workspace.old_manifolds.len()
-        );
+        // This assertion may fire due to the invalid triangle_ids that the
+        // near-phase may return (due to SIMD sentinels).
+        //
+        // assert_eq!(
+        //     workspace
+        //         .old_interferences
+        //         .len()
+        //         .min(trimesh1.num_triangles()),
+        //     workspace.old_manifolds.len()
+        // );
 
         trimesh1
             .waabbs()
@@ -118,6 +127,7 @@ fn do_generate_contacts(
             // than the max.
             continue;
         }
+
         if !same_local_aabb2 {
             loop {
                 match old_inter_it.peek() {
@@ -131,23 +141,13 @@ fn do_generate_contacts(
 
             let manifold = if old_inter_it.peek() != Some(triangle_id) {
                 // We don't have a manifold for this triangle yet.
-                if flipped {
-                    ContactManifold::with_subshape_indices(
-                        ctxt.pair.pair,
-                        collider2,
-                        collider1,
-                        *triangle_id,
-                        0,
-                    )
-                } else {
-                    ContactManifold::with_subshape_indices(
-                        ctxt.pair.pair,
-                        collider1,
-                        collider2,
-                        0,
-                        *triangle_id,
-                    )
-                }
+                ContactManifold::with_subshape_indices(
+                    ctxt_pair_pair,
+                    collider1,
+                    collider2,
+                    *triangle_id,
+                    0,
+                )
             } else {
                 // We already have a manifold for this triangle.
                 old_inter_it.next();
@@ -163,7 +163,7 @@ fn do_generate_contacts(
             .dispatcher
             .dispatch_primitives(&triangle1, collider2.shape());
 
-        let mut ctxt2 = if ctxt.pair.pair.collider1 != manifold.pair.collider1 {
+        let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 {
             PrimitiveContactGenerationContext {
                 prediction_distance: ctxt.prediction_distance,
                 collider1: collider2,
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;