Two simple cylinder mesh cause assertion error in gjk function

[manuel-koch]

I see assertion errors with rather simple geometries.

Using:

• MacOS Ventura 13.4.1 (22F82) on M1 arm64
• Python 3.10.6
• distance3d from master branch commit dede7640f19a19fa7ae19ec304d80a2d39f65a0d
• numba 0.57.0
• numpy 1.24.3

E.g. here are two cylinders, that are in collision:

from pyrr import Vector3

vertices_a = numpy.array(
    [
        Vector3([1.7521845e01, 7.4650002e01, -2.7000427e-02], dtype=numpy.float32),
        Vector3([1.2424170e01, 7.4650002e01, -2.7000427e-02], dtype=numpy.float32),
        Vector3([1.2424170e01, 9.9465002e02, -2.7000427e-02], dtype=numpy.float32),
        Vector3([1.7521845e01, 9.9465002e02, -2.7000427e-02], dtype=numpy.float32),
        Vector3([29.973, 74.65, 17.956755], dtype=numpy.float32),
        Vector3([29.973, 74.65, 12.424185], dtype=numpy.float32),
        Vector3([29.973, 994.65, 12.424185], dtype=numpy.float32),
        Vector3([29.973, 994.65, 17.956755], dtype=numpy.float32),
        Vector3([17.956755, 74.65, 29.973], dtype=numpy.float32),
        Vector3([12.42417, 74.65, 29.973], dtype=numpy.float32),
        Vector3([6.7186646, 74.65, 27.93186], dtype=numpy.float32),
        Vector3([2.0141246, 74.65, 23.22732], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 7.4650002e01, 1.7521845e01], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 7.4650002e01, 1.1989244e01], dtype=numpy.float32),
        Vector3([2.2566445, 74.65, 6.4761295], dtype=numpy.float32),
        Vector3([6.7186494, 74.65, 2.0141397], dtype=numpy.float32),
        Vector3([23.22732, 74.65, 2.0141246], dtype=numpy.float32),
        Vector3([27.93186, 74.65, 6.7186646], dtype=numpy.float32),
        Vector3([27.689354, 74.65, 23.46987], dtype=numpy.float32),
        Vector3([23.46987, 74.65, 27.689354], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 9.9465002e02, 1.7521845e01], dtype=numpy.float32),
        Vector3([-2.7000427e-02, 9.9465002e02, 1.1989244e01], dtype=numpy.float32),
        Vector3([12.42417, 994.65, 29.973], dtype=numpy.float32),
        Vector3([17.956755, 994.65, 29.973], dtype=numpy.float32),
        Vector3([23.46987, 994.65, 27.689354], dtype=numpy.float32),
        Vector3([27.689354, 994.65, 23.46987], dtype=numpy.float32),
        Vector3([27.93186, 994.65, 6.7186646], dtype=numpy.float32),
        Vector3([23.22732, 994.65, 2.0141246], dtype=numpy.float32),
        Vector3([6.7186494, 994.65, 2.0141397], dtype=numpy.float32),
        Vector3([2.2566445, 994.65, 6.4761295], dtype=numpy.float32),
        Vector3([2.0141246, 994.65, 23.22732], dtype=numpy.float32),
        Vector3([6.7186646, 994.65, 27.93186], dtype=numpy.float32),
    ],
    dtype=numpy.float64,
)
vertices_b = numpy.array(
    [
        Vector3([16.861032, 99.65, 26.08411], dtype=numpy.float32),
        Vector3([13.084977, 99.65, 26.08411], dtype=numpy.float32),
        Vector3([13.084977, 54.65, 26.08411], dtype=numpy.float32),
        Vector3([16.861032, 54.65, 26.08411], dtype=numpy.float32),
        Vector3([26.08411, 99.65, 12.762811], dtype=numpy.float32),
        Vector3([26.08411, 99.65, 16.861012], dtype=numpy.float32),
        Vector3([26.08411, 54.65, 16.861012], dtype=numpy.float32),
        Vector3([26.08411, 54.65, 12.762811], dtype=numpy.float32),
        Vector3([17.18319, 99.65, 3.8618884], dtype=numpy.float32),
        Vector3([13.084977, 99.65, 3.8618884], dtype=numpy.float32),
        Vector3([8.858677, 99.65, 5.373844], dtype=numpy.float32),
        Vector3([5.3738327, 99.65, 8.858688], dtype=numpy.float32),
        Vector3([3.8618884, 99.65, 13.084967], dtype=numpy.float32),
        Vector3([3.8618884, 99.65, 17.18319], dtype=numpy.float32),
        Vector3([5.5534773, 99.65, 21.266977], dtype=numpy.float32),
        Vector3([8.858666, 99.65, 24.572155], dtype=numpy.float32),
        Vector3([21.08731, 99.65, 24.572166], dtype=numpy.float32),
        Vector3([24.572155, 99.65, 21.087322], dtype=numpy.float32),
        Vector3([24.39252, 99.65, 8.679022], dtype=numpy.float32),
        Vector3([21.266977, 99.65, 5.5534773], dtype=numpy.float32),
        Vector3([3.8618884, 54.65, 13.084967], dtype=numpy.float32),
        Vector3([3.8618884, 54.65, 17.18319], dtype=numpy.float32),
        Vector3([13.084977, 54.65, 3.8618884], dtype=numpy.float32),
        Vector3([17.18319, 54.65, 3.8618884], dtype=numpy.float32),
        Vector3([21.266977, 54.65, 5.5534773], dtype=numpy.float32),
        Vector3([24.39252, 54.65, 8.679022], dtype=numpy.float32),
        Vector3([24.572155, 54.65, 21.087322], dtype=numpy.float32),
        Vector3([21.08731, 54.65, 24.572166], dtype=numpy.float32),
        Vector3([8.858666, 54.65, 24.572155], dtype=numpy.float32),
        Vector3([5.5534773, 54.65, 21.266977], dtype=numpy.float32),
        Vector3([5.3738327, 54.65, 8.858688], dtype=numpy.float32),
        Vector3([8.858677, 54.65, 5.373844], dtype=numpy.float32),
    ],
    dtype=numpy.float64,
)

