From 4035c66428817c9b1e857cc77482127c5ad6a98d Mon Sep 17 00:00:00 2001
From: Kyle Bayes <kylebayes@google.com>
Date: Wed, 27 May 2026 11:31:53 -0700
Subject: [PATCH] Fix multiccd edge bug that occurs in certain situations.

PiperOrigin-RevId: 922251109
Change-Id: I74c08ae49d5dfd2a41b2dcd28eb9f2b5ba113216
---
 src/engine/engine_collision_gjk.c        | 10 ++++++-
 test/engine/engine_collision_gjk_test.cc | 38 ++++++++++++++++++++++++
 2 files changed, 47 insertions(+), 1 deletion(-)

diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index 42227694..8ad8d782 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -2169,8 +2169,16 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
 
   // face1 is an edge; clip face1 against face2
   if (edgecon1) {
-    scl3(approx_dir, n2 + 3*j, norm3(dir));
+    scl3(approx_dir, n2 + 3*j, -norm3(dir));
     polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir);
+    // x1 and x2 must be flipped as we flipped the faces in polygonClip
+    int nx = status->nx;
+    for (int k = 0; k < nx; k++) {
+      mjtNum tmp[3];
+      copy3(tmp, status->x1 + 3*k);
+      copy3(status->x1 + 3*k, status->x2 + 3*k);
+      copy3(status->x2 + 3*k, tmp);
+    }
     return;
   }
 
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 21fec2cf..f46d0c0f 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -2028,5 +2028,43 @@ TEST_F(MjGjkTest, CylinderBoxMargin) {
   mj_deleteModel(model);
 }
 
+TEST_F(MjGjkTest, BoxEdgeFlipped) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom name="geom1" pos="1.10164554 -0.11389316 0.74"
+            quat="-0.348312918 0 0 0.937378318" type="box" size="0.65 0.48 0.04"/>
+      <geom name="geom2" type="box" size="0.1 1.2 1.4" pos="1.4 0 1.425"/>
+    </worldbody>
+  </mujoco>)";
+
+  char error[1024];
+  mjModel* model = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error;
+
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+
+  int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+  int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+
+  mjCCDStatus status;
+  std::vector<mjtNum> dir, pos;
+  mjtNum dist;
+  int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
+
+  EXPECT_EQ(ncons, 2);
+
+  EXPECT_NEAR(status.x1[0], 1.907368, kTolerance);
+  EXPECT_NEAR(status.x1[1], -0.052973, kTolerance);
+  EXPECT_NEAR(status.x1[2], 0.700000, kTolerance);
+  EXPECT_NEAR(status.x2[0], 1.30000, kTolerance);
+  EXPECT_NEAR(status.x2[1], -0.052973, kTolerance);
+  EXPECT_NEAR(status.x2[2], 0.700000, kTolerance);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 }  // namespace
 }  // namespace mujoco