diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index b97d67a4..0a4c86ff 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -890,7 +890,12 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
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 2;
+ 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
@@ -914,7 +919,7 @@ static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
if (pt->faces[i].dist < mjMINVAL) {
- return 3;
+ return mjEPA_P2_ORIGIN_ON_FACE;
}
}
pt->nmap = 6;
@@ -1003,7 +1008,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
cross3(n, diff1, diff2);
mjtNum n_norm = mju_norm3(n);
if (n_norm < mjMINVAL) {
- return 4;
+ return mjEPA_P3_BAD_NORMAL;
}
// negative of triangle normal n
@@ -1016,7 +1021,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// check that v4 is not contained in the 2-simplex
if (triPointIntersect(v1, v2, v3, v4)) {
- return 5;
+ return mjEPA_P3_INVALID_V4;
}
// get 5th vertex in -n direction
@@ -1026,7 +1031,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// check that v5 is not contained in the 2-simplex
if (triPointIntersect(v1, v2, v3, v5)) {
- return 6;
+ return mjEPA_P3_INVALID_V5;
}
// if origin does not lie on simplex then we need to check that the hexahedron contains the
@@ -1036,7 +1041,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
// it but within tolerance from it. In that case the hexahedron could possibly be constructed
// that doesn't contain the origin, but nonetheless there is penetration depth.
if (status->dist > 10*mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) {
- return 7;
+ return mjEPA_P3_MISSING_ORIGIN;
}
// save vertices and get indices for each one
@@ -1061,7 +1066,7 @@ static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mj
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
if (pt->faces[i].dist < mjMINVAL) {
- return 8;
+ return mjEPA_P3_ORIGIN_ON_FACE;
}
}
pt->nmap = 6;
@@ -1444,7 +1449,8 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
obj1->center(status->x1, obj1);
obj2->center(status->x2, obj2);
status->gjk_iterations = 0;
- status->epa_iterations = -1;
+ status->epa_iterations = 0;
+ status->epa_status = mjEPA_NOCONTACT;
status->tolerance = config->tolerance;
status->max_iterations = config->max_iterations;
status->max_contacts = config->max_contacts;
@@ -1551,12 +1557,12 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
} else {
ret = polytope4(&pt, status, obj1, obj2);
}
+ status->epa_status = ret;
// simplex not on boundary (objects are penetrating)
if (!ret) {
dist = -epa(status, &pt, obj1, obj2);
} else {
- status->epa_iterations = -ret;
dist = 0;
}
mj_freeStack(d);
diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h
index f04ca77a..5c0601c6 100644
--- a/src/engine/engine_collision_gjk.h
+++ b/src/engine/engine_collision_gjk.h
@@ -25,17 +25,30 @@
extern "C" {
#endif
+// Status of an EPA run
+typedef enum {
+ mjEPA_NOCONTACT = -1,
+ mjEPA_SUCCESS = 0,
+ mjEPA_P2_INVALID_FACES,
+ mjEPA_P2_MISSING_ORIGIN,
+ mjEPA_P2_ORIGIN_ON_FACE,
+ mjEPA_P3_BAD_NORMAL,
+ mjEPA_P3_INVALID_V4,
+ mjEPA_P3_INVALID_V5,
+ mjEPA_P3_MISSING_ORIGIN,
+ mjEPA_P3_ORIGIN_ON_FACE,
+} mjEPAStatus;
+
// configuration for convex collision detection
-struct _mjCCDConfig {
+typedef struct {
int max_iterations; // the maximum number of iterations for GJK and EPA
mjtNum tolerance; // tolerance used by GJK and EPA
int max_contacts; // set to max number of contact points to recover
mjtNum dist_cutoff; // set to max geom distance to recover
-};
-typedef struct _mjCCDConfig mjCCDConfig;
+} mjCCDConfig;
// data produced from running GJK and EPA
-struct _mjCCDStatus {
+typedef struct {
// geom distance information
mjtNum dist; // distance between geoms
mjtNum x1[3 * mjMAXCONPAIR]; // witness points for geom 1
@@ -50,13 +63,13 @@ struct _mjCCDStatus {
// statistics for debugging purposes
int gjk_iterations; // number of iterations that GJK ran
- int epa_iterations; // number of iterations that EPA ran (negative if EPA did not run)
+ int epa_iterations; // number of iterations that EPA ran (zero if EPA did not run)
+ mjEPAStatus epa_status; // status of the EPA run
mjtNum simplex1[12]; // the simplex that GJK returned for obj1
mjtNum simplex2[12]; // the simplex that GJK returned for obj2
mjtNum simplex[12]; // the simplex that GJK returned for the Minkowski difference
int nsimplex; // size of simplex 1 & 2
-};
-typedef struct _mjCCDStatus mjCCDStatus;
+} mjCCDStatus;
// run general convex collision detection, returns positive for distance, negative for penetration
MJAPI mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2);
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 241575c6..b2b8f440 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -335,6 +335,33 @@ TEST_F(MjGjkTest, BoxBoxDepth2) {
mj_deleteModel(model);
}
+TEST_F(MjGjkTest, BoxBoxTouching) {
+ 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);
+
+ int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+ int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+ mjtNum dir[3], pos[3];
+ mjtNum dist = Penetration(model, data, geom1, geom2, dir, pos);
+
+ EXPECT_EQ(dist, mjMAXVAL);
+
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(