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"(