Fix bug in box-sphere collider. Fixes #2206

PiperOrigin-RevId: 702627603
Change-Id: Ie6015b7fa6f8507459325d692581b0f5f83aa159

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)
 ---------------------------

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++)
+    for (i = 0; i < 6; i++) {
-      if (closest > mju_abs((i % 2 ? 1 : -1)*size2[i / 2] - center[i / 2]))
+      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);

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"(
+  <mujoco>
+    <worldbody>
+      <geom name="plane" type="plane" size="0.05 0.05 0.001"/>
+      <geom name="box" type="box" pos = "0 0 -0.025" size="0.05 0.05 .025"/>
+      <body>
+        <freejoint/>
+        <geom name="sphere" type="sphere" mass="1" size="0.005"/>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+  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