From 0e7d2ef6df8eb5cab260f0dbef4e989743b7f5be Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 4 Dec 2024 01:18:26 -0800 Subject: [PATCH] Fix bug in box-sphere collider. Fixes #2206 PiperOrigin-RevId: 702627603 Change-Id: Ie6015b7fa6f8507459325d692581b0f5f83aa159 --- doc/changelog.rst | 6 +++++ src/engine/engine_collision_box.c | 14 ++++++------ test/engine/engine_collision_box_test.cc | 29 ++++++++++++++++++++++++ 3 files changed, 42 insertions(+), 7 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 91ee4660..5afaf2f0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,12 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +Bug fixes +^^^^^^^^^ +- Fixed a bug in the box-sphere collider, depth was incorrect for deep penetrations (:github:issue:`2206`). Version 3.2.6 (Dec 2, 2024) --------------------------- diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index bb42ce89..1860ad4d 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -38,10 +38,9 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n) // raw sphere : box int mjraw_SphereBox(mjContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, - const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) -{ + const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) { int i, k; - mjtNum tmp[3], center[3], clamped[3], deepest[3], nearest[3]; + mjtNum tmp[3], center[3], clamped[3], deepest[3]; mjtNum pos[3]; mjtNum dist, closest; @@ -62,19 +61,20 @@ int mjraw_SphereBox(mjContact* con, mjtNum margin, if (dist <= mjMINVAL) { closest = (size2[0] + size2[1] + size2[2]) * 2; - for (i = 0; i < 6; i++) - if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2])) - { + for (i = 0; i < 6; i++) { + if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2])) { closest = mju_abs((i % 2 ? 1 : -1) * size2[i / 2] - center[i / 2]); k = i; } + } - mju_zero3(nearest); + mjtNum nearest[3] = {0}; nearest[k / 2] = (k % 2 ? -1 : 1); mju_copy3(pos, center); mju_addToScl3(pos, nearest, (size1[0] - closest) / 2); mju_mulMatVec3(con[0].frame, mat2, nearest); + dist = -closest; } else { mju_addToScl3(deepest, tmp, size1[0]); mju_zero3(pos); diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc index 5477189d..4f224cf1 100644 --- a/test/engine/engine_collision_box_test.cc +++ b/test/engine/engine_collision_box_test.cc @@ -30,6 +30,7 @@ namespace { using MjCollisionBoxTest = MujocoTest; using ::testing::NotNull; +using ::testing::DoubleNear; static const char* const kBad0FilePath = "engine/testdata/collision_box/boxbox_bad0.xml"; @@ -243,6 +244,34 @@ TEST_F(MjCollisionBoxTest, DeepPenetration) { mj_deleteModel(model); } +TEST_F(MjCollisionBoxTest, BoxSphere) { + constexpr char xml[] = R"( + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + mjData* data = mj_makeData(model); + + for (mjtNum z : {-.015, -.00501, -.005, -.00499, 0.0, 0.004}) { + data->qpos[2] = z; + mj_forward(model, data); + EXPECT_EQ(data->ncon, 2); + EXPECT_THAT(data->contact[0].dist, DoubleNear(data->contact[1].dist, 1e-8)); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco