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

Include gap in body_margin, fixing mid-phase pruning of in-gap contacts.

Browse Source 
The mid-phase BVH descent filter prunes body pairs using body_margin,
which was compiled as the max over geom margins, excluding gap. Broadphase
and the leaf-level test both use margin+gap, so any multi-geom body relying
on gap could silently lose its in-gap contacts when raw AABBs don't
overlap. Single-geom bodies take the leaf-leaf path and were unaffected.

PiperOrigin-RevId: 946967548
Change-Id: I6d92baa296f1a83be68b4dbfd64a96d1c7efd3c4
This commit is contained in:
Yuval Tassa
2026-07-13 05:04:08 -07:00
committed by Copybara-Service
parent f5f9d9efb7
commit dddb2767c6
7 changed files with 45 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_collision_driver_test.cc
+38
View File
@@ -128,6 +128,44 @@ TEST_F(MjCollisionTest, ContactCount) {
EXPECT_EQ(d->ncon, 8);
}
TEST_F(MjCollisionTest, InGapContactsMultiGeomBody) {
// In-gap contacts survive mid-phase BVH pruning. Both bodies
// have a "far" geom that pulls the BVH root box away, so detecting the A-B
// pair requires the descent filter to account for gap (body_margin).
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body pos="0 0 1">
<freejoint/>
<geom name="A" type="box" size=".005 .01 .3" pos="-.015 0 0" gap=".004"/>
<geom name="far1" type="box" size=".005 .01 .3" pos="-.2 0 0"/>
</body>
<body pos="0 0 1">
<freejoint/>
<geom name="B" type="box" size=".005 .01 .3" pos="-.0022 0 0" gap=".004"/>
<geom name="far2" type="box" size=".005 .01 .3" pos=".1 0 0"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d = MakeData(m);
ASSERT_THAT(d, NotNull());
mj_forward(m.get(), d.get());
// geoms A and B are separated by 2.8mm, within the 8mm combined gap:
// inactive contacts are expected
EXPECT_GT(d->ncon, 0);
for (int i = 0; i < d->ncon; i++) {
EXPECT_GT(d->contact[i].dist, 0);
EXPECT_LT(d->contact[i].dist, 0.008);
EXPECT_EQ(d->contact[i].efc_address, -1);
}
}
TEST_F(MjCollisionTest, FilterParent) {
constexpr char xml[] = R"(
<mujoco>