diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 543d3f95..42c4068b 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -81,6 +81,11 @@ static mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj // status must have initial tetrahedrons static int gjkIntersect(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); +// create initial polytope for EPA for a 2, 3, or 4-simplex +static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); +static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); +static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); + // return a face of the expanded polytope that best approximates the pentration depth // witness points are in status->{x1, x2} static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); @@ -808,8 +813,30 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) { } } + // ---------------------------------------- EPA --------------------------------------------------- +// replace a 3-simplex with one of its faces +static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) { + status->nsimplex = 3; + copy3(status->simplex1 + 0, pt->verts1 + v1); + copy3(status->simplex1 + 3, pt->verts1 + v2); + copy3(status->simplex1 + 6, pt->verts1 + v3); + + copy3(status->simplex2 + 0, pt->verts2 + v1); + copy3(status->simplex2 + 3, pt->verts2 + v2); + copy3(status->simplex2 + 6, pt->verts2 + v3); + + copy3(status->simplex + 0, pt->verts + v1); + copy3(status->simplex + 3, pt->verts + v2); + copy3(status->simplex + 6, pt->verts + v3); + + pt->nfaces = 0; + pt->nverts = 0; +} + + + // return 1 if the origin and p3 are on the same side of the plane defined by p0, p1, p2 static int sameSide(const mjtNum p0[3], const mjtNum p1[3], const mjtNum p2[3], const mjtNum p3[3]) { @@ -861,7 +888,7 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) { // create a polytope from a 1-simplex (returns 0 on success) -static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { +static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum v1[3], v2[3]; sub3(v1, status->simplex1 + 0, status->simplex2 + 0); sub3(v2, status->simplex1 + 3, status->simplex2 + 3); @@ -905,18 +932,6 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj epaSupport(v5a, v5b, obj1, obj2, d3, mju_norm3(d3)); sub3(v5, v5a, v5b); - // check that all six faces are valid triangles (not collinear) - if (mju_abs(det3(v1, v3, v4)) < mjMINVAL || mju_abs(det3(v1, v3, v5)) < mjMINVAL || - mju_abs(det3(v1, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v3, v4)) < mjMINVAL || - mju_abs(det3(v2, v3, v5)) < mjMINVAL || mju_abs(det3(v2, v4, v5)) < mjMINVAL) { - return mjEPA_P2_INVALID_FACES; - } - - // check that origin is in the hexahedron - if (!testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) { - return mjEPA_P2_MISSING_ORIGIN; - } - // save vertices and get indices for each one int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); @@ -925,21 +940,39 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj int v5i = newVertex(pt, v5a, v5b); // build hexahedron - attachFace(pt, v1i, v3i, v4i, 1, 3, 2); - attachFace(pt, v1i, v5i, v3i, 2, 4, 0); - attachFace(pt, v1i, v4i, v5i, 0, 5, 1); - attachFace(pt, v2i, v4i, v3i, 5, 0, 4); - attachFace(pt, v2i, v3i, v5i, 3, 1, 5); - attachFace(pt, v2i, v5i, v4i, 4, 2, 3); + if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL) { + replaceSimplex3(pt, status, v1i, v3i, v4i); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL) { + replaceSimplex3(pt, status, v1i, v5i, v3i); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL) { + replaceSimplex3(pt, status, v1i, v4i, v5i); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL) { + replaceSimplex3(pt, status, v2i, v4i, v3i); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL) { + replaceSimplex3(pt, status, v2i, v3i, v5i); + return polytope3(pt, status, obj1, obj2); + } + if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL) { + replaceSimplex3(pt, status, v2i, v5i, v4i); + return polytope3(pt, status, obj1, obj2); + } + + // check that origin is in the hexahedron + if (status->dist > 10*mjMINVAL && !testTetra(v1, v3, v4, v5) && !testTetra(v2, v3, v4, v5)) { + return mjEPA_P2_MISSING_ORIGIN; + } - // if the origin is on the affine hull of any of the faces then the origin is not in the - // hexahedron or the hexahedron is degenerate for (int i = 0; i < 6; i++) { pt->map[i] = pt->faces + i; pt->faces[i].index = i; - if (pt->faces[i].dist < mjMINVAL) { - return mjEPA_P2_ORIGIN_ON_FACE; - } } pt->nmap = 6; @@ -1014,7 +1047,7 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu // create a polytope from a 2-simplex (returns 0 on success) -static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { +static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // get vertices of simplex from GJK const mjtNum *v1 = status->simplex, *v2 = status->simplex + 3, @@ -1094,27 +1127,6 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj -// replace a 3-simplex with one of its faces -static inline void replaceSimplex3(Polytope* pt, mjCCDStatus* status, int v1, int v2, int v3) { - status->nsimplex = 3; - copy3(status->simplex1 + 0, pt->verts1 + v1); - copy3(status->simplex1 + 3, pt->verts1 + v2); - copy3(status->simplex1 + 6, pt->verts1 + v3); - - copy3(status->simplex2 + 0, pt->verts2 + v1); - copy3(status->simplex2 + 3, pt->verts2 + v2); - copy3(status->simplex2 + 6, pt->verts2 + v3); - - copy3(status->simplex + 0, pt->verts + v1); - copy3(status->simplex + 3, pt->verts + v2); - copy3(status->simplex + 6, pt->verts + v3); - - pt->nfaces = 0; - pt->nverts = 0; -} - - - // create a polytope from a 3-simplex (returns 0 on success) static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int v1 = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); @@ -1140,6 +1152,10 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj return polytope3(pt, status, obj1, obj2); } + if (!testTetra(pt->verts + v1, pt->verts + v2, pt->verts + v3, pt->verts + v4)) { + return mjEPA_P4_MISSING_ORIGIN; + } + for (int i = 0; i < 4; i++) { pt->map[i] = pt->faces + i; pt->faces[i].index = i; diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 78e1e135..363eefa7 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -40,6 +40,7 @@ typedef enum { mjEPA_P3_INVALID_V5, mjEPA_P3_MISSING_ORIGIN, mjEPA_P3_ORIGIN_ON_FACE, + mjEPA_P4_MISSING_ORIGIN, } mjEPAStatus; // configuration for convex collision detection diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 234fdce2..2b5b94bf 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -877,6 +877,68 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD8) { mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBoxMultiCCD9) { + 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] = 1.0000000000000000000000000000000000000000; + xmat[1] = -0.0000000000000000000000000000000000050579; + xmat[2] = 0.0000000000000000001927715439853908006818; + xmat[3] = 0.0000000000000000000000000000000000056403; + xmat[4] = 1.0000000000000000000000000000000000000000; + xmat[5] = -0.0000000000000000030208514688407265124010; + xmat[6] = -0.0000000000000000001927715439853908006818; + xmat[7] = 0.0000000000000000030208514688407265124010; + xmat[8] = 1.0000000000000000000000000000000000000000; + + xpos[0] = -0.1071400000000000268807198722242901567370; + xpos[1] = -0.1928599999999999758948376893386011943221; + xpos[2] = 0.1749951524564917204607183975895168259740; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 1.0000000000000000000000000000000000000000; + xmat[1] = 0.0000000000000000000000000000000037070001; + xmat[2] = -0.0000000000000000649578747741268744461630; + xmat[3] = 0.0000000000000000000000000000000064174485; + xmat[4] = 1.0000000000000000000000000000000000000000; + xmat[5] = 0.0000000000000001558617582226398399563910; + xmat[6] = 0.0000000000000000649578747741268744461630; + xmat[7] = -0.0000000000000001558617582226398399563910; + xmat[8] = 1.0000000000000000000000000000000000000000; + + xpos[0] = -0.1071400000000000268807198722242901567370; + xpos[1] = -0.1928599999999999758948376893386011943221; + xpos[2] = 0.2156259187793853615566774806211469694972; + + + 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, 1000); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, SmallBoxMesh) { static constexpr char xml[] = R"(