Fix address bug in meshEdgeNormals in doing edge multiccd with meshes.

PiperOrigin-RevId: 790731924
Change-Id: Ia475b1f01731518166f229c25ebffd1fbf7c7939

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 idx = polymap[i];
-      int adr = m->mesh_polyvertadr[polyadr + idx];
+      int adr = polyvertadr[idx];
-      int nvert =  m->mesh_polyvertnum[polyadr + 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 (polyvert[adr + j] == v1i) {
-        if (v == v1i) {
-          float* verts = m->mesh_vert + 3*vertadr;
           int k = (j == 0) ? nvert - 1 : j - 1;
-          float* vert = verts + 3*k;
+          const float* v = vert + 3*polyvert[adr + k];
-          globalcoord(endverts + 3*i, mat, pos, vert[0], vert[1], vert[2]);
+          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;
+    const float* v = vert + 3*polyvert[adr + i];
-    int v = m->mesh_polyvert[adr + i];
+    globalcoord(res + 3*j++, mat, pos, v[0], v[1], v[2]);
-    float* vert = verts + 3*v;
-    globalcoord(res + 3*j++, mat, pos, vert[0], vert[1], vert[2]);
   }
   return nvert;
 }

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"(
+  <mujoco>
+    <option>
+      <flag multiccd="enable"/>
+    </option>
+
+    <asset>
+      <mesh name="meshbox"
+            vertex="-1 -1 -1
+                     1 -1 -1
+                     1  1 -1
+                     1  1  1
+                     1 -1  1
+                    -1  1 -1
+                    -1  1  1
+                    -1 -1  1"/>
+    </asset>
+
+    <worldbody>
+      <geom type="box" name="floor" size="6 6 2" pos="0 0 -1"/>
+      <body pos="2 0 1" euler="0 5 0">
+        <freejoint/>
+        <geom type="mesh" name="meshbox" mesh="meshbox"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  std::array<char, 1000> 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<mjtNum> 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<char, 1000> error;
   mjModel* model = LoadModelFromString(kEllipoid, error.data(), error.size());