Improvements to GJK implementation:

- Fix various bugs
- Improve readability
- Add support for recovering witness points.

PiperOrigin-RevId: 646515019
Change-Id: I295cf90d885e32013e595ea7aad07ed6c1869239

src/engine/engine_collision_convex.c

@@ -34,13 +34,13 @@
 
 // ccd center function
 void mjccd_center(const void *obj, ccd_vec3_t *center) {
-  mjc_center(center->v, (const mjtCCObj*) obj);
+  mjc_center(center->v, (const mjCCDObj*) obj);
 }
 
 
 
 // center function for convex collision algorithms
-void mjc_center(mjtNum res[3], const mjtCCObj *obj) {
+void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
   int g = obj->geom;
   int f = obj->flex;
   int e = obj->elem;
@@ -66,13 +66,13 @@ void mjc_center(mjtNum res[3], const mjtCCObj *obj) {
 
 // ccd support function
 void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
-  mjc_support(vec->v, (mjtCCObj*) obj, _dir->v);
+  mjc_support(vec->v, (mjCCDObj*) obj, _dir->v);
 }
 
 
 
 // support function for convex collision algorithms
-void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]) {
+void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
   const mjModel* m = obj->model;
   const mjData* d = obj->data;
   int g = obj->geom;
@@ -289,7 +289,7 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) {
 
 
 // find single convex-convex collision, using libccd
-static int mjc_MPRIteration(mjtCCObj* obj1, mjtCCObj* obj2, const ccd_t* ccd,
+static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd,
                             const mjModel* m, const mjData* d,
                             mjContact* con, mjtNum margin) {
   ccd_vec3_t dir, pos;
@@ -360,8 +360,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
 int mjc_Convex(const mjModel* m, const mjData* d,
                mjContact* con, int g1, int g2, mjtNum margin) {
   ccd_t ccd;
-  mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
+  mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
-  mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
+  mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
 
   // init ccd structure
   mjc_initCCD(&ccd, m);
@@ -489,7 +489,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
   mjGETINFO
   mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
   ccd_vec3_t dir, vec;
-  mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
+  mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
 
   // get support point in -normal direction
   ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
@@ -670,7 +670,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
   // ccd-related
   ccd_vec3_t dirccd, vecccd;
   ccd_real_t depth;
-  mjtCCObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
+  mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
   ccd_t ccd;
 
   // point size1 to hfield size instead of geom1 size
@@ -1103,8 +1103,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
 int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
                    int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
   ccd_t ccd;
-  mjtCCObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}};
+  mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}};
-  mjtCCObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
+  mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
 
   // init ccd structure
   mjc_initCCD(&ccd, m);
@@ -1151,7 +1151,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
   // ccd-related
   ccd_vec3_t dirccd, vecccd;
   ccd_real_t depth;
-  mjtCCObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}};
+  mjCCDObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}};
   ccd_t ccd;
 
   //------------------------------------- AABB computation, box-box test

src/engine/engine_collision_convex.h

@@ -23,6 +23,7 @@
 
 #include <ccd/vec3.h>
 
+#include <mujoco/mjexport.h>
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mjtnum.h>
@@ -39,8 +40,8 @@
 extern "C" {
 #endif
 
-// internal object type for convex collision algorithms
+// internal object type for convex collision detection
-struct _mjtCCObj {
+struct _mjCCDObj {
   const mjModel* model;
   const mjData* data;
   int geom;
@@ -50,14 +51,15 @@ struct _mjtCCObj {
   int vert;
   mjtNum margin;
   mjtNum rotate[4];
+  mjtNum x0[3];  // initial guess of the witness point
 };
-typedef struct _mjtCCObj mjtCCObj;
+typedef struct _mjCCDObj mjCCDObj;
 
 // support function for convex collision algorithms
-void mjc_support(mjtNum res[3], mjtCCObj* obj, const mjtNum dir[3]);
+MJAPI void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]);
 
 // center function for convex collision algorithms
-void mjc_center(mjtNum res[3], const mjtCCObj *obj);
+MJAPI void mjc_center(mjtNum res[3], const mjCCDObj *obj);
 
 // ccd support function
 void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec);

