From f2a2afeb0314d0fefa209ef33ff61937e83cccb6 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 4 Aug 2025 07:02:16 -0700 Subject: [PATCH] Fix address bug in meshEdgeNormals in doing edge multiccd with meshes. PiperOrigin-RevId: 790731924 Change-Id: Ia475b1f01731518166f229c25ebffd1fbf7c7939 --- src/engine/engine_collision_gjk.c | 29 ++++++++------ test/engine/engine_collision_gjk_test.cc | 49 ++++++++++++++++++++++++ 2 files changed, 66 insertions(+), 12 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index a0a76004..c57e883c 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1833,21 +1833,26 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj int v1_num = m->mesh_polymapnum[vertadr + v1i]; if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT; + const int* polymap = m->mesh_polymap + v1_adr; + const int* polyvert = m->mesh_polyvert; + const int* polyvertadr = m->mesh_polyvertadr + polyadr; + const int* polyvertnum = m->mesh_polyvertnum + polyadr; + const float* vert = m->mesh_vert + 3*vertadr; + // loop through all faces with vertex v1 for (int i = 0; i < v1_num; i++) { - int idx = m->mesh_polymap[v1_adr + i]; - int adr = m->mesh_polyvertadr[polyadr + idx]; - int nvert = m->mesh_polyvertnum[polyadr + idx]; + int idx = polymap[i]; + int adr = polyvertadr[idx]; + int nvert = polyvertnum[idx]; // find previous vertex in polygon to form edge for (int j = 0; j < nvert; j++) { - int v = m->mesh_polyvert[adr + j]; - if (v == v1i) { - float* verts = m->mesh_vert + 3*vertadr; + if (polyvert[adr + j] == v1i) { int k = (j == 0) ? nvert - 1 : j - 1; - float* vert = verts + 3*k; - globalcoord(endverts + 3*i, mat, pos, vert[0], vert[1], vert[2]); + const float* v = vert + 3*polyvert[adr + k]; + globalcoord(endverts + 3*i, mat, pos, v[0], v[1], v[2]); sub3(res + 3*i, endverts + 3*i, v1); mju_normalize3(res + 3*i); + break; } } } @@ -2041,15 +2046,15 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) { const mjtNum* pos = obj->data->geom_xpos + g; int polyadr = m->mesh_polyadr[m->geom_dataid[obj->geom]]; int vertadr = m->mesh_vertadr[m->geom_dataid[obj->geom]]; + const float* vert = m->mesh_vert + 3*vertadr; + const int* polyvert = m->mesh_polyvert; int adr = m->mesh_polyvertadr[polyadr + idx], j = 0; int nvert = m->mesh_polyvertnum[polyadr + idx]; if (nvert > mjMAX_POLYVERT) nvert = mjMAX_POLYVERT; for (int i = nvert - 1; i >= 0; i--) { - float* verts = m->mesh_vert + 3*vertadr; - int v = m->mesh_polyvert[adr + i]; - float* vert = verts + 3*v; - globalcoord(res + 3*j++, mat, pos, vert[0], vert[1], vert[2]); + const float* v = vert + 3*polyvert[adr + i]; + globalcoord(res + 3*j++, mat, pos, v[0], v[1], v[2]); } return nvert; } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 9e5fa5e1..79f1e7c8 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -1763,6 +1763,55 @@ TEST_F(MjGjkTest, MeshEdge) { mj_deleteModel(model); } +TEST_F(MjGjkTest, MeshEdge2) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int g1 = mj_name2id(model, mjOBJ_GEOM, "floor"); + int g2 = mj_name2id(model, mjOBJ_GEOM, "meshbox"); + + mjCCDStatus status; + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4); + + EXPECT_EQ(ncons, 2); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { std::array error; mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size());