Check if geoms are separated in GJK after an early exit. Fix #3383.

PiperOrigin-RevId: 960316577
Change-Id: I328322e0b1bd14089defb26e9dc35a264642b4d6

src/engine/engine_collision_gjk.c

@@ -203,6 +203,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   mjtNum lambda[4];                        // barycentric coordinates for x_k
   mjtNum cutoff2 = status->dist_cutoff * status->dist_cutoff;
   mjtNum tol2 = status->tolerance * status->tolerance;
+  status->separated = 0;
 
   // if both geoms are discrete, finite convergence is guaranteed; set tolerance to 0
   mjtNum epsilon = discreteGeoms(obj1, obj2) ? 0 : 0.5 * tol2;
@@ -236,6 +237,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
     // if geom distance isn't requested, return early
     if (!get_dist) {
       if (dot3(x_k, s_k) > 0) {
+        status->separated = 1;
         status->gjk_iterations = k;
         status->nsimplex = 0;
         status->nx = 0;
@@ -245,6 +247,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
     } else if (status->dist_cutoff < mjMAX_LIMIT) {
       mjtNum vs = dot3(x_k, s_k);
       if (vs > 0 && (vs * vs) >= cutoff2 * (x_norm * x_norm)) {
+        status->separated = 1;
         status->gjk_iterations = k;
         status->nsimplex = 0;
         status->nx = 0;
@@ -260,6 +263,7 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
       int ret = gjkIntersect(status, obj1, obj2);
       if (ret != -1) {
         status->nx = 0;
+        status->separated = ret == 0;
         status->dist = ret > 0 ? 0 : mjMAX_LIMIT;
         return;
       }
@@ -285,20 +289,20 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
       status->nsimplex = 0;
       status->nx = 0;
       status->dist = mjMAX_LIMIT;
+      status->separated = 1;
       return;
     }
 
-    // we have a tetrahedron containing the origin so return early
-    if (n == 4) {
-      x_norm = 0;
-      break;
-    }
-
     // get the next iteration of x_k, save previous x_norm
     lincomb(x_k, lambda, n, simplex[0].vert, simplex[1].vert,
             simplex[2].vert, simplex[3].vert);
     x_norm_prev = x_norm;
     x_norm = norm3(x_k);
+
+    // we should have a tetrahedron containing the origin so return early
+    if (n == 4) {
+      break;
+    }
   }
 
   // compute the approximate witness points
@@ -309,6 +313,18 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
             simplex[3].vert2);
   }
 
+  // GJK exited early; do a final separation check
+  Vertex tmp;
+  gjkSupport(&tmp, obj1, obj2, x_k, x_norm);
+  if (dot3(x_k, tmp.vert) > 0) {
+    status->separated = 1;
+  }
+
+  // tetrahedron containing the origin
+  if (n == 4 && status->separated == 0) {
+    x_norm = 0;
+  }
+
   status->nx = 1;
   status->gjk_iterations = k;
   status->nsimplex = n;
@@ -2371,7 +2387,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
     return status->dist;
   }
 
