From 883836848a67e06752cde3a4f097c71c79bf4beb Mon Sep 17 00:00:00 2001
From: Taylor Howell <taylorhowell@google.com>
Date: Fri, 7 Nov 2025 07:36:05 -0800
Subject: [PATCH] Fix box-box distance computation.

PiperOrigin-RevId: 829431553
Change-Id: I9e28bc8eb7687c3c83e5ff68c65e93d8a07378a9
---
 doc/changelog.rst                        |  5 ++++
 src/engine/engine_collision_box.c        |  2 +-
 test/engine/CMakeLists.txt               |  2 +-
 test/engine/engine_collision_box_test.cc | 32 ++++++++++++++++++++++--
 test/engine/testdata/sleep/sensor.xml    |  1 -
 5 files changed, 37 insertions(+), 5 deletions(-)

diff --git a/doc/changelog.rst b/doc/changelog.rst
index cf2e9485..0a1fd882 100644
--- a/doc/changelog.rst
+++ b/doc/changelog.rst
@@ -43,6 +43,11 @@ MJX
      We will be migrating users to a Warp-based batch renderer.
 
 
+Bug fixes
+^^^^^^^^^
+
+- Fixed a bug in the box-box distance computation. Reported by :github:user:`nvtw`.
+
 Version 3.3.7 (October 13, 2025)
 -----------------------------------
 
diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c
index 1a784dd6..866f4769 100644
--- a/src/engine/engine_collision_box.c
+++ b/src/engine/engine_collision_box.c
@@ -965,7 +965,7 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2
 
   for (i = 0; i < n; i++)
   {
-    con[i].dist = points[i][2];
+    con[i].dist = 2 * points[i][2];
     points[i][2] += hz;
 
     mju_mulMatVec3(tmp2, r, points[i]);
diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt
index 71c28fb4..c81cdda2 100644
--- a/test/engine/CMakeLists.txt
+++ b/test/engine/CMakeLists.txt
@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-mujoco_test(engine_collision_box_test)
+mujoco_test(engine_collision_box_test ADDITIONAL_LINK_LIBRARIES ccd)
 
 mujoco_test(engine_collision_convex_test)
 
diff --git a/test/engine/engine_collision_box_test.cc b/test/engine/engine_collision_box_test.cc
index d255bd61..ad23f936 100644
--- a/test/engine/engine_collision_box_test.cc
+++ b/test/engine/engine_collision_box_test.cc
@@ -20,10 +20,10 @@
 #include <gtest/gtest.h>
 #include <mujoco/mjmodel.h>
 #include <mujoco/mujoco.h>
-#include "test/fixture.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 {
@@ -274,5 +274,33 @@ TEST_F(MjCollisionBoxTest, BoxSphere) {
 }
 
 
+TEST_F(MjCollisionBoxTest, BoxBoxContactDistance) {
+  constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom type="box" size="1 1 1"/>
+      <geom type="box" size="1 1 1" pos="0 0 1.5"/>
+    </worldbody>
+  </mujoco>
+  )";
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << error;
+
+  mjData* data = mj_makeData(model);
+  mj_kinematics(model, data);
+  mjContact contact[9];
+
+  for (mjfCollision collision : {mjc_BoxBox, mjc_Convex}) {
+    int n = collision(model, data, contact, 0, 1, 0.0);
+    for (int i = 0; i < n; i++) {
+      EXPECT_NEAR(contact[i].dist, -0.5, 1.0e-8);
+    }
+  }
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco
diff --git a/test/engine/testdata/sleep/sensor.xml b/test/engine/testdata/sleep/sensor.xml
index 2d3e9174..40ab01bc 100644
--- a/test/engine/testdata/sleep/sensor.xml
+++ b/test/engine/testdata/sleep/sensor.xml
@@ -29,7 +29,6 @@
   </worldbody>
 
   <sensor>
-    <contact reduce="maxforce" data="force"/>
     <contact geom1="1" geom2="2"/>
     <contact geom1="1" geom2="2" reduce="maxforce" data="force"/>
     <contact geom1="1" geom2="floor"/>