Initial (unoptimized) implementation of EPA.

PiperOrigin-RevId: 659938320
Change-Id: I72c6c6bd7504bce36e4788d90b06b2cb3849b4db

src/engine/engine_collision_gjk.c

@@ -15,12 +15,14 @@
 #include "engine/engine_collision_gjk.h"
 
 #include <stddef.h>
+#include <stdlib.h>
 
 #include <mujoco/mjtnum.h>
 #include <mujoco/mjmodel.h>
-#include "engine/engine_util_blas.h"
-#include "engine/engine_util_spatial.h"
 #include "engine/engine_collision_convex.h"
+#include "engine/engine_util_blas.h"
+#include "engine/engine_util_errmem.h"
+#include "engine/engine_util_spatial.h"
 
 // Computes the shortest distance between the origin and an n-simplex (n <= 3) and returns the
 // barycentric coordinates of the closest point in the simplex. This is the so called distance
@@ -38,16 +40,60 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]);
 static void S1D(mjtNum lambda[2], const mjtNum simplex[6]);
 
 // helper function to compute the support point in the Minkowski difference
-static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
-                    const mjtNum x_k[3]);
+static void support(mjtNum s[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum d[3]);
+
+// support function tweaked for GJK by taking kth iteration point as input and setting both
+// support points to recover witness points
+static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+                        const mjtNum x_k[3]);
 
 // linear algebra utility functions
 static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]);
 static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n);
 
-// returns the distance between the two objects. The witness points are
-// recoverable from x_0 in obj1 and obj2.
-mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
+typedef struct {
+  int ignored;
+  int verts[3];  // indices of the three vertices of the face in the polytope
+  mjtNum v[3];
+  mjtNum dist;
+  mjtNum n[3];
+} Face;
+
+typedef struct {
+  mjtNum* verts;
+  int nverts;
+  int vcap;
+  Face* faces;
+  int nfaces;
+  int fcap;
+} Polytope;
+
+// generates a polytope from a 1-simplex, 2-simplex, or 3-simplex respectively
+// returns true if the polytope can be generated, false otherwise
+static int polytope2(Polytope* pt, const mjtNum simplex[6], mjCCDObj* obj1, mjCCDObj* obj2);
+static int polytope3(Polytope* pt, const mjtNum simplex[9]);
+static int polytope4(Polytope* pt, const mjtNum simplex[12]);
+
+// initializes the polytope (faces and vertices must be freed by caller)
+static void initPolytope(Polytope* pt);
+
+// copies a vertex into the polytope and return its index
+static int newVertex(Polytope* pt, const mjtNum v1[3]);
+
+// attaches a face to the polytope with the given vertex indices in the polytope
+static void attachFace(Polytope* pt, int v1, int v2, int v3);
+
+// returns the penetration depth (negative distance) of the convex objects
+static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2);
+
+// internal data structure for the returning simplex from GJK
+typedef struct {
+  mjtNum verts[12];
+  int nverts;
+} Simplex;
+
+// internal GJK with returned data for EPA
+static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, Simplex* ret) {
   mjtNum simplex[12];  // our current simplex with max 4 vertices due to only 3 dimensions
   int n = 0;           // number of vertices in the simplex
   mjtNum x_k[3];       // the kth approximation point with initial value x_0
@@ -65,7 +111,7 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
     mjtNum lambda[4];     // barycentric coordinates for x_k
 
     // compute the kth support point
-    support(s1, s2, obj1, obj2, x_k);
+    gjk_support(s1, s2, obj1, obj2, x_k);
     mju_sub3(s_k, s1, s2);
 
     // the stopping criteria relies on the Frank-Wolfe duality gap given by
@@ -73,7 +119,7 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
     mjtNum diff[3];
     mju_sub3(diff, x_k, s_k);
     if (2*mju_dot3(x_k, diff) < config->tolerance) {
-      return mju_norm3(x_k);
+      break;
     }
 
     // TODO(kylebayes): signedVolume has been written to assume the first vertex is the latest
@@ -113,14 +159,56 @@ mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
       mju_copy3(simplex + 3*n++, simplex + 3*i);
     }
   }