src/engine/engine_collision_gjk.c

@@ -17,6 +17,7 @@
 #include <stddef.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"
@@ -37,34 +38,38 @@ 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 res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]);
+static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+                    const mjtNum dir[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) {
+  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
 
-
+  // segregated simplices and points for the two objects to recover witness points
-// returns the distance between the two given objects given an initial guess x0
+  mjtNum simplex1[12], simplex2[12];
-mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]) {
+  mjtNum* x1_k = obj1->x0;
-  // our current simplex with max 4 vertices due to only 3 dimensions
+  mjtNum* x2_k = obj2->x0;
-  mjtNum simplex[12];
+  mju_sub3(x_k, x1_k, x2_k);
-  int n = 0;  // number of vertices in the simplex
-
-  // the kth approximation point with initial value x_0
-  mjtNum x_k[3];
-  mju_copy3(x_k, x0);
 
   int N = config->max_iterations;
   for (size_t k = 0; k < N; k++) {
-    mjtNum s_k[3];     // the kth support point
+    mjtNum s1[3], s2[3];  // the support points in obj1 and obj2
-    mjtNum lambda[4];  // barycentric coordinates for x_k
+    mjtNum s_k[3];        // the kth support point of Minkowski difference
+    mjtNum lambda[4];     // barycentric coordinates for x_k
 
     // compute the kth support point in the direction of -(x_k)
     mjtNum x_k_neg[3];
     mju_scl3(x_k_neg, x_k, -1);
     mju_normalize3(x_k_neg);
-    support(s_k, obj1, obj2, x_k_neg);
+    support(s1, s2, obj1, obj2, x_k_neg);
+    mju_sub3(s_k, s1, s2);
 
     // the stopping criteria relies on the Frank-Wolfe duality gap given by
     //  f(x_k) - f(x_min) <= < grad f(x_k), (x_k - s_k) >
@@ -74,36 +79,57 @@ mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const m
       return mju_norm3(x_k);
     }
 
+    // TODO(kylebayes): signedVolume has been written to assume the first vertex is the latest
+    // support to be added. Once the logic has been updated, then this hack should be removed.
+    for (int i = n; i > 0; i--) {
+      // shift the simplex vertices to the right
+      mju_copy3(simplex  + 3*i, simplex  + 3*(i-1));
+      mju_copy3(simplex1 + 3*i, simplex1 + 3*(i-1));
+      mju_copy3(simplex2 + 3*i, simplex2 + 3*(i-1));
+    }
+
     // copy new support point into the simplex
-    mju_copy3(simplex + 3*n++, s_k);
+    mju_copy3(simplex, s_k);
+
+    // copy new support point into the individual simplexes
+    mju_copy3(simplex1, s1);
+    mju_copy3(simplex2, s2);
 
     // run the distance subalgorithm to compute the barycentric coordinates
     // of the closest point to the origin in the simplex
-    signedVolume(lambda, simplex, n);
+    signedVolume(lambda, simplex, ++n);
     lincomb(x_k, lambda, simplex, 4);
 
+    // compute the approximate witness points
+    lincomb(x1_k, lambda, simplex1, 4);
+    lincomb(x2_k, lambda, simplex2, 4);
+
     // for lambda[i] == 0, remove the ith vertex from the simplex
     n = 0;
     for (int i = 0; i < 4; i++) {
       if (lambda[i] == 0) continue;
+      // recover simplex for the two objects
+      mju_copy3(simplex1 + 3*n, simplex1 + 3*i);
+      mju_copy3(simplex2 + 3*n, simplex2 + 3*i);
+
+      // simplex in Minkowski difference
       mju_copy3(simplex + 3*n++, simplex + 3*i);
     }
   }
-
   return mju_norm3(x_k);
 }
 
 
 
 // helper function to compute the support point in the Minkowski difference
