Remove EPA tolerance in the discrete case in nativeccd.

PiperOrigin-RevId: 750649926
Change-Id: I8ab317cd6b227c9486a2916ae9c6445906c23401

src/engine/engine_collision_gjk.c

@@ -59,18 +59,18 @@ typedef struct {
 
 // polytope used in the Expanding Polytope Algorithm (EPA)
 typedef struct {
-  Vertex* verts;     // list of vertices that make up the polytope
+  Vertex* verts;      // list of vertices that make up the polytope
-  int nverts;        // number of vertices
+  int nverts;         // number of vertices
-  Face* faces;       // list of faces that make up the polytope
+  Face* faces;        // list of faces that make up the polytope
-  int nfaces;        // number of faces
+  int nfaces;         // number of faces
-  int maxfaces;      // max number of faces that can be stored in polytope
+  int maxfaces;       // max number of faces that can be stored in polytope
-  Face** map;        // linear map storing faces
+  Face** map;         // linear map storing faces
-  int nmap;          // number of faces in map
+  int nmap;           // number of faces in map
-  struct Horizon {   // polytope boundary edges that can be seen from w
+  struct Horizon {    // polytope boundary edges that can be seen from w
-    int* indices;    // indices of faces on horizon
+    int* indices;     // indices of faces on horizon
-    int* edges;      // corresponding edge of each face on the horizon
+    int* edges;       // corresponding edge of each face on the horizon
-    int nedges;      // number of edges in horizon
+    int nedges;       // number of edges in horizon
-    mjtNum* w;       // point where horizon is created
+    const mjtNum* w;  // point where horizon is created
   } horizon;
 } Polytope;
 
@@ -291,25 +291,32 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
 
 
 // compute the support point in obj1 and obj2 for Minkowski difference
-static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+static inline void support(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
                            const mjtNum dir[3], const mjtNum dir_neg[3]) {
   // obj1
-  obj1->support(s1, obj1, dir);
+  obj1->support(v->vert1, obj1, dir);
   if (obj1->margin > 0 && obj1->geom >= 0) {
     mjtNum margin = 0.5 * obj1->margin;
-    s1[0] += dir[0] * margin;
+    v->vert1[0] += dir[0] * margin;
-    s1[1] += dir[1] * margin;
+    v->vert1[1] += dir[1] * margin;
-    s1[2] += dir[2] * margin;
+    v->vert1[2] += dir[2] * margin;
   }
 
   // obj2
-  obj2->support(s2, obj2, dir_neg);
+  obj2->support(v->vert2, obj2, dir_neg);
   if (obj2->margin > 0 && obj2->geom >= 0) {
     mjtNum margin = 0.5 * obj2->margin;
-    s2[0] += dir_neg[0] * margin;
+    v->vert2[0] += dir_neg[0] * margin;
-    s2[1] += dir_neg[1] * margin;
+    v->vert2[1] += dir_neg[1] * margin;
-    s2[2] += dir_neg[2] * margin;
+    v->vert2[2] += dir_neg[2] * margin;
   }
+
+  // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
+  sub3(v->vert, v->vert1, v->vert2);
+
+  // copy vertex indices of discrete geoms
+  v->index1 = obj1->vertindex;
+  v->index2 = obj2->vertindex;
 }
 
 
@@ -326,17 +333,7 @@ static void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
     scl3(dir_neg, x_k, norm);
     scl3(dir, dir_neg, -1);
   }
-
+  support(v, obj1, obj2, dir, dir_neg);
-  // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
-  support(v->vert1, v->vert2, obj1, obj2, dir, dir_neg);
-  sub3(v->vert, v->vert1, v->vert2);
-  // copy mesh indices
-  if (obj1->vertindex >= 0) {
-    v->index1 = obj1->vertindex;
-  }
-  if (obj2->vertindex >= 0) {
-    v->index2 = obj2->vertindex;
-  }
 }
 
 
@@ -356,16 +353,7 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
 
   int n = pt->nverts++;
   Vertex* v = pt->verts + n;
-
+  support(v, obj1, obj2, dir, dir_neg);
-  // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
-  support(v->vert1, v->vert2, obj1, obj2, dir, dir_neg);
-  sub3(v->vert, v->vert1, v->vert2);
-  if (obj1->vertindex >= 0) {
-    v->index1 = obj1->vertindex;
-  }
-  if (obj2->vertindex >= 0) {
-    v->index2 = obj2->vertindex;
-  }
   return n;
 }
 
@@ -375,15 +363,7 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
 static void gjkIntersectSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
                                 const mjtNum dir[3]) {
   mjtNum dir_neg[3] = {-dir[0], -dir[1], -dir[2]};
-  // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
+  support(v, obj1, obj2, dir, dir_neg);
-  support(v->vert1, v->vert2, obj1, obj2, dir, dir_neg);
-  sub3(v->vert, v->vert1, v->vert2);
-  if (obj1->vertindex >= 0) {
-    v->index1 = obj1->vertindex;
-  }
-  if (obj2->vertindex >= 0) {
-    v->index2 = obj2->vertindex;
-  }
 }
 
 
@@ -1135,9 +1115,7 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
     return mjEPA_P3_ORIGIN_ON_FACE;
   }
 
