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"(
+
+
+
+
+
+ )";
+
+ std::array 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"(