+  if (ret) {
+    ret->nverts = n;
+    for (int i = 0; i < n; i++) {
+      mju_copy3(ret->verts + 3*i, simplex + 3*i);
+    }
+  }
   return mju_norm3(x_k);
 }
 
 
 
+// returns the distance between the two objects. The witness points are
+// recoverable from the x_0 field in obj1 and obj2.
+mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
+  return _gjk(config, obj1, obj2, NULL);
+}
+
+
+
+// Same as mj_gjk, but returns the penetration depth (negative distance) if the objects intersect.
+mjtNum mj_gjkPenetration(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) {
+  Simplex simplex;
+  mjtNum dist = _gjk(config, obj1, obj2, &simplex);
+
+  if (dist <= config->tolerance && simplex.nverts > 1) {
+    Polytope pt;
+    int ret;
+    if (simplex.nverts == 2) {
+      ret = polytope2(&pt, simplex.verts, obj1, obj2);
+    } else if (simplex.nverts == 3) {
+      ret = polytope3(&pt, simplex.verts);
+    } else {
+      ret = polytope4(&pt, simplex.verts);
+    }
+
+    // simplex not on boundary (objects are penetrating)
+    if (ret) {
+      dist = -epa(config, &pt, obj1, obj2);
+    }
+    mju_free(pt.faces);
+    mju_free(pt.verts);
+  }
+  return dist;
+}
+
+
+
 // computes the support points in obj1 and obj2 for the kth approximation point
-static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
-                    const mjtNum x_k[3]) {
+static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+                        const mjtNum x_k[3]) {
   mjtNum dir[3], dir_neg[3];
   mju_copy3(dir_neg, x_k);
   mju_normalize3(dir_neg);  // mjc_support assumes a normalized direction
@@ -133,6 +221,22 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
 
 
 
+// helper function to compute the support point in the Minkowski difference
+static void support(mjtNum s[3], mjCCDObj* obj1, mjCCDObj* obj2,
+                    const mjtNum d[3]) {
+  mjtNum dir[3], dir_neg[3], s1[3], s2[3];
+  mju_copy3(dir, d);
+  mju_normalize3(dir);  // mjc_support assumes a normalized direction
+  mju_scl3(dir_neg, dir, -1);
+
+  // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
+  mjc_support(s1, obj1, dir);
+  mjc_support(s2, obj2, dir_neg);
+  mju_sub3(s, s1, s2);
+}
+
+
+
 // linear combination of n 3D vectors:
 //   res = coef[0]*v[0] + ... + coef[n-1]*v[3*(n-1)]
 static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n) {
@@ -473,3 +577,354 @@ static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) {
     lambda[1] = 0;
   }
 }
