Fix bug introduced in 7942fe957e

That change removed some spurious contacts returned by mjc_BoxBox, but also removed some desirable contacts that occur during very deep penetration of two boxes (when one box is completely inside another box). This is now fixed.

PiperOrigin-RevId: 591036252
Change-Id: I84b51f2179fd6fe29618a4e908e86934c6fc6941

src/engine/engine_collision_driver.c

@@ -1444,9 +1444,6 @@ static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2],
 
 // test two geoms for collision, apply filters, add to contact list
 void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
-  // relative distance (1%) outside of which box-box contacts are removed
-  static mjtNum kBoxRemoveMargin = 1.01;
-
   TM_START;
 
   int num, type1, type2, condim;
@@ -1547,11 +1544,16 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) {
       // box sizes with margin
       mjtNum sz1[3] = {size1[0] + margin, size1[1] + margin, size1[2] + margin};
       mjtNum sz2[3] = {size2[0] + margin, size2[1] + margin, size2[2] + margin};
-      mju_scl3(sz1, sz1, kBoxRemoveMargin);
-      mju_scl3(sz2, sz2, kBoxRemoveMargin);
-      // mark as bad if outside box
-      if (mju_outsideBox(con[i].pos, pos1, mat1, sz1) ||
-          mju_outsideBox(con[i].pos, pos2, mat2, sz2)) {
+
+      // relative distance from surface (1%) outside of which box-box contacts are removed
+      static mjtNum kRemoveRatio = 1.01;
+
+      // is the contact outside: 1, inside: -1, within the removal width: 0
+      int out1 = mju_outsideBox(con[i].pos, pos1, mat1, sz1, kRemoveRatio);
+      int out2 = mju_outsideBox(con[i].pos, pos2, mat2, sz2, kRemoveRatio);
+
+      // mark as bad if outside one box and not inside the other box
+      if ((out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1)) {
         con[i].dim = -1;
       }
     }

src/engine/engine_util_misc.c

@@ -837,21 +837,47 @@ mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum k, mjtNum b, mjtNum t)
 
 
 
-// return 1 if point is outside box given by pos, mat, size
+// return 1 if point is outside box given by pos, mat, size * inflate
+// return -1 if point is inside box given by pos, mat, size / inflate
+// return 0 if point is between the inflated and deflated boxes
 int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9],
-                   const mjtNum size[3]) {
+                   const mjtNum size[3], mjtNum inflate) {
+  // check inflation coefficient
+  if (inflate < 1) {
+    mjERROR("inflation coefficient must be >= 1")
+  }
+
   // vector from pos to point, projected to box frame
   mjtNum vec[3] = {point[0]-pos[0], point[1]-pos[1], point[2]-pos[2]};
   mju_rotVecMatT(vec, vec, mat);
 
-  // outside
-  if (vec[0] > size[0] || vec[0] < -size[0] ||
-      vec[1] > size[1] || vec[1] < -size[1] ||
-      vec[2] > size[2] || vec[2] < -size[2]) {
+  // big: inflated box
+  mjtNum big[3] = {size[0], size[1], size[2]};
+  if (inflate > 1) {
+    mju_scl3(big, big, inflate);
+  }
+
+  // check if outside big box
+  if (vec[0] > big[0] || vec[0] < -big[0] ||
+      vec[1] > big[1] || vec[1] < -big[1] ||
+      vec[2] > big[2] || vec[2] < -big[2]) {
     return 1;
   }
 
-  // inside
+  // quick return if no inflation
+  if (inflate == 1) {
+    return -1;
+  }
+
+  // check if inside small (deflated) box
+  mjtNum small[3] = {size[0]/inflate, size[1]/inflate, size[2]/inflate};
+  if (vec[0] < small[0] && vec[0] > -small[0] &&
+      vec[1] < small[1] && vec[1] > -small[1] &&
+      vec[2] < small[2] && vec[2] > -small[2]) {
+    return -1;
+  }
+
+  // within margin between small and big box
   return 0;
 }
 

src/engine/engine_util_misc.h

@@ -78,9 +78,11 @@ MJAPI void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid,
 // integrate spring-damper analytically, return pos(dt)
 MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt);
 
