From fb07a9ca508e4059abdd14011df6e1254da10451 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 2 Aug 2026 01:34:24 -0700 Subject: [PATCH] Fix missing contacts for deeply penetrating boxes. When penetration exceeds a box's smallest half-size, the midpoint between the contact surfaces can land outside both boxes. The outside- box filter in mjc_BoxBox then removed every contact of the manifold, returning nothing for a visibly overlapping pair, letting boxes fall through each other. Fixes #1800. If the filter would remove all contacts, restore the penetrating ones. This is strictly additive: configurations where any contact survives the filter are unchanged. The removed midpoints are bitwise-identical to the witness midpoints computed by mj_geomDistance for these configurations, so the positions follow the engine-wide contact position convention; re-anchoring them onto a box surface would not. Fixes #1800 PiperOrigin-RevId: 957867315 Change-Id: Ia9c858661d4badeb2a832d25455e33402936011d --- doc/changelog.rst | 2 + src/engine/engine_collision_box.c | 13 +++++ test/engine/engine_collision_box_test.cc | 65 ++++++++++++++++++++++++ 3 files changed, 80 insertions(+) diff --git a/doc/changelog.rst b/doc/changelog.rst index 763576b9..cbd6c416 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -52,6 +52,8 @@ Bug fixes - Fixed a bug in the box-box collider where near-degenerate face clipping could generate contacts with spuriously large penetration depth between nearly touching thin boxes with positive margin, causing resting stacks to explode. +- Fixed a bug in the box-box collider where penetrations deeper than a box's smallest half-size could produce no + contacts, letting boxes tunnel through thin boxes. Fixes :issue:`1800`. Version 3.11.0 (July 27, 2026) ------------------------------ diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index fc421ed4..b9f97350 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -1389,6 +1389,7 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, m 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}; @@ -1404,6 +1405,18 @@ int mjc_BoxBox(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, m // 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; + } } } diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index cf3afbc0..eb0c5518 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -398,6 +398,71 @@ TEST_F(MjCollisionBoxTest, ThinBoxShallowPenetration) { EXPECT_THAT(deepest, MjNear(gap, 1e-8, 1e-6)); } +TEST_F(MjCollisionBoxTest, ThinBoxTunneledPenetration) { + // thin boxes penetrating deeper than their smallest half-dim: the + // midpoint-convention contact position lands outside both boxes, and the + // outside-box filter must not delete the entire manifold; the pose is written + // directly into mjData since the exact bits matter; one face contact and one + // edge-edge contact + constexpr char xml[] = R"( + + + + + + + )"; + 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()); + + struct Config { + mjtNum size1[3], size2[3], margin, pos2[3], quat1[4], quat2[4]; + }; + const Config configs[2] = { + // face contact + {{0.10375412296887962, 0.0014208822825842824, 0.029838770510023913}, + {0.0047738704827163959, 0.0007140947854610601, 0.0045937794963772805}, + 1e-5, + {0.027189909560778446, 0.003935055006556199, -0.0055232476077228749}, + {-0.18064681165220281, -0.38732117424997786, 0.11417869384387985, + 0.89683457966874658}, + {-0.29829616465597159, 0.84519552206173276, 0.23968520782082428, + -0.37311516826607399}}, + // edge-edge contact + {{0.034760484829104925, 0.00071213467312592935, 0.0059314873303977222}, + {0.013576183806600434, 0.0082162263322061238, 0.0011680617239575811}, + 1e-4, + {0.0044392420645136049, -0.0041767483250730571, 0.00078140511512282684}, + {0.58664366826657666, 0.39991671184222333, 0.24085697266687636, + -0.66174296278070577}, + {-0.35168496783547554, 0.48328103093900215, 0.12063144050164629, + -0.79259395915916098}}}; + + for (const Config& config : configs) { + mju_copy3(model->geom_size, config.size1); + mju_copy3(model->geom_size + 3, config.size2); + mju_zero3(data->geom_xpos); + mju_copy3(data->geom_xpos + 3, config.pos2); + mju_quat2Mat(data->geom_xmat, config.quat1); + mju_quat2Mat(data->geom_xmat + 9, config.quat2); + + mjtNum gap = mj_geomDistance(model.get(), data.get(), 0, 1, 0.1, nullptr); + EXPECT_LT(gap, 0); + + mjPreContact precon[mjMAXCONPAIR]; + int num = mjc_BoxBox(model.get(), data.get(), precon, 0, 1, config.margin); + ASSERT_GT(num, 0); + mjtNum deepest = precon[0].dist; + for (int i = 1; i < num; i++) { + deepest = mju_min(deepest, precon[i].dist); + } + EXPECT_THAT(deepest, MjNear(gap, 1e-8, 1e-6)); + } +} + 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