+
+
+
+// helper function to test if the origin is in the same side of the plane formed by P0P1P2 as P3.
+static int sameSide(const mjtNum p0[3], const mjtNum p1[3],
+                    const mjtNum p2[3], const mjtNum p3[3]) {
+    mjtNum diff1[3], diff2[3], diff3[3], diff4[3], n[3];
+    mju_sub3(diff1, p1, p0);
+    mju_sub3(diff2, p2, p0);
+    mju_cross(n, diff1, diff2);
+
+    mju_sub3(diff3, p3, p0);
+    mjtNum dot1 = mju_dot3(n, diff3);
+
+    mju_scl3(diff4, p0, -1);
+    mjtNum dot2 = mju_dot3(n, diff4);
+    if (dot1 > 0 && dot2 > 0) return 1;
+    if (dot1 < 0 && dot2 < 0) return 1;
+    return 0;
+}
+
+
+
+// determines if the origin is contained in the tetrahedron.
+static int testTetra(const mjtNum p0[3], const mjtNum p1[3],
+                     const mjtNum p2[3], const mjtNum p3[3]) {
+  return sameSide(p0, p1, p2, p3)
+      && sameSide(p1, p2, p3, p0)
+      && sameSide(p2, p3, p0, p1)
+      && sameSide(p3, p0, p1, p2);
+}
+
+
+
+// sets rotation matrix for 120 degrees along axis
+static void rotmat(mjtNum R[9], const mjtNum axis[3]) {
+  mjtNum n = mju_norm3(axis);
+  mjtNum u1 = axis[0] / n, u2 = axis[1] / n, u3 = axis[2] / n;
+  const mjtNum sin = 0.86602540378;  // sin(120 deg) = sqrt(3)/2 ~ 0.86602540378
+  const mjtNum cos = -0.5;           // cos(120 deg) = -1/2
+  R[0] = cos + u1*u1*(1 - cos);
+  R[1] = u1*u2*(1 - cos) - u3*sin;
+  R[2] = u1*u3*(1 - cos) + u2*sin;
+  R[3] = u2*u1*(1 - cos) + u3*sin;
+  R[4] = cos + u2*u2*(1 - cos);
+  R[5] = u2*u3*(1 - cos) - u1*sin;
+  R[6] = u1*u3*(1 - cos) - u2*sin;
+  R[7] = u2*u3*(1 - cos) + u1*sin;
+  R[8] = cos + u3*u3*(1 - cos);
+}
+
+
+
+// creates a polytope from a 1-simplex (2 points i.e. line segment)
+static int polytope2(Polytope* pt, const mjtNum simplex[6],
+                              mjCCDObj* obj1, mjCCDObj* obj2) {
+  initPolytope(pt);
+  const mjtNum* s1 = simplex;
+  const mjtNum* s2 = simplex + 3;
+  mjtNum diff[3];
+  mju_sub3(diff, s2, s1);
+
+  // find component with largest magnitude (so cross product is largest)
+  mjtNum value = mjMAXVAL;
+  int index = 0;
+  for (int i = 0; i < 3; i++) {
+    if (mju_abs(diff[i]) < value) {
+      value = mju_abs(diff[i]);
+      index = i;
+    }
+  }
+
+  // cross product with best coordinate axis
+  mjtNum e[3] = {0, 0, 0};
+  e[index] = 1;
+  mjtNum d1[3], d2[3], d3[3];
+  mju_cross(d1, e, diff);
+
+  // rotate around the line segment to get three more points spaced 120 degrees apart
+  mjtNum R[9];
+  rotmat(R, diff);
+
+  mju_mulMatVec(d2, R, d1, 3, 3);
+  mju_mulMatVec(d3, R, d2, 3, 3);
+
+
+  mjtNum v1[3], v2[3], v3[3];
+  support(v1, obj1, obj2, d1);
+  support(v2, obj1, obj2, d2);
+  support(v3, obj1, obj2, d3);
+
+  // points of a hexahedron (we test to see what half the origin is contained in)
+  int s1i = newVertex(pt, s1);
+  int v1i = newVertex(pt, v1);
+  int v2i = newVertex(pt, v2);
+  int v3i = newVertex(pt, v3);
+  int s2i = newVertex(pt, s2);
+
+  if (testTetra(s1, v1, v2, v3)) {
+    attachFace(pt, s1i, v2i, v1i);
+    attachFace(pt, s1i, v3i, v1i);
+    attachFace(pt, s1i, v3i, v2i);
+    attachFace(pt, v1i, v2i, v3i);
+    return 1;
+  }
+
+  if (testTetra(s2, v1, v2, v3)) {
+    attachFace(pt, s2i, v1i, v2i);
+    attachFace(pt, s2i, v1i, v3i);
+    attachFace(pt, s2i, v2i, v3i);
+    attachFace(pt, v1i, v2i, v3i);
+    return 1;
+  }
+  return 0;
+}
+
+
+
+// creates a polytope from a 2-simplex (3 points i.e. triangle)
+static int polytope3(Polytope* pt, const mjtNum simplex[9]) {
+  initPolytope(pt);
+
+  const mjtNum* s1 = simplex;
+  const mjtNum* s2 = simplex + 3;
+  const mjtNum* s3 = simplex + 6;
+
+  // form hexahedron from triangle and two face normals
+
+  mjtNum diff1[3], diff2[3], n[3], neg_n[3];
+  mju_sub3(diff1, s2, s1);
+  mju_sub3(diff2, s3, s1);
+  mju_cross(n, diff1, diff2);
+  mju_scl3(neg_n, n, -1);
+
+  int ni = newVertex(pt, n);
+  int s1i = newVertex(pt, s1);
+  int s2i = newVertex(pt, s2);
+  int s3i = newVertex(pt, s3);
+  int nni = newVertex(pt, neg_n);
+
+  attachFace(pt, s1i, s2i, ni);
+  attachFace(pt, s3i, s1i, ni);
+  attachFace(pt, s2i, s3i, ni);
+
+  attachFace(pt, s1i, s2i, nni);
+  attachFace(pt, s3i, s1i, nni);
+  attachFace(pt, s2i, s3i, nni);
+
+  // TODO(kylebayes): check what side of the hexahedron the origin is on
+  return 1;
+}
+
+
+
+// creates a polytope from a 3-simplex (4 points i.e. tetrahedron)
+static int polytope4(Polytope* pt, const mjtNum simplex[12]) {
+  initPolytope(pt);
+
+  int v1 = newVertex(pt, simplex);
+  int v2 = newVertex(pt, simplex + 3);
+  int v3 = newVertex(pt, simplex + 6);
+  int v4 = newVertex(pt, simplex + 9);
+
+  attachFace(pt, v1, v2, v3);
+  attachFace(pt, v1, v2, v4);
+  attachFace(pt, v1, v4, v3);
+  attachFace(pt, v4, v2, v3);
+
+  // TODO(kylebayes): check if contains origin
+  return 1;
+}
+
+#define mjMINCAP 100  // starting capacity for dynamic buffers
+
+// an edge in the horizon
+typedef struct {
+  int v1;
+  int v2;
+  int ignore;  // deleted
+} Edge;
+
+// the horizon of the polytope
+typedef struct {
+  Edge* edges;   // edges in horizon
+  int n;
+  int capacity;
+} Horizon;
+
+
+
+// initializes the polytope (faces and vertices must be freed by caller)
+static void initPolytope(Polytope* pt) {
+  // vertices
+  pt->nverts = 0;
+  pt->vcap = mjMINCAP;
+  pt->verts = (mjtNum*) mju_malloc(pt->vcap * 3 * sizeof(mjtNum));
+
+  // faces
+  pt->nfaces = 0;
+  pt->fcap = mjMINCAP;
+  pt->faces = (Face*) mju_malloc(pt->fcap * sizeof(Face));
+}
+
+
+
+// copies a vertex into the polytope and return its index
+static int newVertex(Polytope* pt, const mjtNum v[3]) {
+  int capacity = pt->vcap;
+  int n = pt->nverts++;
+  if (n == capacity) {
+    capacity *= 2;
+    pt->verts = (mjtNum*) realloc(pt->verts, capacity * 3 * sizeof(mjtNum));
+    pt->vcap = capacity;
+  }
+
+
+  mju_copy3(pt->verts + 3*n, v);
+  return n;
+}
+
+
+
+// attaches a face to the polytope with the given vertex indices in the polytope
+static void attachFace(Polytope* pt, int v1, int v2, int v3) {
+  int capacity = pt->fcap;
+  if (pt->nfaces == capacity) {
+    capacity *= 2;
+    pt->faces = (Face*) realloc(pt->faces, capacity * sizeof(Face));
+    pt->fcap = capacity;
+  }
+
+  Face* face = &pt->faces[pt->nfaces];
+  face->ignored = 0;
+  face->verts[0] = v1;
+  face->verts[1] = v2;
+  face->verts[2] = v3;
+
+  // compute normal n
+  mjtNum* pv1 = pt->verts + (v1 * 3);
+  mjtNum* pv2 = pt->verts + (v2 * 3);
+  mjtNum* pv3 = pt->verts + (v3 * 3);
+  mjtNum diff1[3], diff2[3];
+  mju_sub3(diff1, pv2, pv1);
+  mju_sub3(diff2, pv3, pv1);
+  mju_cross(face->n, diff1, diff2);
+  mju_normalize3(face->n);
+
+  // compute witness point v
+  mju_scl3(face->v, face->n, mju_dot3(face->n, pv1));
+  face->dist = mju_norm3(face->v);
+
+  // orientation check
+  if (mju_dot3(face->n, pv1) < 0) mju_scl3(face->n, face->n, -1);
+  pt->nfaces++;
+}
+
+
+
+// initializes the horizon (edges must be freed by caller)
+static void initHorizon(Horizon* h) {
+  h->n = 0;
+  h->capacity = mjMINCAP;
+  h->edges = (Edge*) mju_malloc(h->capacity * sizeof(Edge));
+}
+
+
+
+// adds an edge to the horizon, if the edge is already in the horizon, it is
+// deleted (marked ignored)
+static void addEdgeIfUnique(Horizon* h, int v1, int v2) {
+  int capacity = h->capacity;
+  int n = h->n;
+
+  for (int i = 0; i < n; i++) {
+    if (h->edges[i].ignore) continue;
+    int old_v1 = h->edges[i].v1;
+    int old_v2 = h->edges[i].v2;
+    if ((old_v1 == v1 && old_v2 == v2) || (old_v1 == v2 && old_v2 == v1)) {
+      h->edges[i].ignore = 1;
+      return;
+    }
+  }
+  if (n == capacity) {
+    capacity *= 2;
+    h->edges = (Edge*) realloc(h->edges, capacity * sizeof(Edge));
+    h->capacity = capacity;
+  }
+  h->edges[n].v1 = v1;
+  h->edges[n].v2 = v2;
+  h->edges[n].ignore = 0;
+  h->n++;
+}
+
+#undef mjMINCAP
+
+
+// returns the penetration depth (negative distance) of the convex objects
+static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
+  mjtNum dist = mjMAXVAL;
+  Horizon h;
+  initHorizon(&h);
+  int N = config->max_iterations;
+  mjtNum tolerance = config->tolerance;
+
+  for (int j = 0; j < N; j++) {
+    dist = mjMAXVAL;
+    int index = -1;
+
+    // find the closest face to the origin
+    for (int i = 0; i < pt->nfaces; i++) {
+      if (pt->faces[i].ignored) continue;
+      if (pt->faces[i].dist < dist) {
+        dist = pt->faces[i].dist;
+        index = i;
+      }
+    }
+
+    // compute support point w from the closest face's normal
+    mjtNum w[3];
+    support(w, obj1, obj2, pt->faces[index].v);
+    mjtNum next_dist = mju_dot3(pt->faces[index].v, w) / dist;
+    if (next_dist - dist < tolerance) {
+      break;
+    }
+
+    // compute horizon for w
+    for (int i = 0; i < pt->nfaces; i++) {
+      Face* face = &pt->faces[i];
+      if (face->ignored) continue;
+      mjtNum dist2 = face->dist;
+      mjtNum* v = face->v;  // use witness point as normal
+      if (mju_dot3(v, w) >= dist2*dist2) {
+        face->ignored = 1;
+        addEdgeIfUnique(&h, face->verts[0], face->verts[1]);
+        addEdgeIfUnique(&h, face->verts[1], face->verts[2]);
+        addEdgeIfUnique(&h, face->verts[2], face->verts[0]);
+      }
+    }
+
+    // insert w as new vertex and attach faces along the horizon
+    int wi = newVertex(pt, w);
+    for (int i = 0; i < h.n; i++) {
+      if (h.edges[i].ignore) continue;
+      attachFace(pt, wi, h.edges[i].v1, h.edges[i].v2);
+    }
+
+    h.n = 0;  // clear horizon
+  }
+  mju_free(h.edges);
+  return dist;
+}

