diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index fb13c666..16859bf5 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -1546,7 +1546,7 @@ static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
// find what side of a plane a point p lies
static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) {
mjtNum diff[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]};
- return dot3(diff, n) > 0;
+ return dot3(diff, n) >= 0.0;
}
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 437d314d..6db923f2 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -951,6 +951,46 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD9) {
mj_deleteModel(model);
}
+TEST_F(MjGjkTest, BoxBoxMultiCCD10) {
+ 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* xpos = data->geom_xpos;
+
+ xpos[0] = -0.1034859999999999946584949839234468527138;
+ xpos[1] = -0.0765140000000000264357424839545274153352;
+ xpos[2] = 0.1257628745456405572333835607423679903150;
+
+ xpos = data->geom_xpos + 3;
+
+ xpos[0] = -0.1034859999999999946584949839234468527138;
+ xpos[1] = -0.0765140000000000264357424839545274153352;
+ xpos[2] = 0.1751399999999999623767621415026951581240;
+
+ int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+ int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+ std::vector dir, pos;
+ mjtNum dist;
+ int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 8);
+
+ EXPECT_EQ(ncons, 4);
+
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(