-// return 1 if point is outside box given by pos, mat, size
+// return 1 if point is outside box given by pos, mat, size * inflate
+// return -1 if point is inside box given by pos, mat, size / inflate
+// return 0 if point is between the inflated and deflated boxes
 MJAPI int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9],
-                         const mjtNum size[3]);
+                         const mjtNum size[3], mjtNum inflate);
 
 // print matrix
 MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc);

test/engine/engine_collision_box_test.cc

@@ -94,7 +94,7 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
       }
 
       // expect some contacts to have been removed
-      EXPECT_LT(nmatched, num);
+      EXPECT_LT(nmatched, num) << local_path;
 
       // get box info
       const mjtNum* pos1 =  data->geom_xpos  + 3 * g1;
@@ -103,23 +103,27 @@ TEST_F(MjCollisionBoxTest, BadContacts) {
       const mjtNum* pos2 =  data->geom_xpos  + 3 * g2;
       const mjtNum* mat2 =  data->geom_xmat  + 9 * g2;
       const mjtNum* size2 = model->geom_size + 3 * g2;
+      mjtNum margin = mju_max(model->geom_margin[g1], model->geom_margin[g2]);
 
       // loop over raw contacts, find removed
       for (int i = 0; i < num; i++) {
         if (!match_raw[i]) {
           // === check if outside
 
-          // get margin and adjusted sizes
-          const mjtNum kBoxRemoveMargin = 1.01;
-          mjtNum sz1[3], sz2[3];
-          mju_scl3(sz1, size1, kBoxRemoveMargin);
-          mju_scl3(sz2, size2, kBoxRemoveMargin);
+          mjtNum sz1[3] = {size1[0]+margin, size1[1]+margin, size1[2]+margin};
+          mjtNum sz2[3] = {size2[0]+margin, size2[1]+margin, size2[2]+margin};
 
-          // is contact outside one of the boxes
-          bool outside = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1) ||
-                         mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2);
+          // relative distance (1%) outside of which contacts are removed
+          static mjtNum kRatio = 1.01;
 
-          // expect that removed contact was either outside
+          // is the contact outside: 1, inside: -1, within the removal width: 0
+          int out1 = mju_outsideBox(con_raw[i].pos, pos1, mat1, sz1, kRatio);
+          int out2 = mju_outsideBox(con_raw[i].pos, pos2, mat2, sz2, kRatio);
+
+          // mark as bad if outside one box and not inside the other box
+          bool outside = (out1 == 1 && out2 != -1) || (out2 == 1 && out1 != -1);
+
+          // expect that removed contact was outside
           EXPECT_TRUE(outside);
         }
       }
@@ -222,5 +226,23 @@ TEST_F(MjCollisionBoxTest, DuplicateContacts) {
 }
 
 
+static const char* const kDeepFilePath =
+    "engine/testdata/collision_box/boxbox_deep.xml";
+
+TEST_F(MjCollisionBoxTest, DeepPenetration) {
+  const std::string xml_path = GetTestDataFilePath(kDeepFilePath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
+  ASSERT_THAT(model, NotNull());
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  // expect 4 contact
+  EXPECT_EQ(data->ncon ,4);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+
 }  // namespace
 }  // namespace mujoco

test/engine/testdata/collision_box/boxbox_deep.xml

@@ -0,0 +1,11 @@
+<mujoco>
+  <!-- Box deeply penetrating another box -->
+
+  <worldbody>
+    <geom type="box" size="1 1 1" rgba=".5 .5 .5 .5"/>
+    <body pos=".1 .2 .3">
+      <freejoint/>
+      <geom type="box" size=".2 .2 .2"/>
+    </body>
+  </worldbody>
+</mujoco>