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());