Switch to collision boolean only GJK for contacts instead of using S3D.

PiperOrigin-RevId: 682329224
Change-Id: I3db141b0482610a0a88854f47a46b0b7b694b361

src/engine/engine_collision_gjk.c

@@ -16,6 +16,7 @@
 
 #include <stddef.h>
 #include <stdlib.h>
+#include <string.h>
 
 #include <mujoco/mjtnum.h>
 #include <mujoco/mjmodel.h>
@@ -84,6 +85,9 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]);
 // attaches a face to the polytope with the given vertex indices; returns non-zero on error
 static int attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3);
 
+// returns 1 if objects are in contact, 0 otherwise; status must have initial tetrahedrons
+static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2);
+
 // returns the penetration depth of two convex objects; witness points are in status->{x1, x2}
 static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2);
 
@@ -107,7 +111,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   int get_dist = status->has_distances;  // need to recover geom distances if not in contact
   mjtNum *simplex1 = status->simplex1;   // simplex for obj1
   mjtNum *simplex2 = status->simplex2;   // simplex for obj2
-  mjtNum simplex[12];                    // simplex in Minkowski difference
+  mjtNum *simplex  = status->simplex;    // simplex in Minkowski difference
   int n = 0;                             // number of vertices in the simplex
   int k = 0;                             // current iteration
   int kmax = status->max_iterations;     // max number of iterations
@@ -145,6 +149,12 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
       return mjMAXVAL;
     }
 
+    // tetrahedron is generated and only need contact info; fallback to gjkIntersect to
+    // determine contact
+    if (!get_dist && n == 3) {
+      return gjkIntersect(status, k, obj1, obj2) ? 0 : mjMAXVAL;
+    }
+
     // run the distance subalgorithm to compute the barycentric coordinates
     // of the closest point to the origin in the simplex
     subdistance(lambda, simplex, n + 1);
@@ -229,6 +239,93 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
 
 
 
+// helper function to compute the support point in the Minkowski difference (without normalization)
+// TODO(kylebayes): combine support functions
+void support2(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, 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)
+  obj1->support(s1, obj1, dir);
+  obj2->support(s2, obj2, dir_neg);
+}
+
+
+
+// compute the signed distance of a face along with the normal
+static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const mjtNum v2[3],
+                                    const mjtNum v3[3]) {
+  mjtNum diff1[3], diff2[3];
+  mju_sub3(diff1, v3, v1);
+  mju_sub3(diff2, v2, v1);
+  mju_cross(normal, diff1, diff2);
+  mjtNum norm = mju_norm3(normal);
+  if (norm > mjMINVAL && norm < mjMAXVAL) {
+    mjtNum invnorm = 1/norm;
+    normal[0] *= invnorm;
+    normal[1] *= invnorm;
+    normal[2] *= invnorm;
+    return mju_dot3(normal, v1);
+  }
+  return mjMAXVAL;  // cannot recover normal (ignore face)
+}
+
+
+
+// returns 0 if objects are in contact, mjMAXVAL otherwise; status must have initial tetrahedrons
+static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2) {
+  mjtNum simplex1[12], simplex2[12], simplex[12];
+  memcpy(simplex1, status->simplex1, sizeof(mjtNum) * 12);
+  memcpy(simplex2, status->simplex2, sizeof(mjtNum) * 12);
+  memcpy(simplex, status->simplex, sizeof(mjtNum) * 12);
+  int s[4] = {0, 3, 6, 9};
+
+  int kmax = status->max_iterations;
+  for (int k = start; k < kmax; k++) {
+    // compute the signed distance to each face in the simplex along with normals
+    mjtNum dist[4], normals[12];
+    dist[0] = signedDistance(&normals[0], simplex + s[2], simplex + s[1], simplex + s[3]);
+    dist[1] = signedDistance(&normals[3], simplex + s[0], simplex + s[2], simplex + s[3]);
+    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]);
+
+    // 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;
+    int index = (dist[i] < dist[j]) ? i : j;
+
+    // origin inside of simplex (run EPA for contact information)
+    if (dist[index] > 0) {
+      status->nsimplex = 4;
+      for (int n = 0; n < 4; n++) {
+        mju_copy3(status->simplex + 3*n, simplex + s[n]);
+        mju_copy3(status->simplex1 + 3*n, simplex1 + s[n]);
+        mju_copy3(status->simplex2 + 3*n, simplex2 + s[n]);
+      }
+      return 1;
+    }
+
+    // replace worst vertex (farthest from origin) with new candidate
+    support2(simplex1 + s[index], simplex2 + s[index], obj1, obj2, normals + 3*index);
+    mju_sub3(simplex + s[index], simplex1 + s[index], simplex2 + s[index]);
+
+    // found origin outside the Minkowski difference (return no collision)
+    if (mju_dot3(&normals[3*index], simplex + s[index]) < 0) {
+      return 0;
+    }
+
+    // swap vertices in the simplex to retain orientation
+    i = (index + 1) & 3;
+    j = (index + 2) & 3;
+    int swap = s[i];
+    s[i] = s[j];
+    s[j] = swap;
+  }
+  return 0;  // never found origin
+}
+
+
+
 // linear combination of n 3D vectors
 static inline void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n) {
   mju_zero3(res);

src/engine/engine_collision_gjk.h

@@ -48,6 +48,7 @@ struct _mjCCDStatus {
   int epa_iterations;         // number of iterations that EPA ran (negative if EPA did not 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;