From daba00ca9d1635e297d5c611ccd6da21991df37f Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Thu, 9 Jan 2025 08:34:49 -0800
Subject: [PATCH] Fix a box-box bad collision in NativeCCD.

PiperOrigin-RevId: 713683497
Change-Id: I0c4cfd55d00faa196d8759046c87ee4ef74f01ab
---
 src/engine/engine_collision_gjk.c        |  8 +--
 test/engine/engine_collision_gjk_test.cc | 64 ++++++++++++++++++++++++
 2 files changed, 68 insertions(+), 4 deletions(-)

diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index 0a4c86ff..fedaded6 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -1104,19 +1104,19 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
   int v4 = newVertex(pt, status->simplex1 + 9, status->simplex2 + 9);
 
   // if the origin is on a face, replace the 3-simplex with a 2-simplex
-  if (attachFace(pt, v1, v2, v3, 1, 3, 2) == 0.0) {
+  if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL) {
     replaceSimplex3(pt, status, v1, v2, v3);
     return polytope3(pt, status, obj1, obj2);
   }
-  if (attachFace(pt, v1, v4, v2, 2, 3, 0) == 0.0) {
+  if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL) {
     replaceSimplex3(pt, status, v1, v4, v2);
     return polytope3(pt, status, obj1, obj2);
   }
-  if (attachFace(pt, v1, v3, v4, 0, 3, 1) == 0.0) {
+  if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL) {
     replaceSimplex3(pt, status, v1, v3, v4);
     return polytope3(pt, status, obj1, obj2);
   }
-  if (attachFace(pt, v4, v3, v2, 2, 0, 1) == 0.0) {
+  if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL) {
     replaceSimplex3(pt, status, v4, v3, v2);
     return polytope3(pt, status, obj1, obj2);
   }
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index b2b8f440..bb8daf65 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -335,6 +335,70 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, BoxBoxDepth3) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+  <worldbody>
+    <geom name="geom1" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
+    <geom name="geom2" type="box" pos="0 0 0" size="0.25 0.25 0.05"/>
+  </worldbody>
+  </mujoco>)";
+
+  std::array<char, 1000> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data();
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  mjtNum* xmat = data->geom_xmat;
+  mjtNum* xpos = data->geom_xpos;
+
+  xmat[0] = 0.965925826289068201191412299522;
+  xmat[1] = -0.258819045102520739476403832668;
+  xmat[2] = 0.000000000000000006339100926609;
+  xmat[3] = 0.258819045102520739476403832668;
+  xmat[4] = 0.965925826289068201191412299522;
+  xmat[5] = -0.000000000000000214827792362716;
+  xmat[6] = 0.000000000000000049478422780336;
+  xmat[7] = 0.000000000000000209148392896446;
+  xmat[8] = 1.000000000000000000000000000000;
+
+  xpos[0] = -0.015346499999999199323474918799;
+  xpos[1] = -0.023505500000000002086553152481;
+  xpos[2] = -4.562296442400120888294168253196;
+
+  xmat = data->geom_xmat + 9;
+  xpos = data->geom_xpos + 3;
+
+  xmat[0] = 0.866025403784438707610604524234;
+  xmat[1] = -0.499999999999999944488848768742;
+  xmat[2] = 0.000000000000000018716705841316;
+  xmat[3] = 0.499999999999999944488848768742;
+  xmat[4] = 0.866025403784438707610604524234;
+  xmat[5] = -0.000000000000000263161736875730;
+  xmat[6] = 0.000000000000000115371725704125;
+  xmat[7] = 0.000000000000000237263102359077;
+  xmat[8] = 1.000000000000000000000000000000;
+
+  xpos[0] = -0.015346499999999797803074130798;
+  xpos[1] = -0.023505499999999998617106200527;
+  xpos[2] = -4.659230360891631228525966434972;
+
+  int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+  int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+  mjtNum dir[3], pos[3];
+  mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
+
+  EXPECT_NEAR(dist, -0.003066, kTolerance);
+  EXPECT_NEAR(dir[0], 0, kTolerance);
+  EXPECT_NEAR(dir[1], 0, kTolerance);
+  EXPECT_NEAR(dir[2], -1, kTolerance);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 TEST_F(MjGjkTest, BoxBoxTouching) {
   static constexpr char xml[] = R"(
   <mujoco>