chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Rewrite box-box collision: SAT + polygon clipping + true edge-edge contacts.

Browse Source 
Replaces the box-box collider's manifold generation and post-filtering with a
single structured implementation, and deletes the accumulated repair logic it
obsoletes. Net 319 lines out of the engine.

Algorithm:
- The separating-axis test keeps the closed-form support evaluation and chooses
  the axis of maximum separation among the 15 candidates by plain argmax.
  Edge-cross axes whose cross product has norm below rounding are skipped: in
  the nearly-parallel regime their direction is cancellation noise, previously
  the source of arbitrary-normal contacts with box-scale spurious depth. A
  winning edge axis within eight degrees of the best face axis is replaced by
  that face unless it is better by five percent (ODE's classic fudge): resting
  stacks otherwise flip between the edge and face contact codes by rounding
  noise from step to step, thrashing the solver warm start until the stack
  explodes. The substitution runs after the search rather than filtering during
  it, so a worse non-aliasing edge cannot steal the contact the substitution
  meant to give to the face.
- Face contacts clip the incident face against the reference face's side planes
  (Sutherland-Hodgman). Depth is measured along the reference normal only,
  never as a Euclidean distance between unrelated points. Contact position is
  midway between the surfaces along the normal, so its distance to either box
  is bounded by half the contact depth. Every surviving vertex of the clipped
  polygon becomes a contact, so the manifold is the actual contact patch, at
  most eight points as before.
- Edge contacts use the closest-point pair between the two supporting edge
  segments. A near-zero axis component makes the support-corner sign ambiguous;
  both signs are enumerated and the closest witness pair wins.
- Margin is an acceptance band throughout: SAT early-out and clip acceptance.
- The rounding thresholds are stated per precision. The separation tests are
  the ones that cost correctness: comparing exactly against the margin reports
  a pair overlapping by less than the rounding error of its own support
  evaluation as separated, and the boxes pass through each other. Over 239k
  overlapping pairs that is eight misses under mjUSESINGLE and none in double;
  the collider this replaces misses the same eight. Slack proportional to the
  summed half-sizes leaves five, which overlap by 7e-9 to 3e-8 of their own
  scale, below single-precision epsilon, where the boxes are not distinguishable
  from touching. Erring toward contact is the safe direction: the driver already
  excludes a contact whose distance reaches the margin.

Deleted: the conditional acceptance cascade keyed on how many points earlier
generators emitted, the u/v clamping that fabricated contacts from out-of-range
projections, the outside-box removal filter and its missing-fallback hole,
exact-floating-point deduplication, and the edge-path depth clamp. The
structure makes those bug classes unrepresentable rather than filtered: depth
is a projection by construction. Every reported depth is the exact support
overlap along the contact's own normal, verified over 246k overlapping poses to
within two ulps; the face preference costs direction, not depth, deviating from
the minimum-translation axis by at most 8.1 degrees and 5.3% of its depth.

The previous implementation is preserved verbatim as mjc_BoxBoxLegacy in
test/engine/boxbox_legacy.c, a static library that only the box-box tests link,
so the claims above are measured rather than asserted. It needs no private
engine symbols. Three tests compare against it:

- NearAlignedManifoldIsExact sweeps the relative angle of a resting pair across
  the regime where the edge-cross axes degenerate into noise, pinning the full
  clipped polygon and a contact normal equal to the face normal exactly, where
  the previous collider drifts off it.
- AlignedTowerStands settles a twenty-box tower, which comes to rest four
  million times quieter than under the previous collider, which never settles
  and eventually topples.
- ShallowOverlapSurvivesRounding pins a pair overlapping by 7e-8 of its scale,
  reported as separated under mjUSESINGLE without slack on the separation tests.

On stacks of plates across aspect ratios from 4:1 to 25:1, five layouts each,
the collider settles into a tight band of 1e-4 to 3e-4 while the previous one
intermittently blows up to as much as 2.6e-2.

engine_collision_box_fuzz_test.cc cross-validates randomized poses against
GJK/EPA on identical box meshes and against a spherical-Fibonacci support
sweep, with hard gates per sample: no phantom penetration, no missed contact at
zero margin, no contact deeper than the true depth, contacts within half their
own depth of both boxes, and one normal per manifold. Both invocations run in
about a second.

EdgeContactAtDepthBound's tolerance widens to the five percent design band; the
three-orders-of-magnitude depth bug it pins is still caught, the deviation
being 0.13 percent of the depth.

The 100-box pile benchmark steps about 7% faster with 1.6% fewer contacts.

PiperOrigin-RevId: 965114952
Change-Id: Ie98cdcce8d1aed3ff2da938cb29703fd9c241258
This commit is contained in:
Yuval Tassa
2026-08-15 01:45:08 -07:00
committed by Copybara-Service
parent cc7fb98cf4
commit 86e9860106
9 changed files with 2315 additions and 825 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+1
View File
@@ -41,6 +41,7 @@ Engine
iterative solve for ``qacc_smooth``, which now converges on :ref:`tolerance<option-tolerance>` rather than a fixed
threshold. Flexes with :ref:`elastic2d<flex-elasticity-elastic2d>` stretch stiffness step roughly twice as fast;
bending-only flexes keep the exact constant factor and are unchanged.
- Rewrote cleaner box-box SAT collider.
.. admonition:: Breaking API changes
:class: attention
mjx/mujoco/mjx/_src/collision_driver_test.py
+1 -1
View File
@@ -692,7 +692,7 @@ class ConvexTest(absltest.TestCase):
c.frame[:, 0, :], np.array([[0.0, 0.0, 1.0]] * 4), decimal=2
)
np.testing.assert_array_almost_equal(
c.frame.reshape((-1, 9)), d.contact.frame[:4, :]
c.frame.reshape((-1, 9)), d.contact.frame[:4, :], decimal=2
)
_BOX_BOX_EDGE = """
src/engine/engine_collision_box.c
+406 -815
View File
File diff suppressed because it is too large Load Diff
test/engine/CMakeLists.txt
+11 -1
View File
@@ -12,7 +12,17 @@
# See the License for the specific language governing permissions and
# limitations under the License.
mujoco_test(engine_collision_box_test ADDITIONAL_LINK_LIBRARIES ccd)
# the pre-rewrite box-box collider, linked only into the box-box tests so the rewrite is
# measured against it rather than asserted to be better; see boxbox_legacy.h
add_library(boxbox_legacy STATIC boxbox_legacy.h boxbox_legacy.c)
target_include_directories(boxbox_legacy PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/../..)
target_include_directories(boxbox_legacy PRIVATE ${mujoco_SOURCE_DIR}/include)
target_compile_definitions(boxbox_legacy PUBLIC MJSTATIC)
target_link_libraries(boxbox_legacy PUBLIC mujoco::mujoco)
mujoco_test(engine_collision_box_test ADDITIONAL_LINK_LIBRARIES ccd boxbox_legacy)
mujoco_test(engine_collision_box_fuzz_test ADDITIONAL_LINK_LIBRARIES ccd)
mujoco_test(engine_collision_continuous_test)
test/engine/boxbox_legacy.c
+881
View File
@@ -0,0 +1,881 @@
// Copyright 2016 Svetoslav Kolev
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// The box-box collider as it stood before the separating-axis rewrite (MuJoCo 3.11.1),
// preserved verbatim as a test-only fixture so the rewrite's improvements are measured
// rather than asserted. Nothing in the engine links this file; it is built only into the
// box-box tests.
#include "test/engine/boxbox_legacy.h"
#include <math.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_inline.h"
#include "src/engine/engine_util_blas.h"
#include "src/engine/engine_util_misc.h"
// rounding slack for the edge-edge depth bound: relative, and absolute times the sum of
// half-sizes; wide enough to cover rounding between two computations of the same overlap,
// orders of magnitude below the box-scale depths of spurious clipping artifacts
#ifdef mjUSESINGLE
#define mjDEPTHSLACKREL 1e-4f
#define mjDEPTHSLACKABS 1e-5f
#else
#define mjDEPTHSLACKREL 1e-6
#define mjDEPTHSLACKABS 1e-12
#endif
// internal box : box
static inline
int boxboxLegacyRaw(const mjModel* M, const mjData* D, mjPreContact* con, int g1, int g2, mjtNum margin) {
const mjtNum* pos1 = D->geom_xpos + 3 * g1;
const mjtNum* pos2 = D->geom_xpos + 3 * g2;
const mjtNum* mat1 = D->geom_xmat + 9 * g1;
const mjtNum* mat2 = D->geom_xmat + 9 * g2;
const mjtNum* size1 = M->geom_size + 3 * g1;
const mjtNum* size2 = M->geom_size + 3 * g2;
mjtNum pos12[3], pos21[3], rot[9], rott[9], rotabs[9], rottabs[9], tmp1[3], tmp2[3], plen1[3],
plen2[3];
mjtNum rotmore[9], p[3], r[9], s[3], ss[3], lp[3], rt[9], points[mjMAXCONPAIR][3],
depth[mjMAXCONPAIR], pts[6][3], ppts2[4][2], pu[4][3], axi[3][3];
mjtNum linesu[4][6], lines[4][6], clnorm[3], rnorm[3];
mjtNum penetration, c1, c2, c3, a, b, c, d, lx, ly, hz, l, x, y, u, v, llx, lly, innorm, margin2;
mjtNum maxdepth;
int i0, i1, i2;
mjtNum f0, f1, f2;
int i, j, q, code, q1, q2, clcorner, n, m, k;
int cle1, cle2, in, ax1, ax2, pax1, pax2, clface, nl, nf;
n = 0;
code = -1;
margin2 = margin * margin;
mji_sub3(tmp1, pos2, pos1);
mji_mulMatTVec3(pos21, mat1, tmp1);
mji_sub3(tmp1, pos1, pos2);
mji_mulMatTVec3(pos12, mat2, tmp1);
mju_mulMatTMat3(rot, mat1, mat2);
mju_transpose(rott, rot, 3, 3);
for (i = 0; i < 9; i++)
rotabs[i] = mju_abs(rot[i]);
for (i = 0; i < 9; i++)
rottabs[i] = mju_abs(rott[i]);
mji_mulMatVec3(plen2, rotabs, size2);
mji_mulMatTVec3(plen1, rotabs, size1);
for (i = 0, penetration = margin; i < 3; i++)
penetration += size1[i] * 3 + size2[i] * 3;
for (i = 0; i < 3; i++) {
c1 = -mju_abs(pos21[i]) + size1[i] + plen2[i];
c2 = -mju_abs(pos12[i]) + size2[i] + plen1[i];
if (c1 < -margin || c2 < -margin)
return 0;
if (c1 < penetration) {
penetration = c1;
code = i + 3 * (pos21[i] < 0) + 0;
}
if (c2 < penetration) {
penetration = c2;
code = i + 3 * (pos12[i] < 0) + 6;
}
// printf("%24.16e %24.16e %d %24.16e %d \n",c1,c2,i,penetration,code);
}
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
mju_zero3(tmp2);
if (i == 0) {
tmp2[1] = -rott[3 * j + 2];
tmp2[2] = +rott[3 * j + 1];
} else if (i == 1) {
tmp2[0] = +rott[3 * j + 2];
tmp2[2] = -rott[3 * j + 0];
} else if (i == 2) {
tmp2[0] = -rott[3 * j + 1];
tmp2[1] = +rott[3 * j + 0];
}
c1 = mju_normalize3(tmp2);
if (c1 < mjMINVAL)
continue;
c2 = mju_dot3(pos21, tmp2);
c3 = 0;
for (k = 0; k < 3; k++)
if (k != i)
c3 += size1[k] * mju_abs(tmp2[k]);
for (k = 0; k < 3; k++)
if (k != j)
c3 += size2[k] * rotabs[3 * i + 3 - k - j] / c1;
c3 -= mju_abs(c2);
if (c3 < -margin)
return 0;
if (c3 < penetration * (1 - 1e-12))
{
penetration = c3;
for (k = cle1 = 0; k < 3; k++)
if (k != i)
if ((tmp2[k] > 0) ^ (c2 < 0))
cle1 += 1 << k;
for (k = cle2 = 0; k < 3; k++)
if (k != j)
if ((rot[3 * i + 3 - k - j] > 0) ^ (c2 < 0) ^ ((k - j + 3) % 3 == 1))
cle2 += 1 << k;
code = 12 + i * 3 + j;
mji_copy3(clnorm, tmp2);
in = c2 < 0;
}
// printf("%24.16e %d %24.16e %d\n",c3,12+i*3+j,penetration,code);
}
}
// return 0;
// printf("%d\n",code);
if (code == -1)
return 0; // shouldn't happen
if (code >= 12)
goto edgeedge;
q1 = code % 6;
q2 = code / 6;
// printf("%d %d\n",q1,q2);
mju_zero(rotmore, 9);
if (q1 == 0)
rotmore[2] = -1, rotmore[4] = +1, rotmore[6] = +1;
else if (q1 == 1)
rotmore[0] = +1, rotmore[5] = -1, rotmore[7] = +1;
else if (q1 == 2)
rotmore[0] = +1, rotmore[4] = +1, rotmore[8] = +1;
else if (q1 == 3)
rotmore[2] = +1, rotmore[4] = +1, rotmore[6] = -1;
else if (q1 == 4)
rotmore[0] = +1, rotmore[5] = +1, rotmore[7] = -1;
else if (q1 == 5)
rotmore[0] = -1, rotmore[4] = +1, rotmore[8] = -1;
i0 = 0;
i1 = 1;
i2 = 2;
f0 = f1 = f2 = 1;
if (q1 == 0) {
i0 = 2;
f0 = -1;
i2 = 0;
} else if (q1 == 1) {
i1 = 2;
f1 = -1;
i2 = 1;
} else if (q1 == 2) {
} else if (q1 == 3) {
i0 = 2;
i2 = 0;
f2 = -1;
} else if (q1 == 4) {
i1 = 2;
i2 = 1;
f2 = -1;
} else if (q1 == 5) {
f0 = -1;
f2 = -1;
}
#define rotaxis(vecres, vecin) \
{ \
vecres[0]=vecin[i0]*f0; \
vecres[1]=vecin[i1]*f1; \
vecres[2]=vecin[i2]*f2; \
}
#define rotmatx(matres, matin) \
{ \
mji_scl3(matres+0, matin+i0*3, f0); \
mji_scl3(matres+3, matin+i1*3, f1); \
mji_scl3(matres+6, matin+i2*3, f2); \
}
if (q2) {
mju_mulMatMatT3(r, rotmore, rot);
// mju_mulMatVec3(p,rotmore,pos12);
// mju_mulMatVec3(tmp1,rotmore,size2);
rotaxis(p, pos12);
rotaxis(tmp1, size2);
mji_copy3(s, size1);
} else {
// mju_mulMatMat(r,rotmore,rot,3,3,3);
rotmatx(r, rot);
// mju_mulMatVec3(p,rotmore,pos21);
// mju_mulMatVec3(tmp1,rotmore,size1);
rotaxis(p, pos21);
rotaxis(tmp1, size1);
mji_copy3(s, size2);
}
mju_transpose(rt, r, 3, 3);
for (i = 0; i < 3; i++)
ss[i] = mju_abs(tmp1[i]);
lx = ss[0];
ly = ss[1];
hz = ss[2];
p[2] -= hz;
mji_copy3(lp, p);
for (clcorner = 0, i = 0; i < 3; i++)
if (r[6 + i] < 0)
clcorner += 1 << i;
mji_addToScl3(lp, rt + 0, s[0] * ((clcorner & 1) ? 1 : -1));
mji_addToScl3(lp, rt + 3, s[1] * ((clcorner & 2) ? 1 : -1));
mji_addToScl3(lp, rt + 6, s[2] * ((clcorner & 4) ? 1 : -1));
m = k = 0;
mji_copy3(pts[m++], lp);
for (i = 0; i < 3; i++)
if (mju_abs(r[6 + i]) < 0.5)
mju_scl3(pts[m++], rt + 3 * i, s[i] * ((clcorner & (1 << i)) ? -2 : 2));
mji_add3(pts[3], pts[0], pts[1]);
mji_add3(pts[4], pts[0], pts[2]);
mji_add3(pts[5], pts[3], pts[2]);
if (m > 1)
{
mji_copy3(lines[k] + 0, pts[0]);
mji_copy3(lines[k++] + 3, pts[1]);
}
if (m > 2)
{
mji_copy3(lines[k] + 0, pts[0]);
mji_copy3(lines[k++] + 3, pts[2]);
mji_copy3(lines[k] + 0, pts[3]);
mji_copy3(lines[k++] + 3, pts[2]);
mji_copy3(lines[k] + 0, pts[4]);
mji_copy3(lines[k++] + 3, pts[1]);
}
for (i = 0; i < k; i++) {
for (q = 0; q < 2; q++) {
a = lines[i][0 + q];
b = lines[i][3 + q];
c = lines[i][1 - q];
d = lines[i][4 - q];
if (mju_abs(b) > mjMINVAL) {
for (j = -1; j <= 1; j += 2) {
l = ss[q] * j;
c1 = (l - a) * (1 / b);
if (c1 < 0 || c1 > 1)
continue;
c2 = c + d * c1;
if (mju_abs(c2) > ss[1 - q])
continue;
if (n < mjMAXCONPAIR) {
mji_copy3(points[n], lines[i]);
mji_addToScl3(points[n++], lines[i] + 3, c1);
}
}
}
}
}
a = pts[1][0];
b = pts[2][0];
c = pts[1][1];
d = pts[2][1];
c1 = a * d - b * c;
if (m > 2) {
for (i = 0; i < 4; i++) {
llx = i / 2 ? lx : -lx;
lly = i % 2 ? ly : -ly;
x = llx - pts[0][0];
y = lly - pts[0][1];
u = (x * d - y * b) * (1 / c1);
v = (y * a - x * c) * (1 / c1);
if (u <= 0 || v <= 0 || u >= 1 || v >= 1)
continue;
if (n < mjMAXCONPAIR) {
points[n][0] = llx;
points[n][1] = lly;
points[n][2] = (pts[0][2] + u * pts[1][2] + v * pts[2][2]);
n++;
}
}
}
for (i = 0; i < (1 << (m - 1)); i++) {
mji_copy3(tmp1, pts[i == 0 ? 0 : i + 2]);
if (i)
if (tmp1[0] <= -lx || tmp1[0] >= lx)
continue;
if (i)
if (tmp1[1] <= -ly || tmp1[1] >= ly)
continue;
if (n < mjMAXCONPAIR) {
mji_copy3(points[n++], tmp1);
}
}
m = n;
n = 0;
for (i = 0; i < m; i++) {
if (points[i][2] > margin)
continue;
if (n != i) mji_copy3(points[n], points[i]);
depth[n] = points[n][2];
points[n][2] *= 0.5;
n++;
}
mju_mulMatMatT3(r, q2 ? mat2 : mat1, rotmore);
mju_copy3(p, q2 ? pos2 : pos1);
tmp2[0] = (q2 ? -1 : 1) * r[2];
tmp2[1] = (q2 ? -1 : 1) * r[5];
tmp2[2] = (q2 ? -1 : 1) * r[8];
mji_copy3(con[0].normal, tmp2);
mji_zero3(con[0].tangent);
for (i = 0; i < n; i++) {
con[i].dist = 2 * points[i][2];
points[i][2] += hz;
mji_mulMatVec3(tmp2, r, points[i]);
mji_add3(con[i].pos, tmp2, p);
if (i) {
mji_copy3(con[i].normal, con[0].normal);
mji_zero3(con[i].tangent);
}
}
// printf("Path1: %d\n",n);
return n;
edgeedge:
code -= 12;
q1 = code / 3;
q2 = code % 3;
if (q2 == 0)
ax1 = 1, ax2 = 2;
if (q2 == 1)
ax1 = 0, ax2 = 2;
if (q2 == 2)
ax1 = 1, ax2 = 0;
if (q1 == 0)
pax1 = 1, pax2 = 2;
if (q1 == 1)
pax1 = 0, pax2 = 2;
if (q1 == 2)
pax1 = 1, pax2 = 0;
// printf("%lf %lf %lf %lf\n",rot[ 3*q1+ ax1],rot [3*q1+ ax2],rott[3*q2+pax1],rott[3*q2+pax2]);
// printf("%lf %lf\n",mju_dot3(clnorm,rott+3*ax1),mju_dot3(clnorm,rott+3*ax2));
if (rotabs [3 * q1 + ax1] < rotabs [3 * q1 + ax2]) {
ax1 = ax2;
ax2 = 3 - q2 - ax1;
}
if (rottabs[3 * q2 + pax1] < rottabs[3 * q2 + pax2]) {
pax1 = pax2;
pax2 = 3 - q1 - pax1;
}
if (cle1 & (1 << pax2))
clface = pax2;
else
clface = pax2 + 3;
// printf("%lf - %d %d %d %d %d %d %d %d %d %d %d\n",
// penetration,cle1,cle2,code,in,q1,q2,clface,ax1,ax2,pax1,pax2);
mju_zero(rotmore, 9);
if (clface == 0)
rotmore[2] = -1, rotmore[4] = +1, rotmore[6] = +1;
else if (clface == 1)
rotmore[0] = +1, rotmore[5] = -1, rotmore[7] = +1;
else if (clface == 2)
rotmore[0] = +1, rotmore[4] = +1, rotmore[8] = +1;
else if (clface == 3)
rotmore[2] = +1, rotmore[4] = +1, rotmore[6] = -1;
else if (clface == 4)
rotmore[0] = +1, rotmore[5] = +1, rotmore[7] = -1;
else if (clface == 5)
rotmore[0] = -1, rotmore[4] = +1, rotmore[8] = -1;
i0 = 0;
i1 = 1;
i2 = 2;
f0 = f1 = f2 = 1;
if (clface == 0) {
i0 = 2;
f0 = -1;
i2 = 0;
} else if (clface == 1) {
i1 = 2;
f1 = -1;
i2 = 1;
} else if (clface == 2) {
} else if (clface == 3) {
i0 = 2;
i2 = 0;
f2 = -1;
} else if (clface == 4) {
i1 = 2;
i2 = 1;
f2 = -1;
} else if (clface == 5) {
f0 = -1;
f2 = -1;
}
// mju_mulMatVec3(p,rotmore,pos21);
// mju_mulMatVec3(rnorm,rotmore,clnorm);
rotaxis(p, pos21);
rotaxis(rnorm, clnorm);
// print("rnorm",rnorm);
// mju_mulMatMat(r,rotmore,rot,3,3,3);
rotmatx(r, rot);
mji_mulMatTVec3(tmp1, rotmore, size1);
for (i = 0; i < 3; i++)
s[i] = mju_abs(tmp1[i]);
mju_transpose(rt, r, 3, 3);
lx = s[0];
ly = s[1];
hz = s[2];
p[2] -= hz;
n = 0;
mji_copy3(points[n], p);
mji_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? 1 : -1));
mji_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1));
mji_copy3(points[n + 1], points[n]);
mji_addToScl3(points[n], rt + 3 * q2, size2[q2]);
n = 1;
mji_addToScl3(points[n], rt + 3 * q2, -size2[q2]);
n = 2;
mji_copy3(points[n], p);
mji_addToScl3(points[n], rt + 3 * ax1, size2[ax1] * ((cle2 & (1 << ax1)) ? -1 : 1));
mji_addToScl3(points[n], rt + 3 * ax2, size2[ax2] * ((cle2 & (1 << ax2)) ? 1 : -1));
mji_copy3(points[n + 1], points[n]);
mji_addToScl3(points[n], rt + 3 * q2, size2[q2]);
n = 3;
mji_addToScl3(points[n], rt + 3 * q2, -size2[q2]);
n = 4;
mji_copy3(axi[0], points[0]);
mji_sub3(axi[1], points[1], points[0]);
mji_sub3(axi[2], points[2], points[0]);
if (mju_abs(rnorm[2]) < mjMINVAL)
return 0; // shouldn't happen
innorm = (1 / rnorm[2]) * (in ? -1 : 1);
// printf("%lf\n",innorm);
for (i = 0; i < 4; i++)
{
c1 = -points[i][2] * (1 / rnorm[2]);
mji_copy3(pu[i], points[i]);
mji_addToScl3(points[i], rnorm, c1);
// ppts[i][0]=points[i][0];
// ppts[i][1]=points[i][1];
ppts2[i][0] = points[i][0];
ppts2[i][1] = points[i][1];
}
mji_copy3(pts[0], points[0]);
mji_sub3(pts[1], points[1], points[0]);
mji_sub3(pts[2], points[2], points[0]);
m = 3;
k = 0;
n = 0;
if (m > 1) {
mji_copy3(lines[k] + 0, pts[0]);
mji_copy3(lines[k] + 3, pts[1]);
mji_copy3(linesu[k] + 0, axi[0]);
mji_copy3(linesu[k++] + 3, axi[1]);
}
if (m > 2) {
mji_copy3(lines[k] + 0, pts[0]);
mji_copy3(lines[k] + 3, pts[2]);
mji_copy3(linesu[k] + 0, axi[0]);
mji_copy3(linesu[k++] + 3, axi[2]);
mji_add3(lines[k] + 0, pts[0], pts[1]);
mji_copy3(lines[k] + 3, pts[2]);
mji_add3(linesu[k] + 0, axi[0], axi[1]);
mji_copy3(linesu[k++] + 3, axi[2]);
mji_add3(lines[k] + 0, pts[0], pts[2]);
mji_copy3(lines[k] + 3, pts[1]);
mji_add3(linesu[k] + 0, axi[0], axi[2]);
mji_copy3(linesu[k++] + 3, axi[1]);
}
for (i = 0; i < k; i++) {
for (q = 0; q < 2; q++) {
a = lines[i][0 + q];
b = lines[i][3 + q];
c = lines[i][1 - q];
d = lines[i][4 - q];
if (mju_abs(b) > mjMINVAL) {
for (j = -1; j <= 1; j += 2) {
if (n < mjMAXCONPAIR) {
l = s[q] * j;
c1 = (l - a) * (1 / b);
if (c1 < 0 || c1 > 1)
continue;
c2 = c + d * c1;
if (mju_abs(c2) > s[1 - q])
continue;
if ((linesu[i][2] + linesu[i][5]*c1)*innorm > margin)
continue;
mji_scl3(points[n], linesu[i], 0.5);
mji_addToScl3(points[n], linesu[i] + 3, 0.5 * c1);
points[n][0 + q] += 0.5 * l;
points[n][1 - q] += 0.5 * c2;
depth[n] = points[n][2] * innorm * 2;
n++;
}
}
}
}
}
nl = n;
a = pts[1][0];
b = pts[2][0];
c = pts[1][1];
d = pts[2][1];
c1 = a * d - b * c;
for (i = 0; i < 4; i++) {
if (n < mjMAXCONPAIR) {
llx = i / 2 ? lx : -lx;
lly = i % 2 ? ly : -ly;
x = llx - pts[0][0];
y = lly - pts[0][1];
u = (x * d - y * b) * (1 / c1);
v = (y * a - x * c) * (1 / c1);
if (nl == 0) {
if ((u < 0 || u > 1) && (v < 0 || v > 1))
continue;
} else {
if ((u < 0 || u > 1 || v < 0 || v > 1))
continue;
}
if (u < 0)
u = 0;
if (u > 1)
u = 1;
if (v < 0)
v = 0;
if (v > 1)
v = 1;
mji_scl3(tmp1, pu[0], 1 - u - v);
mji_addToScl3(tmp1, pu[1], u);
mji_addToScl3(tmp1, pu[2], v);
points[n][0] = llx;
points[n][1] = lly;
points[n][2] = 0;
mji_sub3(tmp2, points[n], tmp1);
c1 = mju_dot3(tmp2, tmp2);
if (tmp1[2] > 0)
if (c1 > margin2)
continue;
mji_addTo3(points[n], tmp1);
mju_scl3(points[n], points[n], 0.5);
depth[n] = sqrt(c1) * (tmp1[2] < 0 ? -1 : 1);
n++;
}
}
nf = n;
for (i = 0; i < 4; i++) {
if (n < mjMAXCONPAIR) {
x = ppts2[i][0];
y = ppts2[i][1];
if (nl == 0) {
if (nf == 0) {
} else {
if (x < -lx || x > lx)
if (y < -ly || y > ly)
continue;
}
} else {
if (x < -lx || x > lx || y < -ly || y > ly)
continue;
}
for (c1 = 0, j = 0; j < 2; j++)
if (ppts2[i][j] < -s[j])
c1 += (ppts2[i][j] + s[j]) * (ppts2[i][j] + s[j]);
else if (ppts2[i][j] > s[j])
c1 += (ppts2[i][j] - s[j]) * (ppts2[i][j] - s[j]);
c1 += pu[i][2] * innorm * pu[i][2] * innorm;
if (pu[i][2] > 0)
if (c1 > margin2)
continue;
tmp1[0] = ppts2[i][0] * 0.5;
tmp1[1] = ppts2[i][1] * 0.5;
tmp1[2] = 0;
for (j = 0; j < 2; j++) {
if (ppts2[i][j] < -s[j])
tmp1[j] = -s[j] * 0.5;
else if (ppts2[i][j] > s[j])
tmp1[j] = +s[j] * 0.5;
}
mji_addToScl3(tmp1, pu[i], 0.5);
mji_copy3(points[n], tmp1);
depth[n] = sqrt(c1) * (pu[i][2] < 0 ? -1 : 1);
n++;
}
}
mju_mulMatMatT3(r, mat1, rotmore);
mji_mulMatVec3(tmp1, r, rnorm);
mji_scl3(con[0].normal, tmp1, in ? -1 : 1);
mji_zero3(con[0].tangent);
// no contact can be deeper than the support overlap along the separating axis: clipping
// against a grazing face can synthesize spurious points with arbitrarily large depth;
// the slack covers rounding error so the deepest legitimate point is never rejected
maxdepth = mju_max(0, penetration);
maxdepth += margin + mjDEPTHSLACKREL * maxdepth +
mjDEPTHSLACKABS * (size1[0] + size1[1] + size1[2] +
size2[0] + size2[1] + size2[2]);
for (i = 0, m = 0; i < n; i++) {
if (depth[i] < -maxdepth)
continue;
con[m].dist = depth[i];
points[i][2] += hz;
mji_mulMatVec3(tmp2, r, points[i]);
mji_add3(con[m].pos, tmp2, pos1);
mji_copy3(con[m].normal, con[0].normal);
mji_zero3(con[m].tangent);
m++;
}
return m;
#undef rotaxis
#undef rotmatx
}
// box : box
int mjc_BoxBoxLegacy(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin) {
mjPreContact tmp[mjMAXCONPAIR];
int num = boxboxLegacyRaw(m, d, tmp, g1, g2, margin);
// -1: bad, 0: good
int dupe[mjMAXCONPAIR] = {0};
// get box info
const mjtNum* pos1 = d->geom_xpos + 3 * g1;
const mjtNum* mat1 = d->geom_xmat + 9 * g1;
const mjtNum* size1 = m->geom_size + 3 * g1;
const mjtNum* pos2 = d->geom_xpos + 3 * g2;
const mjtNum* mat2 = d->geom_xmat + 9 * g2;
const mjtNum* size2 = m->geom_size + 3 * g2;
// find bad: contacts outside one of the boxes
int nbad = 0;
for (int i=0; i < num; i++) {
// 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};
// 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(tmp[i].pos, pos1, mat1, sz1, kRemoveRatio);
int out2 = mju_outsideBox(tmp[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)) {
dupe[i] = -1;
nbad++;
}
}
// deep penetration can strand the midpoint-convention position outside both boxes; if
// that removed every contact, restore the penetrating ones: an empty manifold for
// overlapping boxes lets them pass through each other
if (nbad && nbad == num) {
for (int i=0; i < num; i++) {
if (tmp[i].dist < 0) {
dupe[i] = 0;
}
}
}
// find duplicates
for (int i=0; i < num-1; i++) {
if (dupe[i] == -1) {
continue; // already marked bad: skip
}
for (int j=i+1; j < num; j++) {
if (dupe[j] == -1) {
continue; // already marked bad: skip
}
if (tmp[i].pos[0] == tmp[j].pos[0] &&
tmp[i].pos[1] == tmp[j].pos[1] &&
tmp[i].pos[2] == tmp[j].pos[2]) {
dupe[i] = -1;
break;
}
}
}
// consolidate good
int ncon = 0;
for (int j=0; j < num; j++) {
if (dupe[j] == 0) {
con[ncon++] = tmp[j];
if (ncon >= 8) {
break;
}
}
}
return ncon;
}
test/engine/boxbox_legacy.h
+41
View File
@@ -0,0 +1,41 @@
// Copyright 2016 Svetoslav Kolev
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// The box-box collider as it stood before the separating-axis rewrite,
// preserved as a test-only fixture. The differential tests in
// engine_collision_box_test.cc measure the rewrite against it; nothing in the
// engine links this file.
#ifndef MUJOCO_TEST_ENGINE_BOXBOX_LEGACY_H_
#define MUJOCO_TEST_ENGINE_BOXBOX_LEGACY_H_
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtype.h>
#ifdef __cplusplus
extern "C" {
#endif
// pre-rewrite mjc_BoxBox: SAT axis search, three edge-edge candidate generators
// with count-dependent acceptance, midpoint positions, and an outside-box
// removal filter
int mjc_BoxBoxLegacy(const mjModel* m, mjData* d, mjPreContact* con, int g1,
int g2, mjtNum margin);
#ifdef __cplusplus
}
#endif
#endif // MUJOCO_TEST_ENGINE_BOXBOX_LEGACY_H_
test/engine/engine_collision_box_fuzz_test.cc
+661
View File
@@ -0,0 +1,661 @@
// Copyright 2026 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Randomized cross-validation of mjc_BoxBox.
//
// Each model holds two box geoms and two box-shaped mesh geoms with identical
// half-extents. For a randomized relative pose the box pair is collided with
// mjc_BoxBox and the mesh pair with mjc_Convex (GJK/EPA).
//
// Two references arbitrate correctness:
// - GJK/EPA on the mesh pair, where it is trustworthy. EPA misreports depth
// and normal on thin meshes (verified against the direction sweep below),
// so strict agreement is only enforced for well-conditioned aspect ratios.
// - A dense direction sweep over support separations. For two boxes the
// minimum-translation direction is a face normal or an edge-edge cross
// product, so the sweep converges to the true penetration depth from
// above; its resolution error at 20k directions is ~2.5e-3 of scale.
//
// Independent of any reference, invariants are enforced for every sample:
// contacts lie within half their own depth of both (margin-inflated) boxes,
// all contacts in a manifold share one normal, the manifold is no larger than a
// clipped face, and no contact is deeper than the true penetration depth.
#include <array>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <random>
#include <string>
#include <vector>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_collision_convex.h"
#include "src/engine/engine_collision_primitive.h"
#include "src/engine/engine_util_misc.h"
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::NotNull;
using MjCollisionBoxFuzzTest = MujocoTest;
// maximum contacts box-box collider may emit: a face manifold is the clipped
// incident face, a 4-gon against 4 half-planes, so at most 8 vertices
constexpr int kMaxContacts = 8;
// direction count for the brute-force sweep; resolution scales as 1/sqrt(n)
constexpr int kSweepDirections = 20000;
struct SizeCase {
const char* name;
mjtNum size1[3];
mjtNum size2[3];
// strict GJK agreement is enforced when true; EPA under-reports deep
// penetration by up to ~1% of depth, which the truth-arbitrated gates
// absorb, so all cases currently enable it
bool gjk_reliable;
};
constexpr SizeCase kSizeCases[] = {
{"cube_cube", {0.05, 0.05, 0.05}, {0.05, 0.05, 0.05}, true},
{"cube_small", {0.05, 0.05, 0.05}, {0.013, 0.013, 0.013}, true},
{"slab_slab", {0.08, 0.08, 0.004}, {0.06, 0.06, 0.006}, true},
{"needle_cube", {0.002, 0.002, 0.09}, {0.04, 0.04, 0.04}, true},
{"slab_needle", {0.07, 0.05, 0.003}, {0.0015, 0.0015, 0.06}, true},
{"aniso", {0.018, 0.038, 0.047}, {0.026, 0.0014, 0.008}, true},
};
// builds a model with box geoms 0,1 and identical box-mesh geoms 2,3; every
// geom hangs from a freejoint body so poses are applied through qpos and
// kinematics -- writing geom_xmat directly would discard the compiled mesh's
// principal-axis frame, silently permuting a thin mesh's axes
std::string MakeXml(const SizeCase& c) {
char buf[3072];
std::snprintf(buf, sizeof(buf), R"(
<mujoco>
<asset>
<mesh name="m1" scale="%g %g %g"
vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"/>
<mesh name="m2" scale="%g %g %g"
vertex="-1 -1 -1 1 -1 -1 1 1 -1 1 1 1 1 -1 1 -1 1 -1 -1 1 1 -1 -1 1"/>
</asset>
<worldbody>
<body><freejoint/><geom type="box" size="%g %g %g"/></body>
<body><freejoint/><geom type="box" size="%g %g %g"/></body>
<body><freejoint/><geom type="mesh" mesh="m1"/></body>
<body><freejoint/><geom type="mesh" mesh="m2"/></body>
</worldbody>
</mujoco>
)",
c.size1[0], c.size1[1], c.size1[2], c.size2[0], c.size2[1],
c.size2[2], c.size1[0], c.size1[1], c.size1[2], c.size2[0],
c.size2[1], c.size2[2]);
return std::string(buf);
}
void RandomQuat(std::mt19937& rng, mjtNum quat[4]) {
std::normal_distribution<double> g(0.0, 1.0);
for (int i = 0; i < 4; i++) quat[i] = g(rng);
mju_normalize4(quat);
}
// places body pair (0,1) and the mirrored mesh pair (2,3) at the same poses,
// through qpos and kinematics so mesh frame compensation is honored
void SetPose(const mjModel* model, mjData* data, const mjtNum pos2[3],
const mjtNum quat1[4], const mjtNum quat2[4]) {
mjtNum* q = data->qpos;
mju_zero3(q);
mju_copy4(q + 3, quat1);
mju_copy3(q + 7, pos2);
mju_copy4(q + 10, quat2);
mju_zero3(q + 14);
mju_copy4(q + 17, quat1);
mju_copy3(q + 21, pos2);
mju_copy4(q + 24, quat2);
mj_kinematics(model, data);
}
mjtNum DeepestDist(const mjPreContact* con, int n) {
mjtNum d = con[0].dist;
for (int i = 1; i < n; i++) d = mju_min(d, con[i].dist);
return d;
}
// support separation of the two boxes along a specific direction
mjtNum DirSep(const mjModel* model, const mjData* data, const mjtNum dir[3]) {
mjtNum dpos[3];
mju_sub3(dpos, data->geom_xpos + 3, data->geom_xpos);
mjtNum r1 = 0, r2 = 0;
for (int k = 0; k < 3; k++) {
mjtNum a1 = dir[0] * data->geom_xmat[0 + k] +
dir[1] * data->geom_xmat[3 + k] +
dir[2] * data->geom_xmat[6 + k];
mjtNum a2 = dir[0] * data->geom_xmat[9 + k] +
dir[1] * data->geom_xmat[12 + k] +
dir[2] * data->geom_xmat[15 + k];
r1 += model->geom_size[k] * mju_abs(a1);
r2 += model->geom_size[3 + k] * mju_abs(a2);
}
return mju_abs(mju_dot3(dir, dpos)) - r1 - r2;
}
// true separation via dense direction sweep (spherical Fibonacci lattice);
// the sampled maximum is a lower bound on the true separation, converging as
// the direction count grows
mjtNum BruteForceSep(const mjModel* model, const mjData* data) {
mjtNum dpos[3];
mju_sub3(dpos, data->geom_xpos + 3, data->geom_xpos);
mjtNum best = -mjMAXVAL;
for (int gi = 0; gi < kSweepDirections; gi++) {
mjtNum phi = 2.399963229728653 * gi;
mjtNum ct = 1.0 - 2.0 * (gi + 0.5) / kSweepDirections;
mjtNum st = std::sqrt(mju_max(0, 1 - ct * ct));
mjtNum dir[3] = {st * std::cos(phi), st * std::sin(phi), ct};
mjtNum r1 = 0, r2 = 0;
for (int k = 0; k < 3; k++) {
mjtNum a1 = dir[0] * data->geom_xmat[0 + k] +
dir[1] * data->geom_xmat[3 + k] +
dir[2] * data->geom_xmat[6 + k];
mjtNum a2 = dir[0] * data->geom_xmat[9 + k] +
dir[1] * data->geom_xmat[12 + k] +
dir[2] * data->geom_xmat[15 + k];
r1 += model->geom_size[k] * mju_abs(a1);
r2 += model->geom_size[3 + k] * mju_abs(a2);
}
best = mju_max(best, mju_abs(mju_dot3(dir, dpos)) - r1 - r2);
}
return best;
}
// closed-form analytical SAT reference across all 15 potential separating axes
// in double precision (exact to machine precision, without discretization
// error)
mjtNum ExactSatSep(const mjModel* model, const mjData* data) {
const mjtNum* pos1 = data->geom_xpos;
const mjtNum* pos2 = data->geom_xpos + 3;
const mjtNum* mat1 = data->geom_xmat;
const mjtNum* mat2 = data->geom_xmat + 9;
const mjtNum* size1 = model->geom_size;
const mjtNum* size2 = model->geom_size + 3;
mjtNum rot[9], rotabs[9], pos21[3], pos12[3], tmp[3];
mju_sub3(tmp, pos2, pos1);
mju_mulMatTVec3(pos21, mat1, tmp);
mju_sub3(tmp, pos1, pos2);
mju_mulMatTVec3(pos12, mat2, tmp);
mju_mulMatTMat(rot, mat1, mat2, 3, 3, 3);
for (int i = 0; i < 9; i++) {
rotabs[i] = mju_abs(rot[i]);
}
mjtNum sep_max = -mjMAXVAL;
// 3 face axes of box1
for (int i = 0; i < 3; i++) {
mjtNum radius2 = rotabs[3 * i + 0] * size2[0] +
rotabs[3 * i + 1] * size2[1] +
rotabs[3 * i + 2] * size2[2];
mjtNum sep = mju_abs(pos21[i]) - size1[i] - radius2;
sep_max = mju_max(sep_max, sep);
}
// 3 face axes of box2
for (int j = 0; j < 3; j++) {
mjtNum radius1 = rotabs[0 + j] * size1[0] +
rotabs[3 + j] * size1[1] +
rotabs[6 + j] * size1[2];
mjtNum sep = mju_abs(pos12[j]) - size2[j] - radius1;
sep_max = mju_max(sep_max, sep);
}
// 9 edge-cross axes
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
int i1 = (i + 1) % 3, i2 = (i + 2) % 3;
mjtNum ax1 = -rot[3 * i2 + j];
mjtNum ax2 = rot[3 * i1 + j];
mjtNum norm2 = ax1 * ax1 + ax2 * ax2;
if (norm2 < 1e-12) {
continue;
}
mjtNum inv = 1 / mju_sqrt(norm2);
ax1 *= inv;
ax2 *= inv;
int j1 = (j + 1) % 3, j2 = (j + 2) % 3;
mjtNum a2_1 = ax1 * rot[3 * i1 + j1] + ax2 * rot[3 * i2 + j1];
mjtNum a2_2 = ax1 * rot[3 * i1 + j2] + ax2 * rot[3 * i2 + j2];
mjtNum radius1 = size1[i1] * mju_abs(ax1) + size1[i2] * mju_abs(ax2);
mjtNum radius2 = size2[j1] * mju_abs(a2_1) + size2[j2] * mju_abs(a2_2);
mjtNum sep =
mju_abs(ax1 * pos21[i1] + ax2 * pos21[i2]) - radius1 - radius2;
sep_max = mju_max(sep_max, sep);
}
}
return sep_max;
}
struct Stats {
int configs = 0;
int both_hit = 0;
int only_box = 0; // box-box hit where GJK did not
int only_gjk = 0; // GJK hit where box-box did not
int normal_bad = 0; // (gjk_reliable only) normal disagreement
int depth_bad = 0; // (gjk_reliable only) deepest-depth disagreement
int outside_bad = 0; // contact farther than |dist|/2 + slack from a box
int count_bad = 0; // more than kMaxContacts contacts
int mixed_normal = 0; // manifold contacts disagree on normal
int overdeep = 0; // contact deeper than the true penetration depth
int phantom = 0; // contact reported where truth says separated
int missed = 0; // no contact reported where truth says penetrating
mjtNum worst_normal = 0;
mjtNum worst_depth = 0;
};
Stats Sweep(const SizeCase& c, int n_configs, mjtNum margin, unsigned seed) {
Stats st;
const std::string xml = MakeXml(c);
char error[1024];
MjModelPtr model_ptr = LoadModelFromString(xml.c_str(), error, sizeof(error));
EXPECT_THAT(model_ptr.get(), NotNull()) << error;
if (!model_ptr) return st;
mjModel* model = model_ptr.get();
MjDataPtr data_ptr = MakeData(model_ptr);
mjData* data = data_ptr.get();
const mjtNum scale =
mju_max(mju_max(c.size1[0], c.size1[1]), c.size1[2]) +
mju_max(mju_max(c.size2[0], c.size2[1]), c.size2[2]);
// sweep resolution: angular spacing ~sqrt(4pi/n), times the pair radius
const mjtNum sweep_tol = 4.0 * scale / std::sqrt((double)kSweepDirections);
std::mt19937 rng(seed);
std::uniform_real_distribution<double> u(-1.0, 1.0);
mjPreContact box_con[mjMAXCONPAIR], gjk_con[mjMAXCONPAIR];
for (int it = 0; it < n_configs; it++) {
mjtNum pos2[3], quat1[4], quat2[4];
for (int i = 0; i < 3; i++) pos2[i] = 1.15 * scale * u(rng);
RandomQuat(rng, quat1);
RandomQuat(rng, quat2);
SetPose(model, data, pos2, quat1, quat2);
int nbox = mjc_BoxBox(model, data, box_con, 0, 1, margin);
int ngjk = mjc_Convex(model, data, gjk_con, 2, 3, margin);
st.configs++;
if (nbox > kMaxContacts) st.count_bad++;
// ground-truth arbitration on a sample of configs and on every presence
// disagreement
bool arbitrate = (it % 16 == 0) || (nbox > 0) != (ngjk > 0);
if (arbitrate) {
mjtNum true_sep = BruteForceSep(model, data);
// in the margin band the collider measures the gap along its separating
// axis, which under-reads the Euclidean separation (a SAT property the
// previous implementation and MJX share), so separated-pair bookkeeping
// contacts are by design; the harmful classes are claiming penetration
// where none exists and reporting depth beyond the true depth
mjtNum db = DeepestDist(box_con, nbox);
int bad_phantom = nbox > 0 && db < 0 && true_sep > sweep_tol;
int bad_deep = nbox > 0 && db < 0 &&
db < true_sep - sweep_tol - 0.05 * mju_abs(true_sep);
if (bad_phantom) st.phantom++;
if (nbox == 0 && true_sep < margin - sweep_tol) st.missed++;
if (bad_deep) st.overdeep++;
if ((bad_phantom || bad_deep) && std::getenv("MJ_FUZZ_DUMP")) {
std::printf(
"%s %s db=%.6e true_sep=%.6e nbox=%d n0=(%.4f %.4f %.4f)\n"
" pos2={%.17g, %.17g, %.17g}\n"
" quat1={%.17g, %.17g, %.17g, %.17g}\n"
" quat2={%.17g, %.17g, %.17g, %.17g}\n",
bad_phantom ? "PHANTOM" : "OVERDEEP", c.name,
db, true_sep, nbox, box_con[0].normal[0],
box_con[0].normal[1], box_con[0].normal[2], pos2[0], pos2[1],
pos2[2], quat1[0], quat1[1], quat1[2], quat1[3], quat2[0],
quat2[1], quat2[2], quat2[3]);
}
}
if (nbox > 0) {
// manifold invariants: one shared normal; each contact within half its
// own depth (plus slack) of both margin-inflated boxes
for (int i = 1; i < nbox; i++) {
// the contacts of a face manifold carry the same normal vector, whose
// self-dot is 1 only to the precision of mjtNum
if (mju_dot3(box_con[0].normal, box_con[i].normal) <
1 - MjTol(1e-9, 1e-5)) {
st.mixed_normal++;
break;
}
}
for (int i = 0; i < nbox; i++) {
mjtNum slack = 0.5 * mju_abs(box_con[i].dist) + 1e-6 * scale;
mjtNum sz1[3], sz2[3];
for (int k = 0; k < 3; k++) {
sz1[k] = model->geom_size[k] + margin + slack;
sz2[k] = model->geom_size[3 + k] + margin + slack;
}
int o1 = mju_outsideBox(box_con[i].pos, data->geom_xpos,
data->geom_xmat, sz1, 1);
int o2 = mju_outsideBox(box_con[i].pos, data->geom_xpos + 3,
data->geom_xmat + 9, sz2, 1);
if (o1 == 1 || o2 == 1) {
st.outside_bad++;
break;
}
}
}
if (nbox > 0 && ngjk > 0) {
st.both_hit++;
if (c.gjk_reliable) {
mjtNum dot = mju_dot3(box_con[0].normal, gjk_con[0].normal);
mjtNum ang = mju_abs(1 - mju_abs(dot));
st.worst_normal = mju_max(st.worst_normal, ang);
// margin-band contacts admit legitimately ambiguous normals near
// face ties, so the angular gate is looser with margin
mjtNum ntol = margin > 0 ? 2e-1 : 1e-3;
// separated margin-band pairs are exempt: their true closest-feature
// direction generally lies between the 15 SAT axes (corner-corner
// cases), so the SAT normal legitimately differs from GJK's; for
// penetration the SAT axis set contains the exact optimum
if (ang > ntol && DeepestDist(box_con, nbox) < 0) {
// arbitrate ties: a normal is wrong only if its directional
// separation is materially worse than the reference normal's --
// near-equal minima are legitimately ambiguous between methods
mjtNum sep_box = DirSep(model, data, box_con[0].normal);
mjtNum sep_gjk = DirSep(model, data, gjk_con[0].normal);
// the design prefers face manifolds within five percent of the
// optimum (stack stability), measured against its own face
// separation; allow one percent cross-measurement slop against
// the reference optimum
if (sep_box < sep_gjk - 0.06 * mju_abs(sep_gjk) - 1e-4 * scale) {
st.normal_bad++;
if (std::getenv("MJ_FUZZ_DUMP")) {
std::printf(
"NORMAL_BAD %s ang=%.3e sep_box=%.6e sep_gjk=%.6e "
"nbox=%d\n nb={%.6f %.6f %.6f} ng={%.6f %.6f %.6f}\n"
" pos2={%.17g, %.17g, %.17g}\n"
" quat1={%.17g, %.17g, %.17g, %.17g}\n"
" quat2={%.17g, %.17g, %.17g, %.17g}\n",
c.name, ang, sep_box, sep_gjk, nbox, box_con[0].normal[0],
box_con[0].normal[1], box_con[0].normal[2],
gjk_con[0].normal[0], gjk_con[0].normal[1],
gjk_con[0].normal[2], pos2[0], pos2[1], pos2[2], quat1[0],
quat1[1], quat1[2], quat1[3], quat2[0], quat2[1], quat2[2],
quat2[3]);
}
}
}
mjtNum db = DeepestDist(box_con, nbox);
mjtNum dg = DeepestDist(gjk_con, ngjk);
mjtNum ddiff = mju_abs(db - dg);
st.worst_depth = mju_max(st.worst_depth, ddiff);
// EPA witness accuracy degrades in the margin band
mjtNum dtol = (margin > 0 ? 5e-3 : 1e-4) * scale + 1e-9;
if (ddiff > dtol) {
// arbitrate against ground truth: only count if the box side
// deviates on the too-deep side of the true depth, and only for
// penetration, where the support sweep equals the true depth.
// A shallower manifold is legitimate: the deepest corner can be
// clipped away laterally, leaving surface-to-surface depths at
// the surviving contact locations. Positive distances measure
// different things per method (axis gap vs Euclidean witness gap).
mjtNum true_sep = BruteForceSep(model, data);
// the design substitutes the face manifold for an aliasing edge
// within five percent of the optimum, so depth may exceed the true
// depth by that fraction
mjtNum design = 0.05 * mju_abs(true_sep);
if (db < 0 && db < true_sep - sweep_tol - design) {
st.depth_bad++;
if (std::getenv("MJ_FUZZ_DUMP")) {
std::printf(
"DEPTH_BAD %s db=%.6e dg=%.6e true_sep=%.6e nbox=%d "
"ngjk=%d\n nb={%.6f %.6f %.6f} ng={%.6f %.6f %.6f}\n"
" pos2={%.17g, %.17g, %.17g}\n"
" quat1={%.17g, %.17g, %.17g, %.17g}\n"
" quat2={%.17g, %.17g, %.17g, %.17g}\n",
c.name, db, dg, true_sep, nbox, ngjk, box_con[0].normal[0],
box_con[0].normal[1], box_con[0].normal[2],
gjk_con[0].normal[0], gjk_con[0].normal[1],
gjk_con[0].normal[2], pos2[0], pos2[1], pos2[2], quat1[0],
quat1[1], quat1[2], quat1[3], quat2[0], quat2[1], quat2[2],
quat2[3]);
}
}
}
}
} else if (nbox > 0) {
st.only_box++;
} else if (ngjk > 0) {
st.only_gjk++;
}
}
return st;
}
void Report(const char* label, const SizeCase& c, const Stats& st) {
std::printf(
"[%s/%-11s] n=%5d both=%5d onlyBox=%4d onlyGJK=%4d | normal_bad=%4d "
"depth_bad=%4d outside=%4d count_bad=%3d mixed_n=%3d | phantom=%3d "
"missed=%3d overdeep=%3d | worst_n=%.3e worst_d=%.3e\n",
label, c.name, st.configs, st.both_hit, st.only_box, st.only_gjk,
st.normal_bad, st.depth_bad, st.outside_bad, st.count_bad,
st.mixed_normal, st.phantom, st.missed, st.overdeep, st.worst_normal,
st.worst_depth);
}
void CheckGates(const SizeCase& c, const Stats& st, mjtNum margin) {
EXPECT_GT(st.both_hit, 0) << c.name << ": no overlapping samples";
EXPECT_EQ(st.count_bad, 0) << c.name;
EXPECT_EQ(st.mixed_normal, 0) << c.name;
EXPECT_EQ(st.outside_bad, 0) << c.name;
EXPECT_EQ(st.phantom, 0) << c.name;
EXPECT_EQ(st.overdeep, 0) << c.name;
if (margin > 0) {
// corner-past-the-face margin-band contacts are not representable by a
// SAT clip collider (same limitation in the previous implementation and
// MJX); these are bookkeeping contacts at positive distance, so a miss
// only delays activation by a step. Observed rate peaks at ~0.3% on the
// most anisotropic case
EXPECT_LE(st.missed, st.configs / 250) << c.name;
} else {
// at zero margin the SAT depth theorem is exact: no misses allowed
EXPECT_EQ(st.missed, 0) << c.name;
}
if (c.gjk_reliable) {
EXPECT_EQ(st.normal_bad, 0) << c.name;
EXPECT_EQ(st.depth_bad, 0) << c.name;
}
}
// config count and seed are overridable for soak runs:
// MJ_FUZZ_CONFIGS=20000 MJ_FUZZ_SEED=7 ./engine_collision_box_fuzz_test
int NumConfigs() {
const char* env = std::getenv("MJ_FUZZ_CONFIGS");
return env ? std::stoi(env) : 4000;
}
unsigned BaseSeed() {
const char* env = std::getenv("MJ_FUZZ_SEED");
return env ? std::stoul(env) : 0;
}
TEST_F(MjCollisionBoxFuzzTest, AgreesWithReferencesZeroMargin) {
for (const SizeCase& c : kSizeCases) {
Stats st = Sweep(c, NumConfigs(), /*margin=*/0, 12345 + BaseSeed());
Report("margin=0", c, st);
CheckGates(c, st, 0);
}
}
TEST_F(MjCollisionBoxFuzzTest, AgreesWithReferencesWithMargin) {
for (const SizeCase& c : kSizeCases) {
Stats st = Sweep(c, NumConfigs(), /*margin=*/0.01, 999 + BaseSeed());
Report("margin>0", c, st);
CheckGates(c, st, 0.01);
}
}
TEST_F(MjCollisionBoxFuzzTest, CanonicalOrientationsAndPerturbations) {
// 24 rotational symmetries of the cube (octahedral group Oh)
std::vector<std::array<mjtNum, 4>> canonical_quats;
for (int ax = 0; ax < 3; ax++) {
for (int sx : {-1, 1}) {
for (int ay = 0; ay < 3; ay++) {
if (ay == ax) continue;
for (int sy : {-1, 1}) {
mjtNum mat[9] = {0};
mat[3 * 0 + ax] = sx;
mat[3 * 1 + ay] = sy;
// col 2 = col 0 x col 1
mat[3 * 2 + 0] = mat[3 * 0 + 1] * mat[3 * 1 + 2] -
mat[3 * 0 + 2] * mat[3 * 1 + 1];
mat[3 * 2 + 1] = mat[3 * 0 + 2] * mat[3 * 1 + 0] -
mat[3 * 0 + 0] * mat[3 * 1 + 2];
mat[3 * 2 + 2] = mat[3 * 0 + 0] * mat[3 * 1 + 1] -
mat[3 * 0 + 1] * mat[3 * 1 + 0];
mjtNum q[4];
mju_mat2Quat(q, mat);
canonical_quats.push_back({q[0], q[1], q[2], q[3]});
}
}
}
}
const mjtNum pert_angles[] = {0.0, 1e-15, 1e-12, 1e-9,
1e-6, 1e-3, 0.05, 0.785398};
const mjtNum pert_axes[5][3] = {
{1, 0, 0},
{0, 1, 0},
{0, 0, 1},
{0.70710678, 0.70710678, 0},
{0.57735027, 0.57735027, 0.57735027}};
// test across cube and anisotropic slab/needle size cases
for (const SizeCase& c :
{kSizeCases[0], kSizeCases[1], kSizeCases[2], kSizeCases[4]}) {
const std::string xml = MakeXml(c);
char error[1024];
MjModelPtr model_ptr =
LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(model_ptr.get(), NotNull()) << error;
mjModel* model = model_ptr.get();
MjDataPtr data_ptr = MakeData(model_ptr);
mjData* data = data_ptr.get();
for (mjtNum margin : {0.0, 0.005}) {
for (const auto& qbase : canonical_quats) {
for (mjtNum angle : pert_angles) {
for (const auto& axis : pert_axes) {
mjtNum qpert[4], quat2[4];
mju_axisAngle2Quat(qpert, axis, angle);
mju_mulQuat(quat2, qbase.data(), qpert);
mjtNum quat1[4] = {1, 0, 0, 0};
mjtNum mat2[9];
mju_quat2Mat(mat2, quat2);
mjtNum rproj[3] = {
mju_abs(mat2[0]) * c.size2[0] + mju_abs(mat2[1]) * c.size2[1] +
mju_abs(mat2[2]) * c.size2[2],
mju_abs(mat2[3]) * c.size2[0] + mju_abs(mat2[4]) * c.size2[1] +
mju_abs(mat2[5]) * c.size2[2],
mju_abs(mat2[6]) * c.size2[0] + mju_abs(mat2[7]) * c.size2[1] +
mju_abs(mat2[8]) * c.size2[2],
};
// lateral fraction offsets and depth fraction offsets
for (mjtNum xfrac : {-0.5, 0.0, 0.5, 0.99, 1.0}) {
for (mjtNum yfrac : {-0.5, 0.0, 0.5, 0.99, 1.0}) {
if (xfrac * xfrac + yfrac * yfrac > 1.01) continue;
for (mjtNum zfrac : {-0.2, -1e-4, 0.0, 1e-4, 0.1}) {
mjtNum pos2[3] = {
xfrac * (c.size1[0] + rproj[0]),
yfrac * (c.size1[1] + rproj[1]),
(c.size1[2] + rproj[2]) +
zfrac * (c.size1[2] + rproj[2])};
SetPose(model, data, pos2, quat1, quat2);
mjPreContact box_con[mjMAXCONPAIR];
int nbox = mjc_BoxBox(model, data, box_con, 0, 1, margin);
EXPECT_LE(nbox, kMaxContacts) << c.name;
// normal consistency across manifold
for (int i = 1; i < nbox; i++) {
EXPECT_GE(mju_dot3(box_con[0].normal, box_con[i].normal),
1 - MjTol(1e-9, 1e-5))
<< c.name;
}
// contacts within half depth of both boxes
mjtNum scale = c.size1[0] + c.size1[1] + c.size1[2] +
c.size2[0] + c.size2[1] + c.size2[2];
for (int i = 0; i < nbox; i++) {
mjtNum slack =
0.5 * mju_abs(box_con[i].dist) + 0.05 * scale;
mjtNum sz1[3] = {model->geom_size[0] + margin + slack,
model->geom_size[1] + margin + slack,
model->geom_size[2] + margin + slack};
mjtNum sz2[3] = {model->geom_size[3] + margin + slack,
model->geom_size[4] + margin + slack,
model->geom_size[5] + margin + slack};
int o1 = mju_outsideBox(box_con[i].pos, data->geom_xpos,
data->geom_xmat, sz1, 1);
int o2 = mju_outsideBox(box_con[i].pos,
data->geom_xpos + 3,
data->geom_xmat + 9, sz2, 1);
EXPECT_FALSE(o1 == 1 && o2 == 1)
<< c.name << " pos=(" << box_con[i].pos[0] << ", "
<< box_con[i].pos[1] << ", " << box_con[i].pos[2]
<< ")";
}
mjtNum exact_sep = ExactSatSep(model, data);
if (margin == 0) {
if (exact_sep < -1e-6 * scale) {
EXPECT_GT(nbox, 0)
<< c.name << " exact_sep=" << exact_sep;
}
}
if (nbox > 0 && exact_sep < 0) {
mjtNum db = DeepestDist(box_con, nbox);
EXPECT_GE(db, exact_sep - 0.06 * mju_abs(exact_sep) -
1e-6 * scale)
<< c.name << " db=" << db
<< " exact_sep=" << exact_sep;
}
}
}
}
}
}
}
}
}
}
} // namespace
} // namespace mujoco
test/engine/engine_collision_box_test.cc
+312 -7
View File
@@ -14,6 +14,7 @@
// Tests for engine/engine_collision_box.c.
#include <cmath>
#include <string>
#include <vector>
@@ -22,8 +23,10 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_collision_convex.h"
#include "src/engine/engine_collision_driver.h"
#include "src/engine/engine_collision_primitive.h"
#include "src/engine/engine_util_misc.h"
#include "test/engine/boxbox_legacy.h"
#include "test/fixture.h"
namespace mujoco {
@@ -351,9 +354,10 @@ TEST_F(MjCollisionBoxTest, ThinBoxNoSpuriousDeepContact) {
}
TEST_F(MjCollisionBoxTest, ThinBoxShallowPenetration) {
// thin boxes with zero margin, penetrating by ~100um: the deepest contact reaches the
// separating-axis bound up to rounding, and must not be dropped by the depth filter;
// the pose is written directly into mjData since the exact bits matter
// thin boxes with zero margin, penetrating by ~100um: the deepest contact
// reaches the separating-axis bound up to rounding, and must not be dropped
// by the depth filter; the pose is written directly into mjData since the
// exact bits matter
constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -464,9 +468,10 @@ TEST_F(MjCollisionBoxTest, ThinBoxTunneledPenetration) {
}
TEST_F(MjCollisionBoxTest, EdgeContactAtDepthBound) {
// edge-edge contacts whose depth equals the separating-axis bound up to rounding: the
// depth filter's slack must cover single-precision rounding or the whole manifold is
// rejected; the pose is written directly into mjData since the exact bits matter
// edge-edge contacts whose depth equals the separating-axis bound up to
// rounding: the depth filter's slack must cover single-precision rounding or
// the whole manifold is rejected; the pose is written directly into mjData
// since the exact bits matter
constexpr char xml[] = R"(
<mujoco>
<worldbody>
@@ -518,7 +523,307 @@ TEST_F(MjCollisionBoxTest, EdgeContactAtDepthBound) {
for (int i = 1; i < num; i++) {
deepest = mju_min(deepest, precon[i].dist);
}
EXPECT_THAT(deepest, MjNear(gap, 1e-8, 1e-6));
// the collider prefers a face manifold when an edge axis is within five
// percent of it (resting-stack stability), so the deepest contact may
// legitimately deviate from the exact minimum by that fraction; the bug
// this test pins was three orders of magnitude
EXPECT_NEAR(deepest, gap, 0.06 * mju_abs(gap) + MjTol(1e-8, 1e-6));
}
}
// ------------------ differential tests against the pre-rewrite collider ------
//
// These measure the two properties the rewrite was written for, against the
// implementation it replaced (boxbox_legacy.h). Both are consequences of the
// same defect: in the nearly face-aligned regime the old collider let a
// cross-product axis built from cancellation noise win the separating-axis
// search, so the manifold it emitted changed shape from one step to the next,
// and the solver's warm start never converged.
// resting stacks live at relative angles of microradians; the manifold must not
// depend on where in that regime the pair happens to sit
TEST_F(MjCollisionBoxTest, NearAlignedManifoldIsExact) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom type="box" size=".05 .05 .05"/>
<geom type="box" size=".05 .05 .05"/>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
// one box resting on the other, overlapping by 10 um, tilted about a generic
// axis by angles spanning the regime where the edge-cross axes degenerate
// into noise
mjtNum axis[3] = {1, 0.5, 3};
mju_normalize3(axis);
struct Result {
int smallest =
mjMAXCONPAIR; // smallest manifold seen while the faces still overlap
mjtNum worst_tilt =
0; // largest deviation of a contact normal from the face normal
};
Result New, Legacy;
const mjtNum identity[4] = {1, 0, 0, 0};
for (int decade = -9; decade <= -2; decade++) {
mjtNum angle = mju_pow(10, decade);
// in this range the mutual rotation is resolvable in both precisions, and
// still small enough that the faces overlap almost completely: the clipped
// polygon is an octagon
bool octagon = decade >= -6 && decade <= -4;
mjtNum quat[4];
mju_axisAngle2Quat(quat, axis, angle);
mju_zero3(data->geom_xpos);
mju_quat2Mat(data->geom_xmat, identity);
mju_quat2Mat(data->geom_xmat + 9, quat);
data->geom_xpos[3] = 0;
data->geom_xpos[4] = 0;
data->geom_xpos[5] = 0.1 - 1e-5;
mjPreContact con[mjMAXCONPAIR];
for (int legacy = 0; legacy < 2; legacy++) {
int n = legacy ? mjc_BoxBoxLegacy(model.get(), data.get(), con, 0, 1, 0)
: mjc_BoxBox(model.get(), data.get(), con, 0, 1, 0);
Result& r = legacy ? Legacy : New;
ASSERT_GT(n, 0) << (legacy ? "legacy" : "new") << " angle=" << angle;
if (octagon) r.smallest = mjMIN(r.smallest, n);
for (int i = 0; i < n; i++) {
// the true contact normal here is the shared face normal, +/- z
r.worst_tilt = mju_max(r.worst_tilt, 1 - mju_abs(con[i].normal[2]));
}
}
}
// the contact patch is the whole clipped incident face -- here an octagon,
// since the two squares are mutually rotated. Reducing it to a four-point
// subset is what an earlier draft of this collider did, and it costs two to
// three orders of magnitude of residual motion on stacks of plates, so the
// full polygon is pinned here.
EXPECT_EQ(New.smallest, 8);
// the contact normal is the face normal, exactly, at every angle in the
// regime where the edge-cross axes are rounding noise; the collider this
// replaced drifts off it
EXPECT_LE(New.worst_tilt, MjTol(1e-15, 1e-6));
EXPECT_LT(New.worst_tilt, Legacy.worst_tilt);
}
// a pair whose overlap is comparable to the rounding error of its own support
// evaluation: the separating-axis test must err toward contact, or the boxes
// pass through each other
TEST_F(MjCollisionBoxTest, ShallowOverlapSurvivesRounding) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom type="box" size="0.1 0.1 0.1"/>
<geom type="box" size="0.1 0.1 0.1"/>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_kinematics(model.get(), data.get());
// found by randomized search against an exact separating-axis reference
// evaluated in double: these two boxes overlap by 7.1e-8 of their scale,
// which the collider reported as separated under mjUSESINGLE while the exact
// comparison had no rounding slack. The pose is written straight into mjData
// because the exact bits matter.
const mjtNum size1[3] = {0.076610468327999115, 0.21989625692367554,
0.0005179486470296979};
const mjtNum size2[3] = {0.02934698574244976, 0.00031350101926364005,
0.1565844863653183};
const mjtNum pos2[3] = {0.1099575087428093, 0.067433357238769531,
-0.27263233065605164};
const mjtNum quat1[4] = {-0.59937000274658203, 0.082370907068252563,
-0.38643673062324524, 0.6961590051651001};
const mjtNum quat2[4] = {0.051869582384824753, -0.75536203384399414,
0.60438132286071777, 0.24791309237480164};
mju_copy3(model->geom_size, size1);
mju_copy3(model->geom_size + 3, size2);
mju_zero3(data->geom_xpos);
mju_copy3(data->geom_xpos + 3, pos2);
mju_quat2Mat(data->geom_xmat, quat1);
mju_quat2Mat(data->geom_xmat + 9, quat2);
mjPreContact precon[mjMAXCONPAIR];
EXPECT_GT(mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 0), 0);
}
// the dynamic consequence: a tall aligned stack. The old collider loses it.
TEST_F(MjCollisionBoxTest, AlignedTowerStands) {
static constexpr int kNumBoxes = 20;
std::string xml = R"(
<mujoco>
<option timestep=".002"/>
<worldbody>
<geom type="plane" size="5 5 .1"/>
)";
// 1 mm gaps: the stack settles onto itself, and it is the settling impacts
// that expose a manifold which changes shape from step to step
for (int i = 0; i < kNumBoxes; i++) {
xml += "<body pos='0 0 " +
std::to_string(0.05 + 0.1 * i + 0.001 * (i + 1)) +
"'><freejoint/><geom type='box' size='.05 .05 .05'/></body>\n";
}
xml += "</worldbody></mujoco>";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
// settled speed and number of boxes that lost most of their height, per
// collider
mjtNum speed[2];
int fallen[2];
for (int legacy = 0; legacy < 2; legacy++) {
mjCOLLISIONFUNC[mjGEOM_BOX][mjGEOM_BOX] =
legacy ? mjc_BoxBoxLegacy : mjc_BoxBox;
MjDataPtr data = MakeData(model);
speed[legacy] = 0;
fallen[legacy] = 0;
for (int step = 0; step < 1500; step++) { // 3 seconds
mj_step(model.get(), data.get());
if (step > 1000) { // measure once the stack has settled
for (int i = 0; i < model->nv; i++) {
speed[legacy] = mju_max(speed[legacy], mju_abs(data->qvel[i]));
}
}
}
for (int b = 1; b < model->nbody; b++) {
if (data->xipos[3 * b + 2] < 0.5 * model->body_pos[3 * b + 2])
fallen[legacy]++;
}
}
mjCOLLISIONFUNC[mjGEOM_BOX][mjGEOM_BOX] = mjc_BoxBox;
// the rewrite brings the tower to rest, every box still stacked
EXPECT_EQ(fallen[0], 0);
EXPECT_LT(speed[0], MjTol(1e-6, 1e-2));
// the collider it replaced leaves it permanently agitated: the residual
// motion is orders of magnitude larger, and given a few more seconds the
// tower falls over
EXPECT_LT(speed[0], 1e-3 * speed[1]);
}
TEST_F(MjCollisionBoxTest, ConcentricAndContainedBoxes) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom type="box" size="1 1 1"/>
<geom type="box" size="0.2 0.2 0.2"/>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_kinematics(model.get(), data.get());
mjPreContact precon[mjMAXCONPAIR];
// 1. Concentric identical boxes: size [1, 1, 1] for both
mju_copy3(model->geom_size + 3, model->geom_size);
mju_zero3(data->geom_xpos);
mju_zero3(data->geom_xpos + 3);
int num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 0);
EXPECT_GT(num, 0);
EXPECT_LE(num, 8);
for (int i = 0; i < num; i++) {
EXPECT_NEAR(precon[i].dist, -2.0, MjTol(1e-8, 1e-6));
}
// 2. Smaller box contained inside larger box, moved through each face
const mjtNum small_size[3] = {0.2, 0.2, 0.2};
mju_copy3(model->geom_size + 3, small_size);
for (int axis = 0; axis < 3; axis++) {
for (mjtNum dir : {-1.0, 1.0}) {
for (mjtNum offset : {0.5, 0.79, 0.8, 0.81, 1.2, 1.3}) {
mju_zero3(data->geom_xpos + 3);
data->geom_xpos[3 + axis] = dir * offset;
num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 0.05);
// penetration/gap along that axis
mjtNum expected_dist = offset - 1.0 - 0.2;
if (expected_dist > 0.05) {
EXPECT_EQ(num, 0);
} else {
EXPECT_GT(num, 0);
for (int i = 0; i < num; i++) {
EXPECT_NEAR(precon[i].dist, expected_dist, MjTol(1e-7, 1e-5));
EXPECT_NEAR(precon[i].normal[axis], dir, MjTol(1e-7, 1e-5));
}
}
}
}
}
}
TEST_F(MjCollisionBoxTest, CanonicalFaceAndEdgeAlignments) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<geom type="box" size="0.1 0.1 0.1"/>
<geom type="box" size="0.1 0.1 0.1"/>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
mj_kinematics(model.get(), data.get());
mjPreContact precon[mjMAXCONPAIR];
// 1. Exact 45-degree edge resting on horizontal face
mjtNum quat_45[4];
mjtNum y_axis[3] = {0, 1, 0};
mju_axisAngle2Quat(quat_45, y_axis, mjPI / 4.0);
mju_zero3(data->geom_xpos);
// corner/edge of tilted box touches top face of lower box
// lower top face is at z = 0.1; lowest edge of upper box is at -sqrt(2)*0.1
mjtNum diag = 0.1 * std::sqrt(2.0);
data->geom_xpos[3] = 0;
data->geom_xpos[4] = 0;
data->geom_xpos[5] = 0.1 + diag - 0.005; // 5mm penetration
mju_quat2Mat(data->geom_xmat, quat_45); // identity for 0, quat_45 for 1
mju_zero(data->geom_xmat, 9);
data->geom_xmat[0] = data->geom_xmat[4] = data->geom_xmat[8] = 1.0;
mju_quat2Mat(data->geom_xmat + 9, quat_45);
int num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 0);
// edge-on-face contact generates contacts along the supporting edge
EXPECT_GT(num, 0);
for (int i = 0; i < num; i++) {
EXPECT_NEAR(precon[i].dist, -0.005, MjTol(1e-6, 1e-4));
EXPECT_NEAR(precon[i].normal[2], 1.0, MjTol(1e-6, 1e-4));
}
// 2. Perpendicular edges crossing (90 degrees around z)
mjtNum quat_90[4];
mjtNum z_axis[3] = {0, 0, 1};
mju_axisAngle2Quat(quat_90, z_axis, mjPI / 2.0);
mju_quat2Mat(data->geom_xmat + 9, quat_90);
data->geom_xpos[5] = 0.2 - 0.002; // 2mm penetration, centered
num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, 0);
EXPECT_GE(num, 4); // overlapping polygons produce a 4 to 8 vertex patch
EXPECT_LE(num, 8);
for (int i = 0; i < num; i++) {
EXPECT_NEAR(precon[i].dist, -0.002, MjTol(1e-7, 1e-5));
EXPECT_NEAR(precon[i].normal[2], 1.0, MjTol(1e-7, 1e-5));
}
}
test/engine/testdata/sensor/contact_net.xml
Vendored
+1 -1
View File
@@ -9,7 +9,7 @@
<light pos="0 0 3"/>
<geom name="floor" type="plane" size=".5 1 .01"/>
<body name="b1" pos="0 0 .4" euler="5 5 5">
<body name="b1" pos="0 0 .4" euler="5 4 3">
<freejoint/>
<geom type="box" pos="0 0 .06" size=".15 .15 .03" rgba=".8 .2 .1 .2"/>
<geom type="box" pos="-.05 0 .005" size=".04 .04 .025" rgba=".8 .2 .1 1" euler="1 1 1"/>