-static void support(mjtNum res[3], mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum dir[3]) {
+static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
-  mjtNum s1[3], s2[3], dir_neg[3];
+                    const mjtNum dir[3]) {
+  mjtNum dir_neg[3];
   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(res, s1, s2);
 }
 
 
@@ -198,20 +224,7 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
   }
 
   // find the smallest distance, and use the corresponding barycentric coordinates
-  mjtNum dist;
+  mjtNum dist = mjMAXVAL;
-
-  if (!comp1) {
-    mjtNum lambda_2d[3], verts[9], x[3];
-    mju_copy3(verts, s2);
-    mju_copy3(verts + 3, s3);
-    mju_copy3(verts + 6, s4);
-    S2D(lambda_2d, verts);
-    lincomb(x, lambda_2d, verts, 3);
-    dist = mju_norm3(x);
-    lambda[1] = lambda_2d[0];
-    lambda[2] = lambda_2d[1];
-    lambda[3] = lambda_2d[2];
-  }
 
   if (!comp2) {
     mjtNum lambda_2d[3], verts[9], x[3];
@@ -221,12 +234,11 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
     S2D(lambda_2d, verts);
     lincomb(x, lambda_2d, verts, 3);
     mjtNum d = mju_norm3(x);
-    if (d < dist) {
+    lambda[0] = lambda_2d[0];
-      lambda[0] = lambda_2d[0];
+    lambda[1] = 0;
-      lambda[2] = lambda_2d[1];
+    lambda[2] = lambda_2d[1];
-      lambda[3] = lambda_2d[2];
+    lambda[3] = lambda_2d[2];
-      dist = d;
+    dist = d;
-    }
   }
 
   if (!comp3) {
@@ -240,6 +252,7 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
     if (d < dist) {
       lambda[0] = lambda_2d[0];
       lambda[1] = lambda_2d[1];
+      lambda[2] = 0;
       lambda[3] = lambda_2d[2];
       dist = d;
     }
@@ -257,6 +270,24 @@ static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) {
       lambda[0] = lambda_2d[0];
       lambda[1] = lambda_2d[1];
       lambda[2] = lambda_2d[2];
+      lambda[3] = 0;
+      dist = d;
+    }
+  }
+
+  if (!comp1) {
+    mjtNum lambda_2d[3], verts[9], x[3];
+    mju_copy3(verts, s2);
+    mju_copy3(verts + 3, s3);
+    mju_copy3(verts + 6, s4);
+    S2D(lambda_2d, verts);
+    lincomb(x, lambda_2d, verts, 3);
+    mjtNum d = mju_norm3(x);
+    if (d < dist) {
+      lambda[0] = 0;
+      lambda[1] = lambda_2d[0];
+      lambda[2] = lambda_2d[1];
+      lambda[3] = lambda_2d[2];
       dist = d;
     }
   }
@@ -273,30 +304,29 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
   // compute normal
   mjtNum diff1[3], diff2[3], n[3];
   mju_sub3(diff1, s2, s1);
-  mju_sub3(diff2, s3, s2);
+  mju_sub3(diff2, s3, s1);
   mju_cross(n, diff1, diff2);
 
   // project origin
   mjtNum p_o[3];
   mju_scl3(p_o, n, mju_dot3(n, s1) / mju_dot3(n, n));
 
-  int index;
   mjtNum mu_max = 0;
-  int k = 1, l = 2;
 
