search

AlexanderFabisch / distance3d

Distance computation and collision detection in 3D.
https://alexanderfabisch.github.io/distance3d/
Other
55 stars 7 forks source link

mpr.mpr_penetration stuck in endless loop for two meshes #79

Open manuel-koch opened 1 year ago

manuel-koch commented 1 year ago

The following code for two meshes ( one is completely covered by the other ) is stuck in endless loop:

Using:

from pyrr import Vector3
from trimesh import Scene, Trimesh

vertices_a = numpy.array(
    [
        Vector3([23.69677426, 1310.32310771, 552.84175763]),
        Vector3([16.30322573, 1310.32310771, 552.84175763]),
        Vector3([12.17142738, 1310.3231053, 548.7099655]),
        Vector3([12.17142738, 1310.3231053, 541.2900345]),
        Vector3([16.30322573, 1310.32310771, 537.15824237]),
        Vector3([23.69677426, 1310.32310771, 537.15824237]),
        Vector3([27.82857167, 1310.32310531, 541.2900345]),
        Vector3([27.82857167, 1310.32310531, 548.7099655]),
        Vector3([23.25910977, 1310.56902972, 552.83423821]),
        Vector3([16.74089023, 1310.56902972, 552.83423821]),
        Vector3([12.178948, 1310.56842155, 548.27298778]),
        Vector3([12.178948, 1310.56842155, 541.72701222]),
        Vector3([23.25910977, 1310.56902972, 537.16576179]),
        Vector3([16.74089023, 1310.56902972, 537.16576179]),
        Vector3([27.82105104, 1310.56842156, 541.72701222]),
        Vector3([27.82105104, 1310.56842156, 548.27298778]),
        Vector3([16.75429825, 1315.14605025, 546.36398605]),
        Vector3([18.64989135, 1315.14605026, 548.25954888]),
        Vector3([21.35010865, 1315.14605026, 548.25954888]),
        Vector3([23.24570175, 1315.14605025, 546.36398605]),
        Vector3([23.24570175, 1315.14605025, 543.63601395]),
        Vector3([21.35010865, 1315.14605026, 541.74045112]),
        Vector3([18.64989135, 1315.14605026, 541.74045112]),
        Vector3([16.75429825, 1315.14605025, 543.63601395]),
    ],
    dtype=numpy.float64,
)
faces_a = numpy.array(
    [
        [1, 16, 2],
        [16, 1, 17],
        [17, 1, 9],
        [2, 16, 10],
        [7, 19, 0],
        [7, 15, 19],
        [0, 19, 18],
        [20, 14, 6],
        [5, 20, 6],
        [21, 20, 5],
        [5, 12, 21],
        [4, 3, 23],
        [23, 22, 4],
        [23, 3, 11],
        [4, 22, 13],
        [0, 18, 8],
        [10, 3, 2],
        [11, 3, 10],
        [14, 7, 6],
        [15, 7, 14],
        [15, 14, 20],
        [19, 15, 20],
        [2, 3, 5],
        [5, 1, 2],
        [6, 7, 5],
        [0, 1, 5],
        [5, 3, 4],
        [5, 7, 0],
        [23, 18, 19],
        [16, 17, 23],
        [19, 20, 23],
        [21, 22, 23],
        [17, 18, 23],
        [23, 20, 21],
        [11, 10, 23],
        [23, 10, 16],
        [13, 5, 4],
        [12, 5, 13],
        [13, 22, 21],
        [21, 12, 13],
        [8, 1, 0],
        [9, 1, 8],
        [8, 18, 17],
        [8, 17, 9],
    ],
)
vertices_b = numpy.array(
    [
        Vector3([23.70402525, 1310.25599226, 552.8347335]),
        Vector3([16.29597475, 1310.25599226, 552.8347335]),
        Vector3([12.17844916, 1310.25599021, 548.71721605]),
        Vector3([12.17844916, 1310.25599021, 541.28278395]),
        Vector3([16.29597475, 1310.25599226, 537.1652665]),
        Vector3([23.70402525, 1310.25599226, 537.1652665]),
        Vector3([27.82154988, 1310.25599022, 541.28278395]),
        Vector3([27.82154988, 1310.25599022, 548.71721605]),
        Vector3([23.25910977, 1310.56902972, 552.83423821]),
        Vector3([16.74089023, 1310.56902972, 552.83423821]),
        Vector3([12.178948, 1310.56842155, 548.27298778]),
        Vector3([12.178948, 1310.56842155, 541.72701222]),
        Vector3([23.25910977, 1310.56902972, 537.16576179]),
        Vector3([16.74089023, 1310.56902972, 537.16576179]),
        Vector3([27.82105104, 1310.56842156, 541.72701222]),
        Vector3([27.82105104, 1310.56842156, 548.27298778]),
        Vector3([16.75429825, 1315.14605025, 546.36398605]),
        Vector3([18.64989135, 1315.14605026, 548.25954888]),
        Vector3([21.35010865, 1315.14605026, 548.25954888]),
        Vector3([23.24570175, 1315.14605025, 546.36398605]),
        Vector3([23.24570175, 1315.14605025, 543.63601395]),
        Vector3([21.35010865, 1315.14605026, 541.74045112]),
        Vector3([18.64989135, 1315.14605026, 541.74045112]),
        Vector3([16.75429825, 1315.14605025, 543.63601395]),
        Vector3([23.71710962, 181.69901258, 552.84825455]),
        Vector3([16.28289038, 181.69901258, 552.84825455]),
        Vector3([12.16492808, 181.69901257, 548.73029888]),
        Vector3([12.16492808, 181.69901257, 541.26970112]),
        Vector3([16.28289038, 181.69901258, 537.15174545]),
        Vector3([23.71710962, 181.69901258, 537.15174545]),
        Vector3([27.83507097, 181.69901257, 541.26970112]),
        Vector3([27.83507097, 181.69901257, 548.73029888]),
    ],
    dtype=numpy.float64,
)
faces_b = numpy.array(
    [
        [1, 16, 2],
        [16, 1, 17],
        [17, 1, 9],
        [2, 16, 10],
        [7, 19, 0],
        [7, 15, 19],
        [0, 19, 18],
        [20, 14, 6],
        [5, 20, 6],
        [21, 20, 5],
        [5, 12, 21],
        [4, 3, 23],
        [23, 22, 4],
        [23, 3, 11],
        [4, 22, 13],
        [0, 18, 8],
        [10, 3, 2],
        [11, 3, 10],
        [14, 7, 6],
        [15, 7, 14],
        [15, 14, 20],
        [19, 15, 20],
        [23, 18, 19],
        [16, 17, 23],
        [19, 20, 23],
        [21, 22, 23],
        [17, 18, 23],
        [23, 20, 21],
        [11, 10, 23],
        [23, 10, 16],
        [13, 5, 4],
        [12, 5, 13],
        [13, 22, 21],
        [21, 12, 13],
        [8, 1, 0],
        [9, 1, 8],
        [8, 18, 17],
        [8, 17, 9],
        [26, 27, 29],
        [29, 25, 26],
        [30, 31, 29],
        [24, 25, 29],
        [29, 27, 28],
        [29, 31, 24],
        [25, 24, 1],
        [1, 24, 0],
        [26, 25, 2],
        [2, 25, 1],
        [27, 26, 3],
        [3, 26, 2],
        [28, 27, 4],
        [4, 27, 3],
        [29, 28, 5],
        [5, 28, 4],
        [30, 29, 6],
        [6, 29, 5],
        [24, 31, 0],
        [0, 31, 7],
        [31, 30, 7],
        [7, 30, 6],
    ],
)

