diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index ba3bf561..f04375a7 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -93,6 +93,7 @@ typedef struct {
Face* faces; // list of faces that make up the polytope
int nfaces; // number of faces
int maxfaces; // max number of faces that can be stored in polytope
+ mjtNum center[3]; // center of the polytope
Face** map; // linear map storing faces
int nmap; // number of faces in map
struct Horizon { // polytope boundary edges that can be seen from w
@@ -929,6 +930,10 @@ static inline int rayTriangle(const mjtNum v1[3], const mjtNum v2[3], const mjtN
static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
mjtNum *v1 = status->simplex[0].vert, *v2 = status->simplex[1].vert;
+ // set the polytope center
+ add3(pt->center, v1, v2);
+ scl3(pt->center, pt->center, 0.5);
+
mjtNum diff[3];
sub3(diff, v2, v1);
@@ -997,13 +1002,12 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
return mjEPA_P2_NONCONVEX;
}
+ // populate face map
for (int i = 0; i < 6; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
}
pt->nmap = 6;
-
- // valid hexahedron for EPA
return 0;
}
@@ -1077,6 +1081,11 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
*v2 = status->simplex[1].vert,
*v3 = status->simplex[2].vert;
+ // set the polytope center
+ add3(pt->center, v1, v2);
+ add3(pt->center, pt->center, v3);
+ scl3(pt->center, pt->center, 1.0 / 3.0);
+
// get normals in both directions
mjtNum diff1[3], diff2[3], n[3], n_neg[3];
sub3(diff1, v2, v1);
@@ -1156,6 +1165,12 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
int v3 = insertVertex(pt, status->simplex + 2);
int v4 = insertVertex(pt, status->simplex + 3);
+ // set the polytope center
+ add3(pt->center, pt->verts[v1].vert, pt->verts[v2].vert);
+ add3(pt->center, pt->center, pt->verts[v3].vert);
+ add3(pt->center, pt->center, pt->verts[v4].vert);
+ scl3(pt->center, pt->center, 0.25);
+
// if the origin is on a face, replace the 3-simplex with a 2-simplex
if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINDIST4) {
replaceSimplex3(pt, status, v1, v2, v3);
@@ -1229,6 +1244,14 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
if (ret) {
return 0;
}
+
+ // ensure projection points outward from the polytope
+ mjtNum outward[3];
+ sub3(outward, pt->verts[v1].vert, pt->center);
+ if (dot3(face->v, outward) < 0) {
+ scl3(face->v, face->v, -1);
+ }
+
face->dist2 = dot3(face->v, face->v);
face->index = -1;
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 223c5568..091b9c52 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -470,6 +470,128 @@ TEST_F(MjGjkTest, BoxBoxSize05) {
ASSERT_EQ(ncons, 4);
}
+TEST_F(MjGjkTest, BoxBoxSize05b) {
+ 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.000000000000000;
+ xmat[1] = -0.000000008764291;
+ xmat[2] = 0.000000386995168;
+ xmat[3] = 0.000000008764733;
+ xmat[4] = 1.000000000000000;
+ xmat[5] = -0.000001144207772;
+ xmat[6] = -0.000000386995168;
+ xmat[7] = 0.000001144207772;
+ xmat[8] = 1.000000000000000;
+
+ xpos[0] = 0.000000962082822;
+ xpos[1] = -0.000001747370789;
+ xpos[2] = 3.469238519668579;
+
+ xmat = data->geom_xmat + 9;
+ xpos = data->geom_xpos + 3;
+
+ xmat[0] = 1.000000000000000;
+ xmat[1] = 0.000000003313412;
+ xmat[2] = -0.000000196321196;
+ xmat[3] = -0.000000003313673;
+ xmat[4] = 1.000000000000000;
+ xmat[5] = -0.000001329654879;
+ xmat[6] = 0.000000196321196;
+ xmat[7] = 0.000001329654879;
+ xmat[8] = 1.000000000000000;
+
+ xpos[0] = 0.000002897753802;
+ xpos[1] = -0.000004625266229;
+ xpos[2] = 4.435211658477783;
+
+ 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, 4);
+ EXPECT_NEAR(dir[0], 0, 1e-5);
+ EXPECT_NEAR(dir[1], 0, 1e-5);
+ EXPECT_NEAR(dir[2], 1, kTolerance);
+}
+
+TEST_F(MjGjkTest, BoxBoxSize05c) {
+ 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.000000000000000;
+ xmat[1] = -0.000000000570266;
+ xmat[2] = 0.000000168314983;
+ xmat[3] = 0.000000000570290;
+ xmat[4] = 1.000000000000000;
+ xmat[5] = -0.000000142656916;
+ xmat[6] = -0.000000168314983;
+ xmat[7] = 0.000000142656916;
+ xmat[8] = 1.000000000000000;
+
+ xpos[0] = 0.000000188941314;
+ xpos[1] = -0.000000195227585;
+ xpos[2] = 0.497281551361084;
+
+ xmat = data->geom_xmat + 9;
+ xpos = data->geom_xpos + 3;
+
+ xmat[0] = 1.000000000000000;
+ xmat[1] = -0.000000000058608;
+ xmat[2] = 0.000001607574859;
+ xmat[3] = 0.000000000060496;
+ xmat[4] = 1.000000000000000;
+ xmat[5] = -0.000001174638669;
+ xmat[6] = -0.000001607574859;
+ xmat[7] = 0.000001174638669;
+ xmat[8] = 1.000000000000000;
+
+ xpos[0] = 0.000000953407323;
+ xpos[1] = -0.000000923845278;
+ xpos[2] = 1.493984460830688;
+
+ 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, 4);
+ EXPECT_NEAR(dir[0], 0, 1e-5);
+ EXPECT_NEAR(dir[1], 0, 1e-5);
+ EXPECT_NEAR(dir[2], 1, kTolerance);
+}
+
TEST_F(MjGjkTest, BoxBoxTouching) {
static constexpr char xml[] = R"(