Merge pull request #21 from dimforge/trimesh_cd_bug

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

5 files changed, 159 insertions, 83 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/examples3d/debug_trimesh3.rs b/examples3d/debug_trimesh3.rs
new file mode 100644
index 0000000..0d0b276
--- /dev/null
+++ b/examples3d/debug_trimesh3.rs
@@ -0,0 +1,68 @@
+use na::Point3;
+use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier3d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    // Triangle ground.
+    let width = 0.5;
+    let vtx = vec![
+        Point3::new(-width, 0.0, -width),
+        Point3::new(width, 0.0, -width),
+        Point3::new(width, 0.0, width),
+        Point3::new(-width, 0.0, width),
+        Point3::new(-width, -width, -width),
+        Point3::new(width, -width, -width),
+        Point3::new(width, -width, width),
+        Point3::new(-width, -width, width),
+    ];
+    let idx = vec![
+        Point3::new(0, 1, 2),
+        Point3::new(0, 2, 3),
+        Point3::new(4, 5, 6),
+        Point3::new(4, 6, 7),
+        Point3::new(0, 4, 7),
+        Point3::new(0, 7, 3),
+        Point3::new(1, 5, 6),
+        Point3::new(1, 6, 2),
+        Point3::new(3, 2, 7),
+        Point3::new(2, 6, 7),
+        Point3::new(0, 1, 5),
+        Point3::new(0, 5, 4),
+    ];
+
+    // Dynamic box rigid body.
+    let rigid_body = RigidBodyBuilder::new_dynamic()
+        .translation(0.0, 35.0, 0.0)
+        // .rotation(Vector3::new(0.8, 0.2, 0.1))
+        .can_sleep(false)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.0).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .translation(0.0, 0.0, 0.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::trimesh(vtx, idx).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin());
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
+    testbed.run()
+}
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;