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

[wlinna]

EDIT: I implemented a fix for epa3 that should also make the test case work as expected

Alleviates/fixes these issues:
#253
#246

Related Discord thread:
https://discord.com/channels/507548572338880513/1274773766811160646

EPA::closest_point crashes at line let mut best_face_id = *self.heap.peek().unwrap(); because heap is empty. This happens when we reach the second branch of closest_point and all of the proj_inside variables are false.

The measure taken in this PR is to log the problem and return None. While the result might be incorrect, it avoids the panic.
This is only a half-measure and someone who understands EPA and/or this implementation of it should write a proper fix.

The included unit test is programmed to fail because in reality there should be an intersection.

[sebcrozet]

Good catch, thanks!

[wlinna]

I think I realized why closest_point wasn't working in the first place, and I figured out a solution. From the resolved conversation:

Was there an assumption, that if the location is OnEdge or OnVertex, origin was not touching the triangle in the first place? In this case the origin seems to be touching an edge already, so maybe instead of relying on OnFace, we should check whether the projection is sufficiently close to the origin? Two of the projections are so close to origin, that the distance could be attributed to numerical instability:

[crates/parry3d/../../src/query/epa/epa3.rs:101:9] &proj = PointProjection {
    is_inside: true,
    point: [
        0.0,
        0.0,
        -1.1920929e-7,
    ],
}
[crates/parry3d/../../src/query/epa/epa3.rs:101:9] &proj = PointProjection {
    is_inside: true,
    point: [
        0.0,
        0.0,
        -1.1920929e-7,
    ],
}

If I change the body of Face::new to this, all the tests, including the new one, pass

 pub fn new(vertices: &[CSOPoint], pts: [usize; 3], adj: [usize; 3]) -> (Self, bool) {
        let tri = Triangle::new(
            vertices[pts[0]].point,
            vertices[pts[1]].point,
            vertices[pts[2]].point,
        );

        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;
                let is_inside = proj.is_inside_eps(&Point::<Real>::origin(), eps_tol);
                (Self::new_with_proj(vertices, loc.barycentric_coordinates().unwrap(), pts, adj), is_inside)
            },            
            TrianglePointLocation::OnFace(_, bcoords) => {
                (Self::new_with_proj(vertices, bcoords, pts, adj), true)
            }
            _ => (Self::new_with_proj(vertices, [0.0; 3], pts, adj), false),
        }
    }

Do you think this could work?

[wlinna]

I went ahead and pushed this change. We can always revert the commit if it doesn't make sense.
Please review the change 😃

[wlinna]

Sorry, this PR has turned into a mess because I did a merge commit. I did a hard reset before the merge commit and force pushed. The commit appears clean again