-  if (status->dist <= config->tolerance && status->nsimplex > 1 && config->buffer) {
+  if (status->dist <= config->tolerance && status->nsimplex > 1
+      && config->buffer && !status->separated) {
     status->dist = 0;  // assume touching
     Polytope pt;
     pt.nfaces = pt.nmap = pt.nverts = pt.horizon.nedges = 0;

src/engine/engine_collision_gjk.h

@@ -19,6 +19,7 @@
 #include <stddef.h>
 
 #include <mujoco/mjexport.h>
+#include <mujoco/mjmodel.h>
 #include <mujoco/mjtype.h>
 
 #include "engine/engine_collision_convex.h"
@@ -82,6 +83,7 @@ typedef struct {
 typedef struct {
   // geom distance information
   mjtNum dist;                  // distance between geoms
+  int separated;                // set to true if geoms are verified to be separated
   mjtNum x1[3 * mjMAXCONPAIR];  // witness points for geom 1
   mjtNum x2[3 * mjMAXCONPAIR];  // witness points for geom 2
   int nx;                       // number of witness points

test/engine/engine_collision_gjk_test.cc

@@ -604,7 +604,6 @@ TEST_F(MjGjkTest, BoxBoxTouching) {
 
   ASSERT_EQ(ncons, 0);
   EXPECT_EQ(status.epa_status, -1);
-
 }
 
 TEST_F(MjGjkTest, BoxBoxMultiCCD) {
@@ -1100,7 +1099,6 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
   int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
 
   ASSERT_EQ(ncons, 4);
-
 }
 
 TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
@@ -1220,7 +1218,6 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
   int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
 
   ASSERT_EQ(ncons, 4);
-
 }
 
 TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
@@ -1959,11 +1956,8 @@ TEST_F(MjGjkTest, BoxBoxLarge) {
   int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
   int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
 
-  mjtNum* xmat = data->geom_xmat;
+  mjtNum* xpos = data->geom_xpos + 3;
-  mjtNum* xpos = data->geom_xpos;
+  mjtNum* xmat = data->geom_xmat + 9;
-
-  xpos = data->geom_xpos + 3;
-  xmat = data->geom_xmat + 9;
 
   xpos[0] = -0.000000000043537;
   xpos[1] = -0.000000000012973;
@@ -2131,5 +2125,58 @@ TEST_F(MjGjkTest, BoxEdgeFlipped) {
   EXPECT_NEAR(status.x2[2], 0.700000, kTolerance);
 }
 
+TEST_F(MjGjkTest, ThinBoxGrazing) {
+  // thin boxes separated by +7.6933e-7, below the ccd_tolerance of 1e-6
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" type="box" size="0.015108062742470629 0.00020768330936692047 0.00033708479398415232"/>
+      <geom name="geom2" type="box" size="0.0022889234034685867 0.00039019141130537644 0.00066943745945985227"/>
+    </worldbody>
+  </mujoco>)";
+
+  MjModelPtr model = LoadModelFromString(xml);
+  MjDataPtr data = MakeData(model);
+  mj_forward(model.get(), data.get());
+
+  mjtNum* xpos = data->geom_xpos;
+  mjtNum* xmat = data->geom_xmat;
+
+  xmat[0] =  0.41747699392674587;
+  xmat[1] =  0.7445704580736479;
+  xmat[2] = -0.5208913442416622;
+  xmat[3] =  0.735898261710082;
+  xmat[4] = -0.6133129289446175;
+  xmat[5] = -0.28688150794614425;
+  xmat[6] = -0.5330728917830726;
+  xmat[7] = -0.263556605216739;
+  xmat[8] = -0.8039721437292835;
+
+  xpos = data->geom_xpos + 3;
+  xmat = data->geom_xmat + 9;
+
+  xpos[0] = -0.0045224108719272319;
+  xpos[1] = -0.0099558526994404459;
+  xpos[2] = 0.00749543399075938;
+
+  xmat[0] = -0.11330328891322261;
+  xmat[1] =  0.6014637637569688;
+  xmat[2] = -0.7908246996703808;
+  xmat[3] =  0.9557435215667675;
+  xmat[4] =  0.2834865022971088;
+  xmat[5] =  0.07867479900517776;
+  xmat[6] =  0.2715081687621951;
+  xmat[7] = -0.7469114699230777;
+  xmat[8] = -0.6069650487406557;
+
+  int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
+  int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
+  mjtNum dist = mj_geomDistance(model.get(), data.get(), g1, g2, 1, nullptr);
+
+  // below ccd_tolerance, but still positive distance
+  EXPECT_GE(dist, 0);
+  EXPECT_LE(dist, model->opt.ccd_tolerance);
+}
+
 }  // namespace
 }  // namespace mujoco