collider_a = colliders.ConvexHullVertices(vertices_a)
collider_b = colliders.ConvexHullVertices(vertices_b)
distance, closest_point_a, closest_point_b, simplex = gjk.gjk(collider_a, collider_b)
colliding = numpy.allclose(distance, 0)
penetration_depth = 0.0
if colliding:
    minimum_translation_vector, minkowski_faces, success = epa.epa(
        simplex,
        collider_a,
        collider_b,
        max_faces=512,
    )
    if not numpy.allclose(minimum_translation_vector, 0):
        penetration_depth = numpy.linalg.norm(minimum_translation_vector)
print(colliding, penetration_depth)

which throws

  File "python/src/distance3d/distance3d/gjk/_gjk_jolt.py", line 213, in gjk_distance_jolt
    assert abs(np.dot(search_direction, search_direction) - v_len_sq) < 1e-12
AssertionError

[AlexanderFabisch]

Hi, thanks for reporting.

That specific assert has been a bit problematic before. I guess this is not a real issue, but just a numerical problem. Could you relax the assert by making the threshold (1e-12) larger. I would be curious for which values it works and whether the result of GJK+EPA is correct in the end.

By the way, if you have the parametric form of the two cylinders, you can also use that and compare the results.

[manuel-koch]

In our use case we need colliders that are build of meshes ( even though they could be build of primitives like the parametric Cylinder you mentioned ).

I'll play with the value you suggested to relax the assertion...

[manuel-koch]

In this particular case an enhanced assertion prints

check_value = abs(np.dot(search_direction, search_direction) - v_len_sq)
assert check_value < 1e-12, f"Assertion triggered by {check_value}"

AssertionError: Assertion triggered by 1.0544675763712512e-10

If I relax the asserted value to 1e-9 the sample code succeeds and prints

True 25.0

I'll try this new value to see if i get more meshes that trigger a problem and if the detected collisions look ok.

[AlexanderFabisch]

OK, please keep me updated here. I'm still not exactly sure what the best way to set this threshold is. At the moment I would just change it to 1e-9.

[manuel-koch]

Relaxed check against 1e-8 did the trick for my collision data. I tracked all non-zero check_value that I saw during my huge test suite run: 625261 values: 1.738719250210968e-61 ... 5.3505679052358215e-09: mean=2.2413402687908774e-11 stddev=1.2231298118824676e-10

Maybe you could make this sanity-check optional or allow caller to use an argument to tweak the value ?

[AlexanderFabisch]

Maybe you could make this sanity-check optional or allow caller to use an argument to tweak the value ?

I like the idea of making it a parameter. Would you be willing to open a pull request?

[manuel-koch]

Yes, I can try a PR. What would be a useful name for this new parameter/argument ? What is the geometrical think the assertion checks ( sorry for my ignorance with respect of the involved math ) ?

[AlexanderFabisch]

It is just a numerical check. v_len_sq should have been computed as np.dot(search_direction, search_direction).