Bit compress face vertices in EPA to reduce memory needed. The parameter ccd_iteration is only supported to 1000, so we only need 10 bits for vertex index.

PiperOrigin-RevId: 885690198
Change-Id: I4c6af170ddc86316d3e9cfaf7ca7e0e4fac8a416

Author: kbayes

src/engine/engine_collision_gjk.c

@@ -51,13 +51,20 @@ static inline void lincomb(mjtNum res[3], const mjtNum* coef, int n, const mjtNu
 
 // one face in a polytope
 typedef struct {
-  int verts[3];      // indices of the three vertices of the face in the polytope
+  int verts;         // indices of the three vertices of the face in the polytope
   int adj[3];        // adjacent faces, one for each edge: [v1,v2], [v2,v3], [v3,v1]
   mjtNum v[3];       // projection of the origin on face, can be used as face normal
   mjtNum dist2;      // squared norm of v; negative if deleted
   int index;         // index in map; -1: not in map, -2: deleted from polytope
 } Face;
 
+// return int[3] with lower 30 bits split between three int
+#define EPA_VERT_EXPAND(n)                     \
+  { (n) & 0x3FF,                               \
+    (n) >> 10 & 0x3FF,                         \
+    (n) >> 20 & 0x3FF }
+
+
 // polytope used in the Expanding Polytope Algorithm (EPA)
 typedef struct {
   Vertex* verts;      // list of vertices that make up the polytope
@@ -1188,9 +1195,7 @@ static inline int maxFaces(Polytope* pt) {
 static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
                                 int adj1, int adj2, int adj3) {
   Face* face = &pt->faces[pt->nfaces++];
-  face->verts[0] = v1;
-  face->verts[1] = v2;
-  face->verts[2] = v3;
+  face->verts = v1 + (v2 << 10) + (v3 << 20);
 
   // adjacent faces
   face->adj[0] = adj1;
@@ -1218,8 +1223,9 @@ static inline void addEdge(Polytope* pt, int index, int edge) {
 
 // get edge index where vertex lies
 static inline int getEdge(Face* face, int vertex) {
-  if (face->verts[0] == vertex) return 0;
-  if (face->verts[1] == vertex) return 1;
+  int verts[3] = EPA_VERT_EXPAND(face->verts);
+  if (verts[0] == vertex) return 0;
+  if (verts[1] == vertex) return 1;
   return 2;
 }
 
@@ -1228,14 +1234,15 @@ static inline int getEdge(Face* face, int vertex) {
 static int horizonRec(Polytope* pt, Face* face, int e) {
     // v is visible from w so it is deleted and adjacent faces are checked
     if (dot3(face->v, pt->horizon.w) - face->dist2 > mjMINVAL) {
+      int verts[3] = EPA_VERT_EXPAND(face->verts);
       deleteFace(pt, face);
 
       // recursively search the adjacent faces on the next two edges
       for (int k = 1; k < 3; k++) {
         int i = (e + k) % 3;
         Face* adjFace = &pt->faces[face->adj[i]];
         if (adjFace->index > -2) {
-          int adjEdge = getEdge(adjFace, face->verts[(i + 1) % 3]);
+          int adjEdge = getEdge(adjFace, verts[(i + 1) % 3]);
           if (!horizonRec(pt, adjFace, adjEdge)) {
             addEdge(pt, face->adj[i], adjEdge);
           }
@@ -1250,24 +1257,25 @@ static int horizonRec(Polytope* pt, Face* face, int e) {
 // create horizon given the face as starting point
 static void horizon(Polytope* pt, Face* face) {
   deleteFace(pt, face);
+  int verts[3] = EPA_VERT_EXPAND(face->verts);
 
   // first edge
   Face* adjFace = &pt->faces[face->adj[0]];
-  int adjEdge = getEdge(adjFace, face->verts[1]);
+  int adjEdge = getEdge(adjFace, verts[1]);
   if (!horizonRec(pt, adjFace, adjEdge)) {
     addEdge(pt, face->adj[0], adjEdge);
   }
 
   // second edge
   adjFace = &pt->faces[face->adj[1]];
-  adjEdge = getEdge(adjFace, face->verts[2]);
+  adjEdge = getEdge(adjFace, verts[2]);
   if (adjFace->index > -2 && !horizonRec(pt, adjFace, adjEdge)) {
     addEdge(pt, face->adj[1], adjEdge);
   }
 
   // third edge
   adjFace = &pt->faces[face->adj[2]];
-  adjEdge = getEdge(adjFace, face->verts[0]);
+  adjEdge = getEdge(adjFace, verts[0]);
   if (adjFace->index > -2 && !horizonRec(pt, adjFace, adjEdge)) {
     addEdge(pt, face->adj[2], adjEdge);
   }
@@ -1278,9 +1286,10 @@ static void horizon(Polytope* pt, Face* face) {
 static mjtNum epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNum x2[3]) {
   // compute affine coordinates for witness points on plane defined by face
   mjtNum lambda[3];
-  Vertex* v1 = pt->verts + face->verts[0];
-  Vertex* v2 = pt->verts + face->verts[1];
-  Vertex* v3 = pt->verts + face->verts[2];
+  int verts[3] = EPA_VERT_EXPAND(face->verts);
+  Vertex* v1 = pt->verts + verts[0];
+  Vertex* v2 = pt->verts + verts[1];
+  Vertex* v3 = pt->verts + verts[2];
   triAffineCoord(lambda, v1->vert, v2->vert, v3->vert, face->v);
 
   // witness point on geom 1
@@ -1306,7 +1315,7 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
     tolerance = mjMINVAL;
   }
 
-  int k, kmax = status->max_iterations;
+  int k, kmax = status->max_iterations < 1000 ? status->max_iterations : 1000;
   for (k = 0; k < kmax; k++) {
     pface = face;
 
@@ -1376,12 +1385,13 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
     }
 
     // attach first face
-    int horIndex = pt->horizon.indices[0], horEdge = pt->horizon.edges[0];
-    Face* horFace = &pt->faces[horIndex];
-    int v1 = horFace->verts[horEdge],
-        v2 = horFace->verts[(horEdge + 1) % 3];
-    horFace->adj[horEdge] = nfaces;
-    mjtNum dist2 = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1);
+    int hznIndex = pt->horizon.indices[0], hznEdge = pt->horizon.edges[0];
+    Face* hznFace = &pt->faces[hznIndex];
+    int hznVerts[3] = EPA_VERT_EXPAND(hznFace->verts);
+    int v1 = hznVerts[hznEdge],
+        v2 = hznVerts[(hznEdge + 1) % 3];
+    hznFace->adj[hznEdge] = nfaces;
+    mjtNum dist2 = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, hznIndex, nfaces + 1);
 
     // unrecoverable numerical issue
     if (dist2 == 0) {
@@ -1401,12 +1411,13 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
       int cur = nfaces + i;                  // index of attached face
       int next = nfaces + (i + 1) % nedges;  // index of next face
 
-      horIndex = pt->horizon.indices[i], horEdge = pt->horizon.edges[i];
-      horFace = &pt->faces[horIndex];
-      v1 = horFace->verts[horEdge];
-      v2 = horFace->verts[(horEdge + 1) % 3];
-      horFace->adj[horEdge] = cur;
-      dist2 = attachFace(pt, wi, v2, v1, cur - 1, horIndex, next);
+      hznIndex = pt->horizon.indices[i], hznEdge = pt->horizon.edges[i];
+      hznFace = &pt->faces[hznIndex];
+      int hznVerts2[3] = EPA_VERT_EXPAND(hznFace->verts);
+      v1 = hznVerts2[hznEdge];
+      v2 = hznVerts2[(hznEdge + 1) % 3];
+      hznFace->adj[hznEdge] = cur;
+      dist2 = attachFace(pt, wi, v2, v1, cur - 1, hznIndex, next);
 
       // unrecoverable numerical issue
       if (dist2 == 0) {
@@ -2078,18 +2089,19 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
 
   mjtNum face1[mjMAX_POLYVERT * 3], face2[mjMAX_POLYVERT * 3], endverts[mjMAX_POLYVERT * 3];
   // get vertices of faces from EPA
-  int v11i = pt->verts[face->verts[0]].index1;
-  int v12i = pt->verts[face->verts[1]].index1;
-  int v13i = pt->verts[face->verts[2]].index1;
-  int v21i = pt->verts[face->verts[0]].index2;
-  int v22i = pt->verts[face->verts[1]].index2;
-  int v23i = pt->verts[face->verts[2]].index2;
-  mjtNum* v11 = pt->verts[face->verts[0]].vert1;
-  mjtNum* v12 = pt->verts[face->verts[1]].vert1;
-  mjtNum* v13 = pt->verts[face->verts[2]].vert1;
-  mjtNum* v21 = pt->verts[face->verts[0]].vert2;
-  mjtNum* v22 = pt->verts[face->verts[1]].vert2;
-  mjtNum* v23 = pt->verts[face->verts[2]].vert2;
+  int verts[3] = EPA_VERT_EXPAND(face->verts);
+  int v11i = pt->verts[verts[0]].index1;
+  int v12i = pt->verts[verts[1]].index1;
+  int v13i = pt->verts[verts[2]].index1;
+  int v21i = pt->verts[verts[0]].index2;
+  int v22i = pt->verts[verts[1]].index2;
+  int v23i = pt->verts[verts[2]].index2;
+  mjtNum* v11 = pt->verts[verts[0]].vert1;
+  mjtNum* v12 = pt->verts[verts[1]].vert1;
+  mjtNum* v13 = pt->verts[verts[2]].vert1;
+  mjtNum* v21 = pt->verts[verts[0]].vert2;
+  mjtNum* v22 = pt->verts[verts[1]].vert2;
+  mjtNum* v23 = pt->verts[verts[2]].vert2;
 
   // get dimensions of features of geoms 1 and 2
   int nface1 = simplexDim(&v11i, &v12i, &v13i, &v11, &v12, &v13);
@@ -2147,7 +2159,7 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
 
   // recover geom1 matching edge or face
   if (edgecon1) {
-    copy3(face1, pt->verts[face->verts[0]].vert1);
+    copy3(face1, pt->verts[verts[0]].vert1);
     copy3(face1 + 3, endverts + 3*i);
     nface1 = 2;
   } else {
@@ -2162,7 +2174,7 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
 
   // recover geom2 matching edge or face
   if (edgecon2) {
-    copy3(face2, pt->verts[face->verts[0]].vert2);
+    copy3(face2, pt->verts[verts[0]].vert2);
     copy3(face2 + 3, endverts + 3*i);
     nface2 = 2;
   } else {
@@ -2369,3 +2381,5 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
   }
   return status->dist;
 }
+
+#undef EPA_VERT_EXPAND