-  for (int i = 0; i < 3; i++) {
+  //  Below are the minors M_i4 of the matrix M given by
-    mjtNum mu = s2[k]*s3[l] + s1[k]*s2[l] + s3[k]*s1[l] - s2[k]*s1[l] - s3[k]*s2[l] - s1[k]*s3[l];
+  //  [[ s1_x, s2_x, s3_x, s4_x ],
-    if (mju_abs(mu) >= mju_abs(mu_max)) {
+  //   [ s1_y, s2_y, s3_y, s4_y ],
-      mu_max = mu;
+  //   [ s1_z, s2_z, s3_z, s4_z ],
-      index = i;
+  //   [ 1,    1,    1,    1    ]]
-    }
+  mjtNum M_14 = s2[1]*s3[2] - s2[2]*s3[1] - s1[1]*s3[2] + s1[2]*s3[1] + s1[1]*s2[2] - s1[2]*s2[1];
-    k = l; l = i;
+  mjtNum M_24 = s2[0]*s3[2] - s2[2]*s3[0] - s1[0]*s3[2] + s1[2]*s3[0] + s1[0]*s2[2] - s1[2]*s2[0];
-  }
+  mjtNum M_34 = s2[0]*s3[1] - s2[1]*s3[0] - s1[0]*s3[1] + s1[1]*s3[0] + s1[0]*s2[1] - s1[1]*s2[0];
 
-  // exclude index component
+  // exclude one of the axes with the largest projection of the simplex using the computed minors
   mjtNum s1_2D[2], s2_2D[2], s3_2D[2], p_o_2D[2];
-
+  mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34);
-  if (index == 0) {
+  if (mu1 >= mu2 && mu1 >= mu3) {
+    mu_max = mu1;
     s1_2D[0] = s1[1];
     s1_2D[1] = s1[2];
 
@@ -308,7 +338,8 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
 
     p_o_2D[0] = p_o[1];
     p_o_2D[1] = p_o[2];
-  } else if (index == 1) {
+  } else if (mu2 >= mu3) {
+    mu_max = mu2;
     s1_2D[0] = s1[0];
     s1_2D[1] = s1[2];
 
@@ -321,6 +352,7 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
     p_o_2D[0] = p_o[0];
     p_o_2D[1] = p_o[2];
   } else {
+    mu_max = mu3;
     s1_2D[0] = s1[0];
     s1_2D[1] = s1[1];
 
@@ -357,35 +389,23 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
   }
 
   // find the smallest distance, and use the corresponding barycentric coordinates
-  mjtNum dist;
+  mjtNum dist = mjMAXVAL;
-
-  if (!comp1) {
-    mjtNum lambda_1d[4], verts[6], x[3];
-    mju_copy3(verts, s2);
-    mju_copy3(verts + 3, s3);
-    S1D(lambda_1d, verts);
-    lincomb(x, lambda_1d, verts, 2);
-    dist = mju_norm3(x);
-    lambda[1] = lambda_1d[0];
-    lambda[2] = lambda_1d[1];
-  }
 
   if (!comp2) {
-    mjtNum lambda_1d[4], verts[6], x[3];
+    mjtNum lambda_1d[2], verts[6], x[3];
     mju_copy3(verts, s1);
     mju_copy3(verts + 3, s3);
     S1D(lambda_1d, verts);
     lincomb(x, lambda_1d, verts, 2);
     mjtNum d = mju_norm3(x);
-    if (d < dist) {
+    lambda[0] = lambda_1d[0];
-      lambda[0] = lambda_1d[0];
+    lambda[1] = 0;
-      lambda[2] = lambda_1d[1];
+    lambda[2] = lambda_1d[1];
-      dist = d;
+    dist = d;
-    }
   }
 
   if (!comp3) {
-    mjtNum lambda_1d[4], verts[6], x[3];
+    mjtNum lambda_1d[2], verts[6], x[3];
     mju_copy3(verts, s1);
     mju_copy3(verts + 3, s2);
     S1D(lambda_1d, verts);
@@ -394,6 +414,22 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) {
     if (d < dist) {
       lambda[0] = lambda_1d[0];
       lambda[1] = lambda_1d[1];
+      lambda[2] = 0;
+      dist = d;
+    }
+  }
+
+  if (!comp1) {
+    mjtNum lambda_1d[2], verts[6], x[3];
+    mju_copy3(verts, s2);
+    mju_copy3(verts + 3, s3);
+    S1D(lambda_1d, verts);
+    lincomb(x, lambda_1d, verts, 2);
+    mjtNum d = mju_norm3(x);
+    if (d < dist) {
+      lambda[1] = lambda_1d[0];
+      lambda[2] = lambda_1d[1];
+      lambda[0] = 0;
       dist = d;
     }
   }
@@ -435,5 +471,6 @@ static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) {
     lambda[1] = C2 / mu_max;
   } else {
     lambda[0] = 1;
+    lambda[1] = 0;
   }
 }

