Circumvent bug in mjc_BoxBox.

The box-box collider sometimes outputs bad contact points which are far outside one of the two boxes. This happens at configurations which are very rare numerically, but rare configurations (e.g. perfect box alignment) are often induced by the designer and the physics, making them actually not-so-rare.

Since the box collision functions are very difficult to understand, we circumvent the issue by extending the post processing code to detect and remove such contacts.

PiperOrigin-RevId: 589178197
Change-Id: I82b8a888ef04f9196111c960757e3314d0ab6d8e

src/engine/engine_util_misc.c

@@ -837,6 +837,26 @@ 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
+int mju_outsideBox(const mjtNum point[3], const mjtNum pos[3], const mjtNum mat[9],
+                   const mjtNum size[3]) {
+  // 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]) {
+    return 1;
+  }
+
+  // inside
+  return 0;
+}
+
+
+
 // print matrix to screen
 void mju_printMat(const mjtNum* mat, int nr, int nc) {
   for (int r=0; r < nr; r++) {