diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index c3a557a2..57e20ef1 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -1394,7 +1394,7 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
}
if (upper - lower < tolerance) {
// terminate without contact when upper < lower on first iteration
- if (k == 0 && upper < lower) {
+ if (k == 0 && upper < lower - 1e-10) {
face = NULL;
}
break;
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index abcc07dd..1e17cd37 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -1954,6 +1954,51 @@ TEST_F(MjGjkTest, BoxBox) {
EXPECT_EQ(dist, 1);
}
+TEST_F(MjGjkTest, BoxBoxLarge) {
+ static constexpr char xml[] = R"(
+
+
+
+
+
+ )";
+
+ TestModel model = LoadModel(xml);
+ TestData data = MakeData(model.get());
+ mj_forward(model.get(), data.get());
+
+ int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1");
+ int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2");
+
+ mjtNum* xmat = data->geom_xmat;
+ mjtNum* xpos = data->geom_xpos;
+
+ xpos = data->geom_xpos + 3;
+ xmat = data->geom_xmat + 9;
+
+ xpos[0] = -0.000000000043537;
+ xpos[1] = -0.000000000012973;
+ xpos[2] = 0.599245786666870;
+
+ xmat[0] = 1.000000000000000;
+ xmat[1] = -0.000000000004566;
+ xmat[2] = -0.000000000167641;
+ xmat[3] = 0.000000000004566;
+ xmat[4] = 1.000000000000000;
+ xmat[5] = -0.000000000017877;
+ xmat[6] = 0.000000000167641;
+ xmat[7] = 0.000000000017877;
+ xmat[8] = 1.000000000000000;
+
+ mjCCDStatus status;
+ std::vector dir, pos;
+ mjtNum dist;
+ int ncon = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 1000);
+
+ EXPECT_NEAR(dist, -0.000754, kTolerance);
+ EXPECT_EQ(ncon, 4);
+}
+
TEST_F(MjGjkTest, LongBox) {
static constexpr char xml[] = R"(