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);