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
This commit is contained in:
Yuval Tassa
2023-12-14 13:29:22 -08:00
committed by Copybara-Service
parent 9f6be3135e
commit f2a967348c
5 changed files with 90 additions and 27 deletions
+4 -2
View File
@@ -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);