Back out of gjkIntersect when convergence fails due to algorithm oscillating between two support points. Also, fix related bug in box support function.

PiperOrigin-RevId: 696482893
Change-Id: Icedaf5489b761b8bf03ec30558b5d1128668a647

src/engine/engine_collision_convex.c

@@ -276,9 +276,9 @@ static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
   mjtNum local_dir[3], tmp[3];
   mulMatTVec3(local_dir, mat, dir);
 
-  tmp[0] = mju_sign(local_dir[0]) * size[0];
-  tmp[1] = mju_sign(local_dir[1]) * size[1];
-  tmp[2] = mju_sign(local_dir[2]) * size[2];
+  tmp[0] = (local_dir[0] >= 0 ? 1 : -1) * size[0];
+  tmp[1] = (local_dir[1] >= 0 ? 1 : -1) * size[1];
+  tmp[2] = (local_dir[2] >= 0 ? 1 : -1) * size[2];
 
   // transform result to global frame
   localToGlobal(res, mat, tmp, pos);

src/engine/engine_collision_gjk.c

@@ -147,6 +147,7 @@ static int discreteGeoms(mjCCDObj* obj1, mjCCDObj* obj2) {
 // GJK algorithm
 static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   int get_dist = status->dist_cutoff > 0;  // need to recover geom distances if not in contact
+  int backup_gjk = !get_dist;              // use gjkIntersect if no geom distances needed
   mjtNum *simplex1 = status->simplex1;     // simplex for obj1
   mjtNum *simplex2 = status->simplex2;     // simplex for obj2
   mjtNum *simplex  = status->simplex;      // simplex in Minkowski difference
@@ -203,10 +204,15 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
 
     // tetrahedron is generated and only need contact info; fallback to gjkIntersect to
     // determine contact
-    if (!get_dist && n == 3) {
+    if (n == 3 && backup_gjk) {
       status->gjk_iterations = k;
-      status->nx = 0;
-      return gjkIntersect(status, obj1, obj2) > 0 ? 0 : mjMAXVAL;
+      int ret = gjkIntersect(status, obj1, obj2);
+      if (ret != -1) {
+        status->nx = 0;
+        return ret > 0 ? 0 : mjMAXVAL;
+      }
+      k = status->gjk_iterations;
+      backup_gjk = 0;
     }
 
     // run the distance subalgorithm to compute the barycentric coordinates
@@ -364,6 +370,12 @@ static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
     dist[2] = signedDistance(&normals[6], simplex + s[1], simplex + s[0], simplex + s[3]);
     dist[3] = signedDistance(&normals[9], simplex + s[0], simplex + s[1], simplex + s[2]);
 
+    // if origin is on any affine hull, convergence will fail
+    if (!dist[3] || !dist[2] || !dist[1] || !dist[0]) {
+      status->gjk_iterations = k;
+      return -1;
+    }
+
     // find the face with the smallest distance to the origin
     int i = (dist[0] < dist[1]) ? 0 : 1;
     int j = (dist[2] < dist[3]) ? 2 : 3;