Prevent crash in epa2 and epa3 ::closest_points if all is_project

CHANGELOG.md

@@ -7,6 +7,10 @@
 - Implement `::to_trimesh` in 2d for `Cuboid` and `Aabb`.
 - Implement `Shape::feature_normal_at_point` for `TriMesh` to retrieve the normal of a face, when passing a `FeatureId::Face`.
 
+### Fix
+
+- Fix panic in `epa3::EPA::closest_points` and `epa2::EPA::closest_points`. Related issues: [#253](https://github.com/dimforge/parry/issues/253), [#246](https://github.com/dimforge/parry/issues/246)
+
 ### Modified
 
 - Propagate error information while creating a mesh and using functions making use of it (See #262):

crates/parry3d/tests/geometry/epa3.rs

@@ -1,6 +1,7 @@
-use na::{self, Isometry3, Vector3};
+use na::{self, Isometry3, Point3, Vector3};
 use parry3d::query;
-use parry3d::shape::Cuboid;
+use parry3d::query::gjk::VoronoiSimplex;
+use parry3d::shape::{Cuboid, Triangle};
 
 #[test]
 #[allow(non_snake_case)]
@@ -20,3 +21,39 @@ fn cuboid_cuboid_EPA() {
     assert_eq!(res.dist, -1.8);
     assert_eq!(res.normal1, -Vector3::y_axis());
 }
+
+#[test]
+fn triangle_vertex_touches_triangle_edge_epa() {
+    // Related issues:
+    // https://github.com/dimforge/parry/issues/253
+    // https://github.com/dimforge/parry/issues/246
+
+    let mesh1 = Triangle::new(
+        Point3::new(-13.174434, 1.0, 8.736801),
+        Point3::new(3.5251038, 1.0, 12.1),
+        Point3::new(3.2048466, 1.0, 12.218325),
+    );
+    let mesh2 = Triangle::new(
+        Point3::new(-1.63, 0.0, 11.19),
+        Point3::new(-2.349647, 0.0, 11.037681),
+        Point3::new(-2.349647, 1.0, 11.037681),
+    );
+
+    let gjk_result = query::details::contact_support_map_support_map_with_params(
+        &Isometry3::identity(),
+        &mesh1,
+        &mesh2,
+        0.00999999977,
+        &mut VoronoiSimplex::new(),
+        None,
+    );
+
+    let query::gjk::GJKResult::ClosestPoints(a, _b, _normal) = &gjk_result else {
+        panic!("PARTIAL SUCCESS: contact_support_map_support_map_with_params did not crash but did not produce the desired result");
+    };
+
+    // The upper triangle (mesh1) lines on plane where y = 1
+    assert_abs_diff_eq!(a.y, 1.0, epsilon = 0.001);
+    // The bottom triangle touches the upper triangle in one point where x = -2.349647.
+    assert_abs_diff_eq!(a.x, -2.349647, epsilon = 0.001);
+}

src/query/epa/epa2.rs

@@ -226,28 +226,36 @@ impl EPA {
             let pts2 = [1, 2];
             let pts3 = [2, 0];
 
-            let (face1, proj_is_inside1) = Face::new(&self.vertices, pts1);
-            let (face2, proj_is_inside2) = Face::new(&self.vertices, pts2);
-            let (face3, proj_is_inside3) = Face::new(&self.vertices, pts3);
+            let (face1, proj_inside1) = Face::new(&self.vertices, pts1);
+            let (face2, proj_inside2) = Face::new(&self.vertices, pts2);
+            let (face3, proj_inside3) = Face::new(&self.vertices, pts3);
 
             self.faces.push(face1);
             self.faces.push(face2);
             self.faces.push(face3);
 
-            if proj_is_inside1 {
+            if proj_inside1 {
                 let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords);
                 self.heap.push(FaceId::new(0, -dist1)?);
             }
 
-            if proj_is_inside2 {
+            if proj_inside2 {
                 let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords);
                 self.heap.push(FaceId::new(1, -dist2)?);
             }
 
-            if proj_is_inside3 {
+            if proj_inside3 {
                 let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords);
                 self.heap.push(FaceId::new(2, -dist3)?);
             }
+
+            if !(proj_inside1 || proj_inside2 || proj_inside3) {
+                // Related issues:
+                // https://github.com/dimforge/parry/issues/253
+                // https://github.com/dimforge/parry/issues/246
+                log::debug!("Hit unexpected state in EPA: failed to project the origin on the initial simplex.");
+                return None;
+            }
         } else {
             let pts1 = [0, 1];
             let pts2 = [1, 0];

src/query/epa/epa3.rs

@@ -95,9 +95,17 @@ impl Face {
             vertices[pts[1]].point,
             vertices[pts[2]].point,
         );
-        let (_, loc) = tri.project_local_point_and_get_location(&Point::<Real>::origin(), true);
+        let (proj, loc) = tri.project_local_point_and_get_location(&Point::<Real>::origin(), true);
 
         match loc {
+            TrianglePointLocation::OnVertex(_) | TrianglePointLocation::OnEdge(_, _) => {
+                let eps_tol = crate::math::DEFAULT_EPSILON * 100.0; // Same as in closest_points
+                (
+                    // barycentric_coordinates is guaranteed to work in OnVertex and OnEdge locations
+                    Self::new_with_proj(vertices, loc.barycentric_coordinates().unwrap(), pts, adj),
+                    proj.is_inside_eps(&Point::<Real>::origin(), eps_tol),
+                )
+            }
             TrianglePointLocation::OnFace(_, bcoords) => {
                 (Self::new_with_proj(vertices, bcoords, pts, adj), true)
             }
@@ -284,6 +292,14 @@ impl EPA {
                 let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords);
                 self.heap.push(FaceId::new(3, -dist4)?);
             }
+
+            if !(proj_inside1 || proj_inside2 || proj_inside3 || proj_inside4) {
+                // Related issues:
+                // https://github.com/dimforge/parry/issues/253
+                // https://github.com/dimforge/parry/issues/246
+                log::debug!("Hit unexpected state in EPA: failed to project the origin on the initial simplex.");
+                return None;
+            }
         } else {
             if simplex.dimension() == 1 {
                 let dpt = self.vertices[1] - self.vertices[0];