src/engine/engine_collision_gjk.h

@@ -23,15 +23,16 @@
 extern "C" {
 #endif
 
-// internal struct with settings for GJK
+// internal configuration for convex collision detection
-struct _mjGjkConfig {
+struct _mjCCDConfig {
-  mjtNum max_iterations;
+  int max_iterations;
   mjtNum tolerance;
 };
-typedef struct _mjGjkConfig mjGjkConfig;
+typedef struct _mjCCDConfig mjCCDConfig;
 
-// Returns the distance between the two objects given an initial guess x0.
+// Returns the distance between the two objects. The witness points are
-MJAPI mjtNum mj_gjk(const mjGjkConfig* config, mjtCCObj* obj1, mjtCCObj* obj2, const mjtNum x0[3]);
+// recoverable from x_0 in obj1 and obj2.
+MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2);
 
 #ifdef __cplusplus
 }

test/engine/engine_collision_gjk_test.cc

@@ -29,18 +29,27 @@ namespace mujoco {
 namespace {
 
 using ::testing::NotNull;
+using ::testing::ElementsAre;
 
-mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum* x_0) {
+constexpr mjtNum kTolerance = 1e-6;
-  mjGjkConfig config = {100, 1e-6};
+constexpr int kMaxIterations = 1000;
-  mjtCCObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
+
-  mjtCCObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
+static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3],
-  return mj_gjk(&config, &obj1, &obj2, x_0);
+               mjtNum x2[3]) {
+  mjCCDConfig config = {kMaxIterations, kTolerance};
+  mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}};
+  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);
+  if (x1 != nullptr) mju_copy3(x1, obj1.x0);
+  if (x2 != nullptr) mju_copy3(x2, obj2.x0);
+  return dist;
 }
 
 using MjGjkTest = MujocoTest;
 
 TEST_F(MjGjkTest, SphereSphereIntersect) {
-  mjtNum x_0[3] = {-2, 0, 0};
   static constexpr char xml[] = R"(
   <mujoco>
   <option>
@@ -61,7 +70,7 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
 
   int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
   int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
-  mjtNum dist = run_gjk(model, data, geom1, geom2, x_0);
+  mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
 
   EXPECT_EQ(dist, 0);
   mj_deleteData(data);
@@ -69,7 +78,6 @@ TEST_F(MjGjkTest, SphereSphereIntersect) {
 }
 
 TEST_F(MjGjkTest, SphereSphere) {
-  mjtNum x_0[3] = {-2, 0, 0};
   static constexpr char xml[] = R"(
   <mujoco>
   <option>
@@ -96,15 +104,17 @@ TEST_F(MjGjkTest, SphereSphere) {
 
   int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
   int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
-  mjtNum dist = run_gjk(model, data, geom1, geom2, x_0);
+  mjtNum x1[3], x2[3];
+  mjtNum dist = run_gjk(model, data, geom1, geom2, x1, x2);
 
   EXPECT_EQ(dist, 1);
+  EXPECT_THAT(x1, ElementsAre(-.5, 0, 0));
+  EXPECT_THAT(x2, ElementsAre(.5, 0, 0));
   mj_deleteData(data);
   mj_deleteModel(model);
 }
 
 TEST_F(MjGjkTest, BoxBox) {
-  mjtNum x_0[3] = {-3, .5, 0};
   static constexpr char xml[] = R"(
   <mujoco>
   <option>
@@ -125,12 +135,68 @@ TEST_F(MjGjkTest, BoxBox) {
 
   int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
   int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
-  mjtNum dist = run_gjk(model, data, geom1, geom2, x_0);
+  mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr);
 
   EXPECT_EQ(dist, 1);
   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"/>
+  </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, 0.7542, .0001);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+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"/>
+  </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, 0.4765, .0001);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco