validateNearestFeatureOfPolytopeBeingEdge(): The origin is outside of the polytope

[wxmerkt]

This issue has been transferred other from #388 (comment).

After applying #388, I ran into the following exception (newly introduced in #388):

Original error message: [fcl]/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:(1593): validateNearestFeatureOfPolytopeBeingEdge(): The origin is outside of the polytope. This should already have been identified as separating.
Shape 1: Box0.02999999999999999889 0.11999999999999999556 0.10000000000000000555
X_FS1
-3.0627937852578681533e-08 -0.99999999999999888978 -2.8893865161583314238e-08 0.63499979627350811029
0.9999999999999980016 -3.0627939739957803544e-08 6.4729926918527511769e-08 -0.48500002215636439651
-6.4729927722963847085e-08 -2.8893863029448751323e-08 0.99999999999999711342 1.0778146458339641356
0 0 0 1
Shape 2: Box0.025000000000000001388 0.3499999999999999778 1.8449999999999999734
X_FS2
6.1232339957367660359e-17 -1 0 0.80000000000000004441
1 6.1232339957367660359e-17 0 -0.45750000000000001776
0 0 1 1.0224999999999999645
0 0 0 1
Solver: GjkSolver_libccd
collision_tolerance: 2.0097183471152322134e-14
max collision iterations: 500
distance tolerance: 9.9999999999999995475e-07
max distance iterations: 1000

Further comments:

• When libccd reports the nearest feature is an edge, validate the result #388 (comment)
• When libccd reports the nearest feature is an edge, validate the result #388 (comment)
• When libccd reports the nearest feature is an edge, validate the result #388 (comment)

[hongkai-dai]

I confirm that in @wxmerkt example, __ccdGJK gives the wrong polytope to EPA. __ccdGJK generates a tetrahedron, and it thinks that the origin is in this tetrahedron (indicating the two boxes are colliding). But here are the four vertices of this tetrahedron
[-0.4, -2.07575e-08, 1.02781]
[0.0699998, -1.70822e-08, -0.817185]
[0.0699998, -1.70822e-08, 1.02781]
[-0.28, -1.70822e-08, 0.105315]
As you can see, all four vertices has negative value along the y axis. Hence the whole tetrahedron is on the -y side. This tetrahedron cannot contain the origin.

I will look into the doSimplex code to find out the cause of the bug.

[SeanCurtis-TRI]

That seems exactly a squared-distance tolerance masquerading as a distance tolerance.

[hongkai-dai]

I think I find the problem, and it is in libccd. When computing the distance from a point to a triangle, libccd has this code
https://github.com/danfis/libccd/blob/7931e764a19ef6b21b443376c699bbc9c6d4fba8/src/vec3.c#L190-L196
The meaning of this code is that to compute the distance from a point P to a triangle ABC. It first computes a point Q that is on the plane coinciding with ABC, as Q = A + s * AB + t * AC. It already determines that Q is within the triangle ABC, the the distance from P to ABC is actually |PQ|. It computes the squared distance of PQ as

|PQ|² = | PA + s * AB + t * AC |²
            = |PA|² + s²|AB|² + t² |AC|² + 2 st AB.dot(AC) + 2s * AB.dot(PA) + 2t * AC.dot(PA)

The summation in the expanded expression causes trouble, when the squared distance is close to epsilon. Due to summation of double numbers (some of these numbers like |PA|² could be big), that epsilon could be discarded, and libccd computes |PQ|² = 0. Hence libccd reports that the point P (origin in this case) has 0 distance to the triangle, hence the origin is incorrectly identified as being in the polytope.

I think the solution is to compute |PQ|² without the expansion. Namely we first compute the vector

PQ = PA + s * AB + t * AC

store the result to a 3 x 1 vector, and then compute the squared norm of this vector as the squared distance.

[sherm1]

It would be so nice to get this fixed in libccd rather than have to patch around it in FCL :(
Maybe a PR to libccd with the fix would be accepted? Otherwise should we have FCL embed its own copy of libccd so that we can efficiently make fixes there rather than have to wait for the notoriously slow turnaround on libccd? I don't think it is actually much code.

[hongkai-dai]

I filed the issue danfis/libccd#55 to libccd. But I think there is a workaround in FCL without changing the libccd code. I will include it in my PR #388

[hongkai-dai]

@wxmerkt I just pushed a commit to #388 to fix this error. When you got time, is it OK if you could pull in #388, and run your test locally? Thanks a lot!

There are many pieces of code in FCL/libccd that we didn't fix, so the code is still buggy. We very much appreciate your effort to test FCL for your use case. This helps us to find out the bugs in FCL/libccd.

[wxmerkt]

Thank you Hongkai. I have now updated my local copy and will re-run the tests - it may be a day before we see the bug appearing though.

Meanwhile, before applying the fix I caught another case:

Original error message: [fcl]/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:(1593): validateNearestFeatureOfPolytopeBeingEdge(): The origin is outside of the polytope. This should already have been identified as separating.
Shape 1: Cylinder(r: 0.050000000000000002776, lz: 0.05999999999999999778)
X_FS1
-0.97313010759279283679 -0.12202804064972551379 0.19526123781136842106 0.87472781461138560122
0.20950801135757171623 -0.11745920593569325607 0.97072639199619581429 -0.4038687881347159947
-0.095520609678929752073 0.98555187191549953329 0.13986894183635001365 1.5871328698116491385
0 0 0 1
Shape 2: Box0.025000000000000001388 0.3499999999999999778 1.8449999999999999734
X_FS2
6.1232339957367660359e-17 -1 0 0.80000000000000004441
1 6.1232339957367660359e-17 0 -0.45750000000000001776
0 0 1 1.0224999999999999645
0 0 0 1
Solver: GjkSolver_libccd
collision_tolerance: 2.0097183471152322134e-14
max collision iterations: 500
distance tolerance: 9.9999999999999995475e-07
max distance iterations: 1000

[hongkai-dai]

@wxmerkt , I added the failure reported in #390 (comment) to the commit in #388. I think #388 should fix that failure as well.

[wxmerkt]

@hongkai-dai I have now been running your fix for 35h+ and there were 0 exceptions thrown (before applying your patch I got about ~8 in 10 minutes).

[hongkai-dai]

@wxmerkt thanks a lot for the test! We will merge that PR soon once we resolved some cmake issue with libccd.

[hongkai-dai]

Resolved by #388