diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index f04375a7..c3a557a2 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -1393,6 +1393,10 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob upper2 = upper * upper; } if (upper - lower < tolerance) { + // terminate without contact when upper < lower on first iteration + if (k == 0 && upper < lower) { + face = NULL; + } break; } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 091b9c52..abcc07dd 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -1353,6 +1353,64 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD14) { ASSERT_EQ(ncons, 4); } +TEST_F(MjGjkTest, BoxBoxMultiCCD15) { + static constexpr char xml[] = R"( + + + + + + )"; + + TestModel model = LoadModel(xml); + TestData data = MakeData(model.get()); + mj_forward(model.get(), data.get()); + + mjtNum* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 1.0; + xmat[1] = 0.0; + xmat[2] = 0.0; + xmat[3] = 0.0; + xmat[4] = 1.0; + xmat[5] = 0.0; + xmat[6] = 0.0; + xmat[7] = 0.0; + xmat[8] = 1.0; + + xpos[0] = 0.0; + xpos[1] = 0.0; + xpos[2] = 0.0; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 1.0; + xmat[1] = 1.62423755001306e-10; + xmat[2] = -1.73500047822017e-05; + xmat[3] = 1.44241105171083e-10; + xmat[4] = 1.0; + xmat[5] = 1.76752037077677e-05; + xmat[6] = 1.73500047822017e-05; + xmat[7] = -1.76752037077677e-05; + xmat[8] = 1.0; + + xpos[0] = 0.0520339831709862; + xpos[1] = -0.0520339831709862; + xpos[2] = 0.124986477196217; + + int g1 = mj_name2id(model.get(), mjOBJ_GEOM, "geom1"); + int g2 = mj_name2id(model.get(), mjOBJ_GEOM, "geom2"); + + mjCCDStatus status; + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 4); + + ASSERT_EQ(ncons, sizeof(mjtNum) == 8 ? 4 : 0); +} + TEST_F(MjGjkTest, SmallBoxMesh) { static constexpr char xml[] = R"(