diff --git a/CHANGELOG.md b/CHANGELOG.md index c827df36..1ece976b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,10 @@ - Fix some edge-cases in `point_in_poly2d` for self-intersecting polygons. - Fix some edge-cases in mesh/mesh intersection that could result in degenerate triangles being generated. +### 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): diff --git a/crates/parry3d/tests/geometry/epa3.rs b/crates/parry3d/tests/geometry/epa3.rs index 9f7ed249..e77c38f3 100644 --- a/crates/parry3d/tests/geometry/epa3.rs +++ b/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); +} diff --git a/src/query/epa/epa2.rs b/src/query/epa/epa2.rs index 02357f08..49961a80 100644 --- a/src/query/epa/epa2.rs +++ b/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]; diff --git a/src/query/epa/epa3.rs b/src/query/epa/epa3.rs index 7c05daa0..5ed482d4 100644 --- a/src/query/epa/epa3.rs +++ b/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::::origin(), true); + let (proj, loc) = tri.project_local_point_and_get_location(&Point::::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::::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];