diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index 3c2df6c3..268de4c2 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -59,18 +59,18 @@ typedef struct {
// polytope used in the Expanding Polytope Algorithm (EPA)
typedef struct {
- Vertex* verts; // list of vertices that make up the polytope
- int nverts; // number of vertices
- 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
- Face** map; // linear map storing faces
- int nmap; // number of faces in map
- struct Horizon { // polytope boundary edges that can be seen from w
- int* indices; // indices of faces on horizon
- int* edges; // corresponding edge of each face on the horizon
- int nedges; // number of edges in horizon
- mjtNum* w; // point where horizon is created
+ Vertex* verts; // list of vertices that make up the polytope
+ int nverts; // number of vertices
+ 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
+ Face** map; // linear map storing faces
+ int nmap; // number of faces in map
+ struct Horizon { // polytope boundary edges that can be seen from w
+ int* indices; // indices of faces on horizon
+ int* edges; // corresponding edge of each face on the horizon
+ int nedges; // number of edges in horizon
+ const mjtNum* w; // point where horizon is created
} horizon;
} Polytope;
@@ -291,25 +291,32 @@ static void gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) {
// compute the support point in obj1 and obj2 for Minkowski difference
-static inline void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2,
+static inline void support(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3], const mjtNum dir_neg[3]) {
// obj1
- obj1->support(s1, obj1, dir);
+ obj1->support(v->vert1, obj1, dir);
if (obj1->margin > 0 && obj1->geom >= 0) {
mjtNum margin = 0.5 * obj1->margin;
- s1[0] += dir[0] * margin;
- s1[1] += dir[1] * margin;
- s1[2] += dir[2] * margin;
+ v->vert1[0] += dir[0] * margin;
+ v->vert1[1] += dir[1] * margin;
+ v->vert1[2] += dir[2] * margin;
}
// obj2
- obj2->support(s2, obj2, dir_neg);
+ obj2->support(v->vert2, obj2, dir_neg);
if (obj2->margin > 0 && obj2->geom >= 0) {
mjtNum margin = 0.5 * obj2->margin;
- s2[0] += dir_neg[0] * margin;
- s2[1] += dir_neg[1] * margin;
- s2[2] += dir_neg[2] * margin;
+ v->vert2[0] += dir_neg[0] * margin;
+ v->vert2[1] += dir_neg[1] * margin;
+ v->vert2[2] += dir_neg[2] * margin;
}
+
+ // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
+ sub3(v->vert, v->vert1, v->vert2);
+
+ // copy vertex indices of discrete geoms
+ v->index1 = obj1->vertindex;
+ v->index2 = obj2->vertindex;
}
@@ -326,17 +333,7 @@ static void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
scl3(dir_neg, x_k, norm);
scl3(dir, dir_neg, -1);
}
-
- // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
- support(v->vert1, v->vert2, obj1, obj2, dir, dir_neg);
- sub3(v->vert, v->vert1, v->vert2);
- // copy mesh indices
- if (obj1->vertindex >= 0) {
- v->index1 = obj1->vertindex;
- }
- if (obj2->vertindex >= 0) {
- v->index2 = obj2->vertindex;
- }
+ support(v, obj1, obj2, dir, dir_neg);
}
@@ -356,16 +353,7 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
int n = pt->nverts++;
Vertex* v = pt->verts + n;
-
- // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
- support(v->vert1, v->vert2, obj1, obj2, dir, dir_neg);
- sub3(v->vert, v->vert1, v->vert2);
- if (obj1->vertindex >= 0) {
- v->index1 = obj1->vertindex;
- }
- if (obj2->vertindex >= 0) {
- v->index2 = obj2->vertindex;
- }
+ support(v, obj1, obj2, dir, dir_neg);
return n;
}
@@ -375,15 +363,7 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
static void gjkIntersectSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
const mjtNum dir[3]) {
mjtNum dir_neg[3] = {-dir[0], -dir[1], -dir[2]};
- // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir)
- support(v->vert1, v->vert2, obj1, obj2, dir, dir_neg);
- sub3(v->vert, v->vert1, v->vert2);
- if (obj1->vertindex >= 0) {
- v->index1 = obj1->vertindex;
- }
- if (obj2->vertindex >= 0) {
- v->index2 = obj2->vertindex;
- }
+ support(v, obj1, obj2, dir, dir_neg);
}
@@ -1135,9 +1115,7 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
return mjEPA_P3_ORIGIN_ON_FACE;
}
-
- // 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
+ // populate face map
for (int i = 0; i < 6; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
@@ -1177,6 +1155,7 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
return mjEPA_P4_MISSING_ORIGIN;
}
+ // populate face map
for (int i = 0; i < 4; i++) {
pt->map[i] = pt->faces + i;
pt->faces[i].index = i;
@@ -1186,16 +1165,10 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
}
-
// make a copy of vertex in polytope and return its index
static inline int insertVertex(Polytope* pt, const Vertex* v) {
int n = pt->nverts++;
- Vertex* new_v = pt->verts + n;
- copy3(new_v->vert1, v->vert1);
- copy3(new_v->vert2, v->vert2);
- new_v->index1 = v->index1;
- new_v->index2 = v->index2;
- sub3(new_v->vert, v->vert1, v->vert2);
+ pt->verts[n] = *v;
return n;
}
@@ -1344,10 +1317,17 @@ static void epaWitness(const Polytope* pt, const Face* face, mjtNum x1[3], mjtNu
// 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) {
- mjtNum tolerance = status->tolerance, lower2, upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT;
- int k, kmax = status->max_iterations;
+ mjtNum upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT, lower2;
Face* face = NULL, *pface = NULL; // face closest to origin
+ mjtNum tolerance = status->tolerance;
+ int discrete = discreteGeoms(obj1, obj2);
+ // tolerance is not used for discrete geoms
+ if (discrete && sizeof(mjtNum) == sizeof(double)) {
+ tolerance = mjMINVAL;
+ }
+
+ int k, kmax = status->max_iterations;
for (k = 0; k < kmax; k++) {
pface = face;
@@ -1375,8 +1355,8 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
// compute support point w from the closest face's normal
mjtNum lower = mju_sqrt(lower2);
int wi = epaSupport(pt, obj1, obj2, face->v, lower);
- mjtNum* w = pt->verts[wi].vert;
- mjtNum upper_k = dot3(face->v, w) / lower; // upper bound for kth iteration
+ const Vertex* w = pt->verts + wi;
+ mjtNum upper_k = dot3(face->v, w->vert) / lower; // upper bound for kth iteration
if (upper_k < upper) {
upper = upper_k;
upper2 = upper * upper;
@@ -1385,7 +1365,20 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
break;
}
- pt->horizon.w = w;
+ // check if vertex w is a repeated support point
+ if (discrete) {
+ int i = 0, nverts = pt->nverts - 1;
+ for (; i < nverts; i++) {
+ if (w->index1 == pt->verts[i].index1 && w->index2 == pt->verts[i].index2) {
+ break;
+ }
+ }
+ if (i != nverts) {
+ break;
+ }
+ }
+
+ pt->horizon.w = w->vert;
horizon(pt, face);
// unrecoverable numerical issue; at least one face was deleted so nedges is 3 or more
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index 5eb5f6df..9237aef4 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -1220,6 +1220,70 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD13) {
mj_deleteModel(model);
}
+TEST_F(MjGjkTest, BoxBoxMultiCCD14) {
+ 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;
+ mjtNum* xmat = data->geom_xmat;
+
+ xmat[0] = 0.9999999980312528347070610834634862840176;
+ xmat[1] = 0.0000179109150612445166097282805983681442;
+ xmat[2] = -0.0000601389470252842008382576644009986921;
+ xmat[3] = -0.0000179108686851742081238159781664265324;
+ xmat[4] = 0.9999999998393023226128661917755380272865;
+ xmat[5] = 0.0000007716871733595438989517368948145570;
+ xmat[6] = 0.0000601389608372434404702858157243383630;
+ xmat[7] = -0.0000007706100310572527002924239115932981;
+ xmat[8] = 0.9999999981913554325529958077822811901569;
+
+ xpos[0] = 0.0002051257133161473724877743585182088282;
+ xpos[1] = 0.0000051793157380883478958571650152542531;
+ xpos[2] = -0.0800031938952457943869944756443146616220;
+
+ xpos = data->geom_xpos + 3;
+ xmat = data->geom_xmat + 9;
+
+ xmat[0] = 0.9999999606378873195922096783760935068130;
+ xmat[1] = -0.0000186818570733572177707156047876679850;
+ xmat[2] = -0.0002799557310143530259108346491814245383;
+ xmat[3] = 0.0000186853252997592718994551708178164517;
+ xmat[4] = 0.9999999997487241110150080203311517834663;
+ xmat[5] = 0.0000123858711158191162315369768243122905;
+ xmat[6] = 0.0002799554995529331168427344955773605761;
+ xmat[7] = -0.0000123911016921886008170612322731862776;
+ xmat[8] = 0.9999999607356884201436741932411678135395;
+
+ xpos[0] = 0.0002145111032389043976328218965576866140;
+ xpos[1] = -0.0000051338999751368759734112059978095033;
+ xpos[2] = -0.0400059009625639144802633495601185131818;
+
+ int g1 = mj_name2id(model, mjOBJ_GEOM, "geom1");
+ int g2 = mj_name2id(model, mjOBJ_GEOM, "geom2");
+
+ mjCCDStatus status;
+ std::vector dir, pos;
+ mjtNum dist;
+ int ncons = Penetration(status, dist, dir, pos, model, data, g1, g2, 0, 8);
+
+ EXPECT_EQ(ncons, 4);
+
+ mj_deleteData(data);
+ mj_deleteModel(model);
+}
+
TEST_F(MjGjkTest, SmallBoxMesh) {
static constexpr char xml[] = R"(