diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index f7b180ee..1603746c 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -1572,6 +1572,11 @@ static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd,
 static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
                         const mjtNum* face2, int nface2, const mjtNum n[3],
                        const mjtNum dir[3]) {
+  // clipping face needs to be at least a triangle
+  if (nface1 < 3) {
+    return;
+  }
+
   // compute plane normal and distance to plane for each vertex
   mjtNum pn[3 * mjMAX_POLYVERT], pd[mjMAX_POLYVERT];
   for (int i = 0; i < nface1 - 1; i++) {
@@ -1658,11 +1663,23 @@ static void polygonClip(mjCCDStatus* status, const mjtNum* face1, int nface1,
   }
 
   // no pruning needed
-  status->nx = npolygon;
+  int k = 0;
   for (int i = 0; i < 3*npolygon; i += 3) {
-    copy3(status->x2 + i, polygon + i);
-    sub3(status->x1 + i, status->x2 + i, dir);
+    int skip = 0;
+
+    // find possible duplicate vertices
+    for (int j = 0; j < k; j += 3) {
+      if (equal3(status->x2 + j, polygon + i)) {
+        skip = 1;
+        break;
+      }
+    }
+    if (skip) continue;
+    copy3(status->x2 + k, polygon + i);
+    sub3(status->x1 + k, status->x2 + k, dir);
+    k += 3;
   }
+  status->nx = k/3;
 }
 
 
@@ -1768,6 +1785,56 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj,
 
 
 
+// compute normal directional vectors along possible edges given by up to two vertices
+static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj,
+                           const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
+  // mesh data
+  int g = obj->geom;
+  const mjtNum* mat = obj->data->geom_xmat + 9*g;
+  const mjtNum* pos = obj->data->geom_xpos + 3*g;
+
+  // only one edge
+  if (dim == 2) {
+    copy3(endverts, v2);
+    sub3(res, v2, v1);
+    mju_normalize3(res);
+    return 1;
+  }
+
+  if (dim == 1) {
+    const mjModel* m = obj->model;
+    int polyadr = m->mesh_polyadr[m->geom_dataid[g]];
+    int vertadr = m->mesh_vertadr[m->geom_dataid[g]];
+
+    int v1_adr = m->mesh_polymapadr[vertadr + v1i];
+    int v1_num = m->mesh_polymapnum[vertadr + v1i];
+    if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT;
+
+    // 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];
+      // 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;
+          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]);
+          sub3(res + 3*i, endverts + 3*i, v1);
+          mju_normalize3(res + 3*i);
+        }
+      }
+    }
+    return 3;
+  }
+  return 0;
+}
+
+
+
 // compute possible face normals of a box given up to 3 vertices
 static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
                       int v1, int v2, int v3) {
@@ -1825,6 +1892,45 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
 
 
 
+// compute possible edge normals for box for edge collisions
+static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj* obj,
+                          const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
+  // box data
+  int g = 3*obj->geom;
+  const mjtNum* mat = obj->data->geom_xmat + 3*g;
+  const mjtNum* pos = obj->data->geom_xpos + g;
+  const mjtNum* size = obj->model->geom_size + g;
+
+  if (dim == 2) {
+    copy3(endverts, v2);
+    sub3(res, v2, v1);
+    mju_normalize3(res);
+    return 1;
+  }
+
+  // return 3 adjacent vertices
+  if (dim == 1) {
+    mjtNum x = (v1i & 1) ? size[0] : -size[0];
+    mjtNum y = (v1i & 2) ? size[1] : -size[1];
+    mjtNum z = (v1i & 4) ? size[2] : -size[2];
+
+    globalcoord(endverts, mat, pos, -x, y, z);
+    sub3(res, endverts, v1);
+    mju_normalize3(res);
+
+    globalcoord(endverts + 3, mat, pos, x, -y, z);
+    sub3(res + 3, endverts + 3, v1);
+    mju_normalize3(res + 3);
+
+    globalcoord(endverts + 6, mat, pos, x, y, -z);
+    sub3(res + 6, endverts + 6, v1);
+    mju_normalize3(res + 6);
+    return 3;
+  }
+  return 0;
+}
+
+
 // recover face of a box from its index
 static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) {
   // box data
@@ -1903,11 +2009,11 @@ static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) {
 
 
 // find two normals that are facing each other within a tolerance, return 1 if found
-static inline int alignedNormals(int res[2], const mjtNum* v, int nv,
+static inline int alignedFaces(int res[2], const mjtNum* v, int nv,
                                  const mjtNum* w, int nw) {
   for (int i = 0; i < nv; i++) {
     for (int j = 0; j < nw; j++) {
-      if (dot3(v + 3*i, w + 3*j) < -mjCOSINE_TOL) {
+      if (dot3(v + 3*i, w + 3*j) < -mjFACE_TOL) {
         res[0] = i;
         res[1] = j;
         return 1;
@@ -1919,16 +2025,33 @@ static inline int alignedNormals(int res[2], const mjtNum* v, int nv,
 
 
 
-// return number of dimensions of a feature (1, 2 or 3)
-static inline int simplexDim(int* v1, int* v2, int* v3) {
-  int val1 = *v1;
-  int val2 = *v2;
-  int val3 = *v3;
+// find two normals that are perpendicular to each other within a tolerance, return 1 if found
+static inline int alignedFaceEdge(int res[2], const mjtNum* edge, int nedge,
+                                  const mjtNum* face, int nface) {
+  for (int i = 0; i < nface; i++) {
+    for (int j = 0; j < nedge; j++) {
+      if (mju_abs(dot3(edge + 3*j, face + 3*i)) < mjEDGE_TOL) {
+        res[0] = j;
+        res[1] = i;
+        return 1;
+      }
+    }
+  }
+  return 0;
+}
+
+
+// return number (1, 2 or 3) of dimensions of a simplex; reorder vertices if necessary
+static inline int simplexDim(int* v1i, int* v2i, int* v3i, mjtNum** v1, mjtNum** v2, mjtNum** v3) {
+  int val1 = *v1i;
+  int val2 = *v2i;
+  int val3 = *v3i;
 
   if (val1 != val2) {
     return (val3 == val1 || val3 == val2) ? 2 : 3;
   }
   if (val1 != val3) {
+    *v2i = *v3i;
     *v2 = *v3;
     return 2;
   }
@@ -1940,65 +2063,122 @@ static inline int simplexDim(int* v1, int* v2, int* v3) {
 // recover multiple contacts from EPA polytope
 static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
                          mjCCDObj* obj1, mjCCDObj* obj2) {
-  mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3];
-
+  mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3], endverts[mjMAX_POLYVERT * 3];
   // get vertices of faces from EPA
-  int v11 = pt->verts[face->verts[0]].index1;
-  int v12 = pt->verts[face->verts[1]].index1;
-  int v13 = pt->verts[face->verts[2]].index1;
-  int v21 = pt->verts[face->verts[0]].index2;
-  int v22 = pt->verts[face->verts[1]].index2;
-  int v23 = pt->verts[face->verts[2]].index2;
+  int v11i = pt->verts[face->verts[0]].index1;
+  int v12i = pt->verts[face->verts[1]].index1;
+  int v13i = pt->verts[face->verts[2]].index1;
+  int v21i = pt->verts[face->verts[0]].index2;
+  int v22i = pt->verts[face->verts[1]].index2;
+  int v23i = pt->verts[face->verts[2]].index2;
+  mjtNum* v11 = pt->verts[face->verts[0]].vert1;
+  mjtNum* v12 = pt->verts[face->verts[1]].vert1;
+  mjtNum* v13 = pt->verts[face->verts[2]].vert1;
+  mjtNum* v21 = pt->verts[face->verts[0]].vert2;
+  mjtNum* v22 = pt->verts[face->verts[1]].vert2;
+  mjtNum* v23 = pt->verts[face->verts[2]].vert2;
 
   // get dimensions of features of geoms 1 and 2
-  int nface1 = simplexDim(&v11, &v12, &v13);
-  int nface2 = simplexDim(&v21, &v22, &v23);
+  int nface1 = simplexDim(&v11i, &v12i, &v13i, &v11, &v12, &v13);
+  int nface2 = simplexDim(&v21i, &v22i, &v23i, &v21, &v22, &v23);
   int nnorms1 = 0, nnorms2 = 0;
   mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT];  // normals of possible face collisions
   int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT];         // indices of faces
 
   // get all possible face normals for each geom
   if (obj1->geom_type == mjGEOM_BOX) {
-    nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11, v12, v13);
+    nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
   } else if (obj1->geom_type == mjGEOM_MESH) {
-    nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11, v12, v13);
+    nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
   }
   if (obj2->geom_type == mjGEOM_BOX) {
-    nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21, v22, v23);
+    nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
   } else if (obj2->geom_type == mjGEOM_MESH) {
-    nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21, v22, v23);
+    nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
   }
 
-  // determine if any two normals match
-  int res[2];
-  if (!alignedNormals(res, n1, nnorms1, n2, nnorms2)) {
-    return;
+  // determine if any two face normals match
+  int res[2], edgecon1 = 0, edgecon2 = 0;
+  if (!alignedFaces(res, n1, nnorms1, n2, nnorms2)) {
+    // check if edge-face collision
+    if (nface1 < 3 && nface1 <= nface2) {
+      nnorms1 = 0;
+      if (obj1->geom_type == mjGEOM_BOX) {
+        nnorms1 = boxEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i);
+      } else if (obj1->geom_type == mjGEOM_MESH) {
+        nnorms1 = meshEdgeNormals(n1, endverts, nface1, obj1, v11, v12, v11i, v12i);
+      }
+      if (!alignedFaceEdge(res, n1, nnorms1, n2, nnorms2)) return;
+      edgecon1 = 1;
+
+    // check if face-edge collision
+    } else if (nface2 < 3) {
+      nnorms2 = 0;
+      if (obj2->geom_type == mjGEOM_BOX) {
+        nnorms2 = boxEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i);
+      } else if (obj2->geom_type == mjGEOM_MESH) {
+        nnorms2 = meshEdgeNormals(n2, endverts, nface2, obj2, v21, v22, v21i, v22i);
+      }
+      if (!alignedFaceEdge(res, n2, nnorms2, n1, nnorms1)) return;
+      edgecon2 = 1;
+    } else {
+      // no multi-contact
+      return;
+    }
   }
   int i = res[0], j = res[1];
 
-  // recover matching faces
-  if (obj1->geom_type == mjGEOM_BOX) {
-    nface1 = boxFace(face1, obj1, idx1[i]);
-  } else if (obj1->geom_type == mjGEOM_MESH) {
-    nface1 = meshFace(face1, obj1, idx1[i]);
-  }
-  if (obj2->geom_type == mjGEOM_BOX) {
-    nface2 = boxFace(face2, obj2, idx2[j]);
-  } else if (obj2->geom_type == mjGEOM_MESH) {
-    nface2 = meshFace(face2, obj2, idx2[j]);
+  // recover geom1 matching edge or face
+  if (edgecon1) {
+    copy3(face1, pt->verts[face->verts[0]].vert1);
+    copy3(face1 + 3, endverts + 3*i);
+    nface1 = 2;
+  } else {
+    if (obj1->geom_type == mjGEOM_BOX) {
+      int ind = (edgecon2 ? idx1[j] : idx1[i]);
+      nface1 = boxFace(face1, obj1, ind);
+    } else if (obj1->geom_type == mjGEOM_MESH) {
+      int ind = (edgecon2 ? idx1[j] : idx1[i]);
+      nface1 = meshFace(face1, obj1, ind);
+    }
   }
 
-  if (nface1 >= 3 && nface2 >= 3) {
-    // TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the
-    // single contact direction's magnitude. This is effective, but polygonClip should compute
-    // this for each contact point.
-    mjtNum diff[3], approx_dir[3];
-    sub3(diff, status->x2, status->x1);
+  // recover geom2 matching edge or face
+  if (edgecon2) {
+    copy3(face2, pt->verts[face->verts[0]].vert2);
+    copy3(face2 + 3, endverts + 3*i);
+    nface2 = 2;
+  } else {
+    if (obj2->geom_type == mjGEOM_BOX) {
+      nface2 = boxFace(face2, obj2, idx2[j]);
+    } else if (obj2->geom_type == mjGEOM_MESH) {
+      nface2 = meshFace(face2, obj2, idx2[j]);
+    }
+  }
+
+  // TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the
+  // single contact direction's magnitude. This is effective, but polygonClip should compute
+  // this for each contact point.
+  mjtNum diff[3], approx_dir[3];
+  sub3(diff, status->x2, status->x1);
+
+  // face1 is an edge; clip face1 against face2
+  if (edgecon1) {
     scl3(approx_dir, n2 + 3*j, norm3(diff));
-
-    // clip the faces and store the results in status
-    polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir);
+    polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir);
+    return;
   }
+
+  // face2 is an edge; clip face2 against face1
+  if (edgecon2) {
+    scl3(approx_dir, n1 + 3*j, -norm3(diff));
+    polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir);
+    return;
+  }
+
+  // face-face collision
+  scl3(approx_dir, n2 + 3*j, norm3(diff));
+  polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir);
 }
 
 
diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h
index cca220cb..d4bdf006 100644
--- a/src/engine/engine_collision_gjk.h
+++ b/src/engine/engine_collision_gjk.h
@@ -25,8 +25,11 @@
 extern "C" {
 #endif
 
-// tolerance for considering two normals to be aligned
-#define mjCOSINE_TOL 0.99999872
+// tolerance for normal alignment of two faces (cosine of 1.6e-3)
+#define mjFACE_TOL 0.99999872
+
+// tolerance for edge-face alignment (sine of 1.6e-3)
+#define mjEDGE_TOL 0.00159999931
 
 // max number of supported vertices in a polygon face of a mesh
 #define mjMAX_POLYVERT 150
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 52c63002..f54e0ca2 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -1167,6 +1167,237 @@ TEST_F(MjGjkTest, MeshMeshPrune) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, BoxEdge) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag nativeccd="enable" multiccd="enable"/>
+    </option>
+
+    <worldbody>
+      <geom type="box" name="box1" size="5 5 .1" pos="0 0 0"/>
+      <body pos="0 0 2">
+        <freejoint/>
+        <geom type="box" name="box2" size="1 1 1"/>
+      </body>
+      <body pos="0 0 4.4" euler="0 90 40">
+        <freejoint/>
+        <geom type="box" name="box3" size="1 1 1"/>
+      </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 geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
+
+  EXPECT_EQ(ncons, 2);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(MjGjkTest, BoxEdge2) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag nativeccd="enable" multiccd="enable"/>
+    </option>
+
+    <worldbody>
+      <geom type="box" name="box1" size="5 5 .1" pos="0 0 0"/>
+      <body pos="0 0 2">
+        <freejoint/>
+        <geom type="box" name="box2" size="1 1 1"/>
+      </body>
+
+      <body pos="0 0 4.4" euler="0 90 40">
+        <freejoint/>
+        <geom type="box" name="box3" size="1 1 1"/>
+      </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);
+
+  mjtNum* xmat = data->geom_xmat + 9;
+  mjtNum* xpos = data->geom_xpos + 3;
+
+  xmat[0] = 0.9999979704374094557906005320546682924032;
+  xmat[1] = -0.0017789363449516469497385662279498319549;
+  xmat[2] = -0.0009457835609818190025430140188689165370;
+  xmat[3] = 0.0017817418144636251123996695255868871755;
+  xmat[4] = 0.9999939910254954655854930933855939656496;
+  xmat[5] = 0.0029737701675293876438233020564894104609;
+  xmat[6] = 0.0009404877299599626429629783963548561587;
+  xmat[7] = -0.0029754492741947335607277658198199787876;
+  xmat[8] = 0.9999951310803708581786963804916013032198;
+
+  xpos[0] = 0.0005578602979296120537716641152314878127;
+  xpos[1] = 0.0098645950089783600300830102014515432529;
+  xpos[2] = 1.1037596929447945903746131079969927668571;
+
+  xmat = data->geom_xmat + 18;
+  xpos = data->geom_xpos + 6;
+
+  xmat[0] = 0.0006737475542006746490053537002040684456;
+  xmat[1] = -0.0095603689585630827196816028390458086506;
+  xmat[2] = 0.9999540716500983084102927023195661604404;
+  xmat[3] = -0.1095658134726250898527410981841967441142;
+  xmat[4] = 0.9939334085756179604231874691322445869446;
+  xmat[5] = 0.0095766296438734854756802405972848646343;
+  xmat[6] = -0.9939793149670246297233688892447389662266;
+  xmat[7] = -0.1095672335264066821203243762283818796277;
+  xmat[8] = -0.0003778293989772788311065632171903416747;
+
+  xpos[0] = -0.0218119359455731035013492657981259981170;
+  xpos[1] = 0.9828851949225971829093850828940048813820;
+  xpos[2] = 3.0930077345364814789263618877157568931580;
+
+  int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
+
+  EXPECT_EQ(ncons, 2);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(MjGjkTest, BoxEdgeEdge) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag nativeccd="enable" multiccd="enable"/>
+    </option>
+
+    <worldbody>
+      <geom type="box" name="box1" size="5 5 .1" pos="0 0 -.1"/>
+      <body pos="-2 0 2.99" euler="0 10 0">
+        <freejoint/>
+        <geom type="box" name="box2" size=".15 1 3"/>
+      </body>
+      <body pos="2 0 2.99" euler="0 -10 0">
+        <freejoint/>
+        <geom type="box" name="box3" size=".15 1 3"/>
+      </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);
+
+  mjtNum* xmat = data->geom_xmat + 9;
+  mjtNum* xpos = data->geom_xpos + 3;
+
+  xmat[0] = 0.9182779243587342321575306414160877466202;
+  xmat[1] = -0.0000000000000000000364268564068890756444;
+  xmat[2] = 0.3959364262547898638544552341045346111059;
+  xmat[3] = -0.0000000000000000000591321577502441383525;
+  xmat[4] = 1.0000000000000000000000000000000000000000;
+  xmat[5] = 0.0000000000000000002291443915550544102718;
+  xmat[6] = -0.3959364262547898638544552341045346111059;
+  xmat[7] = -0.0000000000000000002338308114719865891040;
+  xmat[8] = 0.9182779243587342321575306414160877466202;
+
+  xpos[0] = -1.3241298058948087756903078116010874509811;
+  xpos[1] = 0.0000000000000000007148993364299687318184;
+  xpos[2] = 2.8141526153588731773425024584867060184479;
+
+  xmat = data->geom_xmat + 18;
+  xpos = data->geom_xpos + 6;
+
+  xmat[0] = 0.9182779243587342321575306414160877466202;
+  xmat[1] = 0.0000000000000000000728398144756416399722;
+  xmat[2] = -0.3959364262547898638544552341045346111059;
+  xmat[3] = -0.0000000000000000001674060251593158490713;
+  xmat[4] = 1.0000000000000000000000000000000000000000;
+  xmat[5] = -0.0000000000000000002042889652712837518138;
+  xmat[6] = 0.3959364262547898638544552341045346111059;
+  xmat[7] = 0.0000000000000000002538761903338069406631;
+  xmat[8] = 0.9182779243587342321575306414160877466202;
+
+  xpos[0] = 1.3241298058948089977349127366323955357075;
+  xpos[1] = -0.0000000000000000008679606505055748997840;
+  xpos[2] = 2.8141526153588731773425024584867060184479;
+
+  int geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 4);
+
+  EXPECT_EQ(ncons, 2);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(MjGjkTest, MeshEdge) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <option>
+      <flag nativeccd="enable" multiccd="enable"/>
+    </option>
+    <asset>
+      <mesh name="smallbox"
+        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"/>
+      <mesh name="floor"
+        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"
+        scale="5 5 1"/>
+    </asset>
+    <worldbody>
+      <geom type="mesh" name="box1" mesh="floor" pos="0 0 0"/>
+      <body pos="0 0 2">
+        <freejoint/>
+        <geom type="mesh" mesh="smallbox" name="box2" size="1 1 1"/>
+      </body>
+      <body pos="0 0 4.4" euler="0 90 40">
+        <freejoint/>
+        <geom type="mesh" mesh="smallbox" name="box3" size="1 1 1"/>
+      </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 geom1 = mj_name2id(model, mjOBJ_GEOM, "box2");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "box3");
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 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());
diff --git a/test/engine/testdata/collision_convex/dice.xml b/test/engine/testdata/collision_convex/dice.xml
new file mode 100644
index 00000000..2a3b6880
--- /dev/null
+++ b/test/engine/testdata/collision_convex/dice.xml
@@ -0,0 +1,46 @@
+<mujoco>
+  <option>
+    <flag multiccd="enable"/>
+  </option>
+
+  <visual>
+    <scale forcewidth="0.01" contactwidth="0.05" contactheight="0.05"/>
+    <map force="0.1"/>
+  </visual>
+
+  <asset>
+    <mesh name="die" file="die.obj" scale=".02 .02 .02"/>
+  </asset>
+
+  <default>
+    <geom condim="1" type="mesh" solref="2e-5 1"/>
+    <default class="wall">
+      <geom type="box" rgba="0.1 0.2 1 .2"/>
+    </default>
+    <default class="floor">
+      <geom type="box" rgba="0.1 0.2 1 1"/>
+    </default>
+  </default>
+
+  <worldbody>
+    <light pos="0 0 1" dir="0 0 -1" castshadow="false"/>
+     <body name="container" pos="0 0 -0.11">
+      <geom class="floor" size="0.27 0.27 0.01"/>>
+      <geom class="wall"  size="0.26 0.01 0.10" pos="0.01 0.26 0.11"/>
+      <geom class="wall"  size="0.26 0.01 0.10" pos="-0.01 -0.26 0.11"/>
+      <geom class="wall"  size="0.01 0.26 0.10" pos="0.26 -0.01 0.11"/>
+      <geom class="wall"  size="0.01 0.26 0.10" pos="-0.26 0.01 0.11"/>
+    </body>
+
+    <replicate count="5" offset=".1 0 0">
+      <replicate count="5" offset="0 .1 0">
+        <replicate count="4" offset="0 0 .1">
+          <body pos="-.2 -.2 0.15">
+            <freejoint/>
+            <geom size=".02 .02 .02" type="mesh" mesh="die"/>
+          </body>
+        </replicate>
+      </replicate>
+    </replicate>
+  </worldbody>
+</mujoco>
diff --git a/test/engine/testdata/collision_convex/die.obj b/test/engine/testdata/collision_convex/die.obj
new file mode 100644
index 00000000..91964032
--- /dev/null
+++ b/test/engine/testdata/collision_convex/die.obj
@@ -0,0 +1,296 @@
+v 0.740000 0.740000 -1.000000
+v 0.740000 1.000000 -0.740000
+v 1.000000 0.740000 -0.740000
+v 0.740000 0.807269 -0.991147
+v 0.740000 0.870021 -0.965154
+v 0.740000 0.923848 -0.923848
+v 0.807269 0.740000 -0.991147
+v 0.809387 0.809958 -0.980606
+v 0.808254 0.870420 -0.954318
+v 0.806858 0.917583 -0.917748
+v 0.870021 0.740000 -0.965154
+v 0.870490 0.808646 -0.954150
+v 0.863847 0.863908 -0.932116
+v 0.858399 0.903670 -0.903688
+v 0.890111 0.890111 -0.890111
+v 0.807269 0.991147 -0.740000
+v 0.870021 0.965154 -0.740000
+v 0.923848 0.923848 -0.740000
+v 0.740000 0.991147 -0.807269
+v 0.809958 0.980606 -0.809387
+v 0.870420 0.954318 -0.808254
+v 0.917583 0.917748 -0.806858
+v 0.740000 0.965154 -0.870021
+v 0.808646 0.954150 -0.870490
+v 0.863908 0.932116 -0.863847
+v 0.903670 0.903688 -0.858399
+v 0.991147 0.740000 -0.807269
+v 0.965154 0.740000 -0.870021
+v 0.923848 0.740000 -0.923848
+v 0.991147 0.807269 -0.740000
+v 0.980606 0.809387 -0.809958
+v 0.954318 0.808254 -0.870420
+v 0.917748 0.806858 -0.917583
+v 0.965154 0.870021 -0.740000
+v 0.954150 0.870490 -0.808646
+v 0.932116 0.863847 -0.863908
+v 0.903688 0.858399 -0.903670
+v 0.740000 -1.000000 -0.740000
+v 0.740000 -0.740000 -1.000000
+v 1.000000 -0.740000 -0.740000
+v 0.740000 -0.991147 -0.807269
+v 0.740000 -0.965154 -0.870021
+v 0.740000 -0.923848 -0.923848
+v 0.807269 -0.991147 -0.740000
+v 0.809387 -0.980606 -0.809958
+v 0.808254 -0.954318 -0.870420
+v 0.806858 -0.917748 -0.917583
+v 0.870021 -0.965154 -0.740000
+v 0.870490 -0.954150 -0.808646
+v 0.863847 -0.932116 -0.863908
+v 0.858399 -0.903688 -0.903670
+v 0.890111 -0.890111 -0.890111
+v 0.807269 -0.740000 -0.991147
+v 0.870021 -0.740000 -0.965154
+v 0.923848 -0.740000 -0.923848
+v 0.740000 -0.807269 -0.991147
+v 0.809958 -0.809387 -0.980606
+v 0.870420 -0.808254 -0.954318
+v 0.917583 -0.806858 -0.917748
+v 0.740000 -0.870021 -0.965154
+v 0.808646 -0.870490 -0.954150
+v 0.863908 -0.863847 -0.932116
+v 0.903670 -0.858399 -0.903688
+v 0.991147 -0.807269 -0.740000
+v 0.965154 -0.870021 -0.740000
+v 0.923848 -0.923848 -0.740000
+v 0.991147 -0.740000 -0.807269
+v 0.980606 -0.809958 -0.809387
+v 0.954318 -0.870420 -0.808254
+v 0.917748 -0.917583 -0.806858
+v 0.965154 -0.740000 -0.870021
+v 0.954150 -0.808646 -0.870490
+v 0.932116 -0.863908 -0.863847
+v 0.903688 -0.903670 -0.858399
+v 1.000000 0.740000 0.740000
+v 0.740000 1.000000 0.740000
+v 0.740000 0.740000 1.000000
+v 0.991147 0.807269 0.740000
+v 0.965154 0.870021 0.740000
+v 0.923848 0.923848 0.740000
+v 0.991147 0.740000 0.807269
+v 0.980606 0.809958 0.809387
+v 0.954318 0.870420 0.808254
+v 0.917748 0.917583 0.806858
+v 0.965154 0.740000 0.870021
+v 0.954150 0.808646 0.870490
+v 0.932116 0.863908 0.863847
+v 0.903688 0.903670 0.858399
+v 0.890111 0.890111 0.890111
+v 0.740000 0.991147 0.807269
+v 0.740000 0.965154 0.870021
+v 0.740000 0.923848 0.923848
+v 0.807269 0.991147 0.740000
+v 0.809387 0.980606 0.809958
+v 0.808254 0.954318 0.870420
+v 0.806858 0.917748 0.917583
+v 0.870021 0.965154 0.740000
+v 0.870490 0.954150 0.808646
+v 0.863847 0.932116 0.863908
+v 0.858399 0.903688 0.903670
+v 0.807269 0.740000 0.991147
+v 0.870021 0.740000 0.965154
+v 0.923848 0.740000 0.923848
+v 0.740000 0.807269 0.991147
+v 0.809958 0.809387 0.980606
+v 0.870420 0.808254 0.954318
+v 0.917583 0.806858 0.917748
+v 0.740000 0.870021 0.965154
+v 0.808646 0.870490 0.954150
+v 0.863908 0.863847 0.932116
+v 0.903670 0.858399 0.903688
+v 1.000000 -0.740000 0.740000
+v 0.740000 -0.740000 1.000000
+v 0.740000 -1.000000 0.740000
+v 0.991147 -0.740000 0.807269
+v 0.965154 -0.740000 0.870021
+v 0.923848 -0.740000 0.923848
+v 0.991147 -0.807269 0.740000
+v 0.980606 -0.809387 0.809958
+v 0.954318 -0.808254 0.870420
+v 0.917748 -0.806858 0.917583
+v 0.965154 -0.870021 0.740000
+v 0.954150 -0.870490 0.808646
+v 0.932116 -0.863847 0.863908
+v 0.903688 -0.858399 0.903670
+v 0.890111 -0.890111 0.890111
+v 0.740000 -0.807269 0.991147
+v 0.740000 -0.870021 0.965154
+v 0.740000 -0.923848 0.923848
+v 0.807269 -0.740000 0.991147
+v 0.809387 -0.809958 0.980606
+v 0.808254 -0.870420 0.954318
+v 0.806858 -0.917583 0.917748
+v 0.870021 -0.740000 0.965154
+v 0.870490 -0.808646 0.954150
+v 0.863847 -0.863908 0.932116
+v 0.858399 -0.903670 0.903688
+v 0.807269 -0.991147 0.740000
+v 0.870021 -0.965154 0.740000
+v 0.923848 -0.923848 0.740000
+v 0.740000 -0.991147 0.807269
+v 0.809958 -0.980606 0.809387
+v 0.870420 -0.954318 0.808254
+v 0.917583 -0.917748 0.806858
+v 0.740000 -0.965154 0.870021
+v 0.808646 -0.954150 0.870490
+v 0.863908 -0.932116 0.863847
+v 0.903670 -0.903688 0.858399
+v -0.740000 0.740000 -1.000000
+v -1.000000 0.740000 -0.740000
+v -0.740000 1.000000 -0.740000
+v -0.807269 0.740000 -0.991147
+v -0.870021 0.740000 -0.965154
+v -0.923848 0.740000 -0.923848
+v -0.740000 0.807269 -0.991147
+v -0.809958 0.809387 -0.980606
+v -0.870420 0.808254 -0.954318
+v -0.917583 0.806858 -0.917748
+v -0.740000 0.870021 -0.965154
+v -0.808646 0.870490 -0.954150
+v -0.863908 0.863847 -0.932116
+v -0.903670 0.858399 -0.903688
+v -0.890111 0.890111 -0.890111
+v -0.991147 0.807269 -0.740000
+v -0.965154 0.870021 -0.740000
+v -0.923848 0.923848 -0.740000
+v -0.991147 0.740000 -0.807269
+v -0.980606 0.809958 -0.809387
+v -0.954318 0.870420 -0.808254
+v -0.917748 0.917583 -0.806858
+v -0.965154 0.740000 -0.870021
+v -0.954150 0.808646 -0.870490
+v -0.932116 0.863908 -0.863847
+v -0.903688 0.903670 -0.858399
+v -0.740000 0.991147 -0.807269
+v -0.740000 0.965154 -0.870021
+v -0.740000 0.923848 -0.923848
+v -0.807269 0.991147 -0.740000
+v -0.809387 0.980606 -0.809958
+v -0.808254 0.954318 -0.870420
+v -0.806858 0.917748 -0.917583
+v -0.870021 0.965154 -0.740000
+v -0.870490 0.954150 -0.808646
+v -0.863847 0.932116 -0.863908
+v -0.858399 0.903688 -0.903670
+v -1.000000 -0.740000 -0.740000
+v -0.740000 -0.740000 -1.000000
+v -0.740000 -1.000000 -0.740000
+v -0.991147 -0.740000 -0.807269
+v -0.965154 -0.740000 -0.870021
+v -0.923848 -0.740000 -0.923848
+v -0.991147 -0.807269 -0.740000
+v -0.980606 -0.809387 -0.809958
+v -0.954318 -0.808254 -0.870420
+v -0.917748 -0.806858 -0.917583
+v -0.965154 -0.870021 -0.740000
+v -0.954150 -0.870490 -0.808646
+v -0.932116 -0.863847 -0.863908
+v -0.903688 -0.858399 -0.903670
+v -0.890111 -0.890111 -0.890111
+v -0.740000 -0.807269 -0.991147
+v -0.740000 -0.870021 -0.965154
+v -0.740000 -0.923848 -0.923848
+v -0.807269 -0.740000 -0.991147
+v -0.809387 -0.809958 -0.980606
+v -0.808254 -0.870420 -0.954318
+v -0.806858 -0.917583 -0.917748
+v -0.870021 -0.740000 -0.965154
+v -0.870490 -0.808646 -0.954150
+v -0.863847 -0.863908 -0.932116
+v -0.858399 -0.903670 -0.903688
+v -0.807269 -0.991147 -0.740000
+v -0.870021 -0.965154 -0.740000
+v -0.923848 -0.923848 -0.740000
+v -0.740000 -0.991147 -0.807269
+v -0.809958 -0.980606 -0.809387
+v -0.870420 -0.954318 -0.808254
+v -0.917583 -0.917748 -0.806858
+v -0.740000 -0.965154 -0.870021
+v -0.808646 -0.954150 -0.870490
+v -0.863908 -0.932116 -0.863847
+v -0.903670 -0.903688 -0.858399
+v -1.000000 0.740000 0.740000
+v -0.740000 0.740000 1.000000
+v -0.740000 1.000000 0.740000
+v -0.991147 0.740000 0.807269
+v -0.965154 0.740000 0.870021
+v -0.923848 0.740000 0.923848
+v -0.991147 0.807269 0.740000
+v -0.980606 0.809387 0.809958
+v -0.954318 0.808254 0.870420
+v -0.917748 0.806858 0.917583
+v -0.965154 0.870021 0.740000
+v -0.954150 0.870490 0.808646
+v -0.932116 0.863847 0.863908
+v -0.903688 0.858399 0.903670
+v -0.890111 0.890111 0.890111
+v -0.740000 0.807269 0.991147
+v -0.740000 0.870021 0.965154
+v -0.740000 0.923848 0.923848
+v -0.807269 0.740000 0.991147
+v -0.809387 0.809958 0.980606
+v -0.808254 0.870420 0.954318
+v -0.806858 0.917583 0.917748
+v -0.870021 0.740000 0.965154
+v -0.870490 0.808646 0.954150
+v -0.863847 0.863908 0.932116
+v -0.858399 0.903670 0.903688
+v -0.807269 0.991147 0.740000
+v -0.870021 0.965154 0.740000
+v -0.923848 0.923848 0.740000
+v -0.740000 0.991147 0.807269
+v -0.809958 0.980606 0.809387
+v -0.870420 0.954318 0.808254
+v -0.917583 0.917748 0.806858
+v -0.740000 0.965154 0.870021
+v -0.808646 0.954150 0.870490
+v -0.863908 0.932116 0.863847
+v -0.903670 0.903688 0.858399
+v -0.740000 -1.000000 0.740000
+v -0.740000 -0.740000 1.000000
+v -1.000000 -0.740000 0.740000
+v -0.740000 -0.991147 0.807269
+v -0.740000 -0.965154 0.870021
+v -0.740000 -0.923848 0.923848
+v -0.807269 -0.991147 0.740000
+v -0.809387 -0.980606 0.809958
+v -0.808254 -0.954318 0.870420
+v -0.806858 -0.917748 0.917583
+v -0.870021 -0.965154 0.740000
+v -0.870490 -0.954150 0.808646
+v -0.863847 -0.932116 0.863908
+v -0.858399 -0.903688 0.903670
+v -0.890111 -0.890111 0.890111
+v -0.807269 -0.740000 0.991147
+v -0.870021 -0.740000 0.965154
+v -0.923848 -0.740000 0.923848
+v -0.740000 -0.807269 0.991147
+v -0.809958 -0.809387 0.980606
+v -0.870420 -0.808254 0.954318
+v -0.917583 -0.806858 0.917748
+v -0.740000 -0.870021 0.965154
+v -0.808646 -0.870490 0.954150
+v -0.863908 -0.863847 0.932116
+v -0.903670 -0.858399 0.903688
+v -0.991147 -0.807269 0.740000
+v -0.965154 -0.870021 0.740000
+v -0.923848 -0.923848 0.740000
+v -0.991147 -0.740000 0.807269
+v -0.980606 -0.809958 0.809387
+v -0.954318 -0.870420 0.808254
+v -0.917748 -0.917583 0.806858
+v -0.965154 -0.740000 0.870021
+v -0.954150 -0.808646 0.870490
+v -0.932116 -0.863908 0.863847
+v -0.903688 -0.903670 0.858399
diff --git a/test/engine/testdata/collision_convex/edge_edge.xml b/test/engine/testdata/collision_convex/edge_edge.xml
new file mode 100644
index 00000000..b4815850
--- /dev/null
+++ b/test/engine/testdata/collision_convex/edge_edge.xml
@@ -0,0 +1,24 @@
+<mujoco>
+  <option>
+    <flag multiccd="enable"/>
+  </option>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" width="512" height="512"
+             rgb1="1 1 1" rgb2="1 1 1"/>
+    <mesh name="domino" 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"
+          scale=".15 1 3"/>
+  </asset>
+
+  <worldbody>
+    <geom type="box" size="5 5 .1" pos="0 0 -.1"/>
+    <body pos="-2 0 2.99" euler="0 10 0">
+      <freejoint/>
+      <geom type="mesh" name="domino1" mesh="domino" rgba=".1 .1 .1 1"/>
+    </body>
+    <body pos="2 0 2.99" euler="0 -10 0">
+      <freejoint/>
+      <geom type="mesh" name="domino2" mesh="domino" rgba=".1 .1 .1 1"/>
+    </body>
+  </worldbody>
+</mujoco>
diff --git a/test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml b/test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml
new file mode 100644
index 00000000..f626eb1f
--- /dev/null
+++ b/test/engine/testdata/sensor/fromto_capsule_ellipsoid.xml
@@ -0,0 +1,32 @@
+<mujoco model="Capsule-Ellipsoid Distance">
+  <visual>
+    <headlight ambient=".7 .7 .7"/>
+    <rgba rangefinder="1 .2 .1 1"/>
+  </visual>
+  <option>
+    <flag gravity="disable"/>
+  </option>
+  <asset>
+    <texture type="skybox" builtin="gradient" width="512" height="512" rgb1="1 1 1" rgb2="1 1 1"/>
+  </asset>
+
+  <default>
+    <geom rgba=".1 .1 .1 1"/>
+  </default>
+
+  <worldbody>
+    <body pos="-.5 0 0">
+      <freejoint/>
+      <geom name="capsule" type="capsule" size=".15 .30"/>
+    </body>
+
+    <body pos=".5 0 0">
+      <freejoint/>
+      <geom name="ellipsoid" type="ellipsoid" size=".10 .10 .15"/>
+    </body>
+  </worldbody>
+
+  <sensor>
+    <fromto geom1="capsule" geom2="ellipsoid" cutoff="15"/>
+  </sensor>
+</mujoco>