-
+  // populate face map
-  // if the origin is on the affine hull of any of the faces then the origin is not in the
-  //  hexahedron or the hexahedron is degenerate
   for (int i = 0; i < 6; i++) {
     pt->map[i] = pt->faces + i;
     pt->faces[i].index = i;
@@ -1177,6 +1155,7 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
     return mjEPA_P4_MISSING_ORIGIN;
   }
 
+  // populate face map
   for (int i = 0; i < 4; i++) {
     pt->map[i] = pt->faces + i;
     pt->faces[i].index = i;
@@ -1186,16 +1165,10 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
 }
 
 
-
 // make a copy of vertex in polytope and return its index
 static inline int insertVertex(Polytope* pt, const Vertex* v) {
   int n = pt->nverts++;
-  Vertex* new_v = pt->verts + n;
+  pt->verts[n] = *v;
-  copy3(new_v->vert1, v->vert1);
-  copy3(new_v->vert2, v->vert2);
-  new_v->index1 = v->index1;
-  new_v->index2 = v->index2;
-  sub3(new_v->vert, v->vert1, v->vert2);
   return n;
 }
 
@@ -1344,10 +1317,17 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu
 // return a face of the expanded polytope that best approximates the pentration depth
 // witness points are in status->{x1, x2}
 static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) {
-  mjtNum tolerance = status->tolerance, lower2, upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT;
+  mjtNum upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT, lower2;
-  int k, kmax = status->max_iterations;
   Face* face = NULL, *pface = NULL;  // face closest to origin
+  mjtNum tolerance = status->tolerance;
+  int discrete = discreteGeoms(obj1, obj2);
 
+  // tolerance is not used for discrete geoms
+  if (discrete && sizeof(mjtNum) == sizeof(double)) {
+    tolerance = mjMINVAL;
+  }
+
+  int k, kmax = status->max_iterations;
   for (k = 0; k < kmax; k++) {
     pface = face;
 
@@ -1375,8 +1355,8 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
     // compute support point w from the closest face's normal
     mjtNum lower = mju_sqrt(lower2);
     int wi = epaSupport(pt, obj1, obj2, face->v, lower);
-    mjtNum* w = pt->verts[wi].vert;
+    const Vertex* w = pt->verts + wi;
-    mjtNum upper_k = dot3(face->v, w) / lower;  // upper bound for kth iteration
+    mjtNum upper_k = dot3(face->v, w->vert) / lower;  // upper bound for kth iteration
     if (upper_k < upper) {
       upper = upper_k;
       upper2 = upper * upper;
@@ -1385,7 +1365,20 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
       break;
     }
 
-    pt->horizon.w = w;
+    // check if vertex w is a repeated support point
+    if (discrete) {
+      int i = 0, nverts = pt->nverts - 1;
+      for (; i < nverts; i++) {
+        if (w->index1 == pt->verts[i].index1 && w->index2 == pt->verts[i].index2) {
+          break;
+        }
+      }
+      if (i != nverts) {
+        break;
+      }
+    }
+
+    pt->horizon.w = w->vert;
     horizon(pt, face);
 
     // unrecoverable numerical issue; at least one face was deleted so nedges is 3 or more

test/engine/engine_collision_gjk_test.cc

@@ -1220,6 +1220,70 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
+      <geom name="geom2" type="box" pos="0 0 0" size="0.02 0.02 0.02"/>
+    </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);
+
+  mjtNum* xpos = data->geom_xpos;
+  mjtNum* xmat = data->geom_xmat;
+
+  xmat[0] = 0.9999999980312528347070610834634862840176;
+  xmat[1] = 0.0000179109150612445166097282805983681442;
+  xmat[2] = -0.0000601389470252842008382576644009986921;
+  xmat[3] = -0.0000179108686851742081238159781664265324;
+  xmat[4] = 0.9999999998393023226128661917755380272865;
+  xmat[5] = 0.0000007716871733595438989517368948145570;
+  xmat[6] = 0.0000601389608372434404702858157243383630;
+  xmat[7] = -0.0000007706100310572527002924239115932981;
+  xmat[8] = 0.9999999981913554325529958077822811901569;
+
+  xpos[0] = 0.0002051257133161473724877743585182088282;
+  xpos[1] = 0.0000051793157380883478958571650152542531;
+  xpos[2] = -0.0800031938952457943869944756443146616220;
+
+  xpos = data->geom_xpos + 3;
+  xmat = data->geom_xmat + 9;
+
+  xmat[0] = 0.9999999606378873195922096783760935068130;
+  xmat[1] = -0.0000186818570733572177707156047876679850;
+  xmat[2] = -0.0002799557310143530259108346491814245383;
+  xmat[3] = 0.0000186853252997592718994551708178164517;
+  xmat[4] = 0.9999999997487241110150080203311517834663;
+  xmat[5] = 0.0000123858711158191162315369768243122905;
+  xmat[6] = 0.0002799554995529331168427344955773605761;
+  xmat[7] = -0.0000123911016921886008170612322731862776;
+  xmat[8] = 0.9999999607356884201436741932411678135395;
+
+  xpos[0] = 0.0002145111032389043976328218965576866140;
+  xpos[1] = -0.0000051338999751368759734112059978095033;
+  xpos[2] = -0.0400059009625639144802633495601185131818;
+
+  int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+  int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+
+  mjCCDStatus status;
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
+
+  EXPECT_EQ(ncons, 4);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjGjkTest, SmallBoxMesh) {
   static constexpr char xml[] = R"(
   <mujoco>