src/engine/engine_collision_gjk.h

@@ -34,6 +34,9 @@ typedef struct _mjCCDConfig mjCCDConfig;
 // recoverable from x_0 in obj1 and obj2.
 MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2);
 
+// Same as mj_gjk, but returns the penetration depth (negative distance) if the objects intersect.
+MJAPI mjtNum mj_gjkPenetration(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2);
+
 #ifdef __cplusplus
 }
 #endif

test/engine/engine_collision_gjk_test.cc

@@ -41,7 +41,7 @@ static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
   mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
   mjc_center(obj1.x0, &obj1);
   mjc_center(obj2.x0, &obj2);
-  mjtNum dist = mj_gjk(&config, &obj1, &obj2);
+  mjtNum dist = mj_gjkPenetration(&config, &obj1, &obj2);
   if (x1 != nullptr) mju_copy3(x1, obj1.x0);
   if (x2 != nullptr) mju_copy3(x2, obj2.x0);
   return dist;
@@ -49,40 +49,9 @@ static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
 
 using MjGjkTest = MujocoTest;
 
-TEST_F(MjGjkTest, SphereSphereIntersect) {
-  static constexpr char xml[] = R"(
-  <mujoco>
-  <option>
-    <flag gravity="disable"/>
-  </option>
-  <worldbody>
-    <geom name="geom1" type="sphere" pos="-1 0 0" size="1"/>
-    <geom name="geom2" type="sphere" pos="1 0 0" size="1"/>
-  </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, "geom1");
-  int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
-  mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
-
-  EXPECT_EQ(dist, 0);
-  mj_deleteData(data);
-  mj_deleteModel(model);
-}
-
 TEST_F(MjGjkTest, SphereSphere) {
   static constexpr char xml[] = R"(
   <mujoco>
-  <option>
-    <flag gravity="disable"/>
-  </option>
   <worldbody>
     <body pos="-1.5 0 0">
       <freejoint/>
@@ -117,9 +86,6 @@ TEST_F(MjGjkTest, SphereSphere) {
 TEST_F(MjGjkTest, BoxBox) {
   static constexpr char xml[] = R"(
   <mujoco>
-  <option>
-    <flag gravity="disable"/>
-  </option>
   <worldbody>
     <geom name="geom1" type="box"  pos="-1.5 .5 0" size="1 1 1"/>
     <geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
@@ -142,12 +108,34 @@ TEST_F(MjGjkTest, BoxBox) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, BoxBoxIntersect) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <geom name="geom1" type="box"  pos="-1 0 0" size="2.5 2.5 2.5"/>
+    <geom name="geom2" type="box" pos="1.5 0 0" size="1 1 1"/>
+  </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, "geom1");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+  mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
+
+  EXPECT_NEAR(dist, -1, kTolerance);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjGjkTest, EllipsoidEllipsoid) {
   static constexpr char xml[] = R"(
   <mujoco>
-  <option>
-    <flag gravity="disable"/>
-  </option>
   <worldbody>
     <geom name="geom1" type="ellipsoid" pos="1.5 0 -.5" size=".15 .30 .20"/>
     <geom name="geom2" type="ellipsoid" pos="1.5 .5 .5" size=".10 .10 .15"/>
@@ -173,9 +161,6 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) {
 TEST_F(MjGjkTest, CapsuleCapsule) {
   static constexpr char xml[] = R"(
   <mujoco>
-  <option>
-    <flag gravity="disable"/>
-  </option>
   <worldbody>
     <geom name="geom1" type="capsule" pos="-.3 .2 -.4" size=".15 .30"/>
     <geom name="geom2" type="capsule" pos=".3 .2 .4" size=".10 .10"/>