Fix a box-box bad collision in NativeCCD.

PiperOrigin-RevId: 711773153
Change-Id: I8a526ef5933be19d863e67f40edeb95b0b1249cd

Author: kbayes

src/engine/engine_collision_gjk.c

@@ -890,7 +890,12 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
   if (mju_abs(det3(v1, v3, v4)) < mjMINVAL || mju_abs(det3(v1, v3, v5)) < mjMINVAL ||
       mju_abs(det3(v1, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v3, v4)) < mjMINVAL ||
       mju_abs(det3(v2, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v4, v5)) < mjMINVAL) {
-    return 2;
+    return mjEPA_P2_INVALID_FACES;
+  }
+
+  // check that origin is in the hexahedron
+  if (!testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) {
+    return mjEPA_P2_MISSING_ORIGIN;
   }
 
   // save vertices and get indices for each one
@@ -914,7 +919,7 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
     pt->map[i] = pt->faces + i;
     pt->faces[i].index = i;
     if (pt->faces[i].dist < mjMINVAL) {
-      return 3;
+      return mjEPA_P2_ORIGIN_ON_FACE;
     }
   }
   pt->nmap = 6;
@@ -1003,7 +1008,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
   cross3(n, diff1, diff2);
   mjtNum n_norm = mju_norm3(n);
   if (n_norm < mjMINVAL) {
-    return 4;
+    return mjEPA_P3_BAD_NORMAL;
   }
 
   // negative of triangle normal n
@@ -1016,7 +1021,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
 
   // check that v4 is not contained in the 2-simplex
   if (triPointIntersect(v1, v2, v3, v4)) {
-    return 5;
+    return mjEPA_P3_INVALID_V4;
   }
 
   // get 5th vertex in -n direction
@@ -1026,7 +1031,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
 
   // check that v5 is not contained in the 2-simplex
   if (triPointIntersect(v1, v2, v3, v5)) {
-    return 6;
+    return mjEPA_P3_INVALID_V5;
   }
 
   // if origin does not lie on simplex then we need to check that the hexahedron contains the
@@ -1036,7 +1041,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
   // it but within tolerance from it. In that case the hexahedron could possibly be constructed
   // that doesn't contain the origin, but nonetheless there is penetration depth.
   if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
-    return 7;
+    return mjEPA_P3_MISSING_ORIGIN;
   }
 
   // save vertices and get indices for each one
@@ -1061,7 +1066,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
     pt->map[i] = pt->faces + i;
     pt->faces[i].index = i;
     if (pt->faces[i].dist < mjMINVAL) {
-      return 8;
+      return mjEPA_P3_ORIGIN_ON_FACE;
     }
   }
   pt->nmap = 6;
@@ -1444,7 +1449,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
   obj1->center(status->x1, obj1);
   obj2->center(status->x2, obj2);
   status->gjk_iterations = 0;
-  status->epa_iterations = -1;
+  status->epa_iterations = 0;
+  status->epa_status = mjEPA_NOCONTACT;
   status->tolerance = config->tolerance;
   status->max_iterations = config->max_iterations;
   status->max_contacts = config->max_contacts;
@@ -1551,12 +1557,12 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
     } else {
       ret = polytope4(&pt, status, obj1, obj2);
     }
+    status->epa_status = ret;
 
     // simplex not on boundary (objects are penetrating)
     if (!ret) {
       dist = -epa(status, &pt, obj1, obj2);
     } else {
-      status->epa_iterations = -ret;
       dist = 0;
     }
     mj_freeStack(d);

src/engine/engine_collision_gjk.h

@@ -25,17 +25,30 @@
 extern "C" {
 #endif
 
+// Status of an EPA run
+typedef enum {
+  mjEPA_NOCONTACT           = -1,
+  mjEPA_SUCCESS             = 0,
+  mjEPA_P2_INVALID_FACES,
+  mjEPA_P2_MISSING_ORIGIN,
+  mjEPA_P2_ORIGIN_ON_FACE,
+  mjEPA_P3_BAD_NORMAL,
+  mjEPA_P3_INVALID_V4,
+  mjEPA_P3_INVALID_V5,
+  mjEPA_P3_MISSING_ORIGIN,
+  mjEPA_P3_ORIGIN_ON_FACE,
+} mjEPAStatus;
+
 // configuration for convex collision detection
-struct _mjCCDConfig {
+typedef struct {
   int max_iterations;   // the maximum number of iterations for GJK and EPA
   mjtNum tolerance;     // tolerance used by GJK and EPA
   int max_contacts;     // set to max number of contact points to recover
   mjtNum dist_cutoff;   // set to max geom distance to recover
-};
-typedef struct _mjCCDConfig mjCCDConfig;
+} mjCCDConfig;
 
 // data produced from running GJK and EPA
-struct _mjCCDStatus {
+typedef struct {
   // geom distance information
   mjtNum dist;                  // distance between geoms
   mjtNum x1[3 * mjMAXCONPAIR];  // witness points for geom 1
@@ -50,13 +63,13 @@ struct _mjCCDStatus {
 
   // statistics for debugging purposes
   int gjk_iterations;           // number of iterations that GJK ran
-  int epa_iterations;           // number of iterations that EPA ran (negative if EPA did not run)
+  int epa_iterations;           // number of iterations that EPA ran (zero if EPA did not run)
+  mjEPAStatus epa_status;       // status of the EPA run
   mjtNum simplex1[12];          // the simplex that GJK returned for obj1
   mjtNum simplex2[12];          // the simplex that GJK returned for obj2
   mjtNum simplex[12];           // the simplex that GJK returned for the Minkowski difference
   int nsimplex;                 // size of simplex 1 & 2
-};
-typedef struct _mjCCDStatus mjCCDStatus;
+} mjCCDStatus;
 
 // run general convex collision detection, returns positive for distance, negative for penetration
 MJAPI mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);

test/engine/engine_collision_gjk_test.cc

@@ -335,6 +335,33 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, BoxBoxTouching) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <geom name="geom1" type="box" pos="0 0 1.859913200000001376466229885409" size="1 1 1"/>
+    <geom name="geom2" type="box" pos="0 2 1.859913200000001376466229885409" 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 dir[3], pos[3];
+  mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
+
+  EXPECT_EQ(dist, mjMAXVAL);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjGjkTest, SmallBoxMesh) {
   static constexpr char xml[] = R"(
   <mujoco>