scene = Scene()
mesh_a = Trimesh(vertices=vertices_a, faces=faces_a)
mesh_a.visual.face_colors = (255, 0, 0, 80)
scene.add_geometry(mesh_a)
mesh_b = Trimesh(vertices=vertices_b, faces=faces_b)
mesh_b.visual.face_colors = (0, 255, 0, 80)
scene.add_geometry(mesh_b)
scene.show()

collider_a = colliders.ConvexHullVertices(vertices_a)
collider_b = colliders.ConvexHullVertices(vertices_b)
# call to mpr.mpr_penetration gets stuck in endless loop
colliding, penetration_depth, penetration_dir, contact_position = mpr.mpr_penetration(
    collider_a, collider_b
)
print(f"A vs B: colliding={colliding} penetration_depth={penetration_depth}")

Scene looks like

stuck_view_1_solid stuck_view_1_wireframe

Scene from another view point

stuck_view_2_solid stuck_view_2_wireframe
AlexanderFabisch commented 1 year ago

I limited the number of iterations in https://github.com/AlexanderFabisch/distance3d/commit/4d99472b0d0ae1e148660413c8db6e8f03773c9e . It seems to work for this case.

manuel-koch commented 1 year ago

I can confirm that at least two of my test data sets that formerly stuck in endless loop, are running ok with this fix. Thank you for the quick fix !

AlexanderFabisch commented 1 year ago

I'll leave this issue open as there might be a better solution, but I don't have time to investigate this at the moment