Correct projected origin on face when the magnitude of face->v becomes very small in EPA.

PiperOrigin-RevId: 929768876
Change-Id: I5daf0203e998aa1489d9ce4c78cbf0764a8963e3

src/engine/engine_collision_gjk.c

@@ -93,6 +93,7 @@ typedef struct {
   Face* faces;        // list of faces that make up the polytope
   int nfaces;         // number of faces
   int maxfaces;       // max number of faces that can be stored in polytope
+  mjtNum center[3];   // center of the polytope
   Face** map;         // linear map storing faces
   int nmap;           // number of faces in map
   struct Horizon {    // polytope boundary edges that can be seen from w
@@ -929,6 +930,10 @@ static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtN
 static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
   mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert;
 
+  // set the polytope center
+  add3(pt->center, v1, v2);
+  scl3(pt->center, pt->center, 0.5);
+
   mjtNum diff[3];
   sub3(diff, v2, v1);
 
@@ -997,13 +1002,12 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
     return mjEPA_P2_NONCONVEX;
   }
 
+  // populate face map
   for (int i = 0; i < 6; i++) {
     pt->map[i] = pt->faces + i;
     pt->faces[i].index = i;
   }
   pt->nmap = 6;
-
-  // valid hexahedron for EPA
   return 0;
 }
 
@@ -1077,6 +1081,11 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
                *v2 = status->simplex[1].vert,
                *v3 = status->simplex[2].vert;
 
+  // set the polytope center
+  add3(pt->center, v1, v2);
+  add3(pt->center, pt->center, v3);
+  scl3(pt->center, pt->center, 1.0 / 3.0);
+
   // get normals in both directions
   mjtNum diff1[3], diff2[3], n[3], n_neg[3];
   sub3(diff1, v2, v1);
@@ -1156,6 +1165,12 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
   int v3 = insertVertex(pt, status->simplex + 2);
   int v4 = insertVertex(pt, status->simplex + 3);
 
+  // set the polytope center
+  add3(pt->center, pt->verts[v1].vert, pt->verts[v2].vert);
+  add3(pt->center, pt->center, pt->verts[v3].vert);
+  add3(pt->center, pt->center, pt->verts[v4].vert);
+  scl3(pt->center, pt->center, 0.25);
+
   // if the origin is on a face, replace the 3-simplex with a 2-simplex
   if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINDIST4) {
     replaceSimplex3(pt, status, v1, v2, v3);
@@ -1229,6 +1244,14 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
   if (ret) {
     return 0;
   }
+
+  // ensure projection points outward from the polytope
+  mjtNum outward[3];
+  sub3(outward, pt->verts[v1].vert, pt->center);
+  if (dot3(face->v, outward) < 0) {
+    scl3(face->v, face->v, -1);
+  }
+
   face->dist2 = dot3(face->v, face->v);
   face->index = -1;
 

test/engine/engine_collision_gjk_test.cc

@@ -470,6 +470,128 @@ TEST_F(MjGjkTest, BoxBoxSize05) {
   ASSERT_EQ(ncons, 4);
 }
 
+TEST_F(MjGjkTest, BoxBoxSize05b) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" type="box" size="0.5 0.5 0.5"/>
+      <geom name="geom2" type="box" size="0.5 0.5 0.5"/>
+    </worldbody>
+  </mujoco>)";
+
+  TestModel model = LoadModel(xml);
+  TestData data = MakeData(model.get());
+  mj_forward(model.get(), data.get());
+
+  mjtNum* xmat = data->geom_xmat;
+  mjtNum* xpos = data->geom_xpos;
+
+  xmat[0] = 1.000000000000000;
+  xmat[1] = -0.000000008764291;
+  xmat[2] = 0.000000386995168;
+  xmat[3] = 0.000000008764733;
+  xmat[4] = 1.000000000000000;
+  xmat[5] = -0.000001144207772;
+  xmat[6] = -0.000000386995168;
+  xmat[7] = 0.000001144207772;
+  xmat[8] = 1.000000000000000;
+
+  xpos[0] = 0.000000962082822;
+  xpos[1] = -0.000001747370789;
+  xpos[2] = 3.469238519668579;
+
+  xmat = data->geom_xmat + 9;
+  xpos = data->geom_xpos + 3;
+
+  xmat[0] = 1.000000000000000;
+  xmat[1] = 0.000000003313412;
+  xmat[2] = -0.000000196321196;
+  xmat[3] = -0.000000003313673;
+  xmat[4] = 1.000000000000000;
+  xmat[5] = -0.000001329654879;
+  xmat[6] = 0.000000196321196;
+  xmat[7] = 0.000001329654879;
+  xmat[8] = 1.000000000000000;
+
+  xpos[0] = 0.000002897753802;
+  xpos[1] = -0.000004625266229;
+  xpos[2] = 4.435211658477783;
+
+  int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
+  int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
+
+  mjCCDStatus status;
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
+
+  ASSERT_EQ(ncons, 4);
+  EXPECT_NEAR(dir[0], 0, 1e-5);
+  EXPECT_NEAR(dir[1], 0, 1e-5);
+  EXPECT_NEAR(dir[2], 1, kTolerance);
+}
+
+TEST_F(MjGjkTest, BoxBoxSize05c) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" type="box" size="0.5 0.5 0.5"/>
+      <geom name="geom2" type="box" size="0.5 0.5 0.5"/>
+    </worldbody>
+  </mujoco>)";
+
+  TestModel model = LoadModel(xml);
+  TestData data = MakeData(model.get());
+  mj_forward(model.get(), data.get());
+
+  mjtNum* xmat = data->geom_xmat;
+  mjtNum* xpos = data->geom_xpos;
+
+  xmat[0] = 1.000000000000000;
+  xmat[1] = -0.000000000570266;
+  xmat[2] = 0.000000168314983;
+  xmat[3] = 0.000000000570290;
+  xmat[4] = 1.000000000000000;
+  xmat[5] = -0.000000142656916;
+  xmat[6] = -0.000000168314983;
+  xmat[7] = 0.000000142656916;
+  xmat[8] = 1.000000000000000;
+
+  xpos[0] = 0.000000188941314;
+  xpos[1] = -0.000000195227585;
+  xpos[2] = 0.497281551361084;
+
+  xmat = data->geom_xmat + 9;
+  xpos = data->geom_xpos + 3;
+
+  xmat[0] = 1.000000000000000;
+  xmat[1] = -0.000000000058608;
+  xmat[2] = 0.000001607574859;
+  xmat[3] = 0.000000000060496;
+  xmat[4] = 1.000000000000000;
+  xmat[5] = -0.000001174638669;
+  xmat[6] = -0.000001607574859;
+  xmat[7] = 0.000001174638669;
+  xmat[8] = 1.000000000000000;
+
+  xpos[0] = 0.000000953407323;
+  xpos[1] = -0.000000923845278;
+  xpos[2] = 1.493984460830688;
+
+  int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
+  int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
+
+  mjCCDStatus status;
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4);
+
+  ASSERT_EQ(ncons, 4);
+  EXPECT_NEAR(dir[0], 0, 1e-5);
+  EXPECT_NEAR(dir[1], 0, 1e-5);
+  EXPECT_NEAR(dir[2], 1, kTolerance);
+}
+
 TEST_F(MjGjkTest, BoxBoxTouching) {
   static constexpr char xml[] = R"(
   <mujoco>