diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c
index e1ac8739..cb58530d 100644
--- a/src/engine/engine_collision_gjk.c
+++ b/src/engine/engine_collision_gjk.c
@@ -24,6 +24,9 @@
#include "engine/engine_util_blas.h"
#include "engine/engine_util_errmem.h"
+#define mjMINVAL2 (mjMINVAL * mjMINVAL)
+#define mjMAXVAL2 (mjMAXVAL * mjMAXVAL)
+
// subdistance algorithm for GJK that computes the barycentric coordinates of the point in a
// simplex closest to the origin
// implementation adapted from Montanari et al, ToG 2017
@@ -50,7 +53,7 @@ typedef struct {
int verts[3]; // indices of the three vertices of the face in the polytope
int adj[3]; // adjacent faces, one for each edge: [v1,v2], [v2,v3], [v3,v1]
mjtNum v[3]; // projection of the origin on face, can be used as face normal
- mjtNum dist; // norm of v; negative if deleted
+ mjtNum dist2; // squared norm of v; negative if deleted
int index; // index in map; -1: not in map, -2: deleted from polytope
} Face;
@@ -78,7 +81,7 @@ static int epaSupport(Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2,
// make copy of vertex in polytope and return its index
static int insertVertex(Polytope* pt, const Vertex* v);
-// attach a face to the polytope with the given vertex indices; return distance to origin
+// attach a face to the polytope with the given vertex indices; return squared distance to origin
static mjtNum attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3);
// return 1 if objects are in contact; 0 if not; -1 if inconclusive
@@ -318,7 +321,7 @@ static void gjkSupport(Vertex* v, mjCCDObj* obj1, mjCCDObj* obj2,
// mjc_support requires a normalized direction
mjtNum norm = dot3(x_k, x_k);
- if (norm > mjMINVAL*mjMINVAL) {
+ if (norm > mjMINVAL2) {
norm = 1/mju_sqrt(norm);
scl3(dir_neg, x_k, norm);
scl3(dir, dir_neg, -1);
@@ -392,10 +395,9 @@ static inline mjtNum signedDistance(mjtNum normal[3], const Vertex* v1, const Ve
sub3(diff1, v3->vert, v1->vert);
sub3(diff2, v2->vert, v1->vert);
cross3(normal, diff1, diff2);
- mjtNum norm = dot3(normal, normal);
- if (norm > mjMINVAL*mjMINVAL && norm < mjMAXVAL*mjMAXVAL) {
- norm = 1/mju_sqrt(norm);
- scl3(normal, normal, norm);
+ mjtNum norm2 = dot3(normal, normal);
+ if (norm2 > mjMINVAL2 && norm2 < mjMAXVAL2) {
+ scl3(normal, normal, 1 / mju_sqrt(norm2));
return dot3(normal, v1->vert);
}
return mjMAX_LIMIT; // cannot recover normal (ignore face)
@@ -964,27 +966,27 @@ static int polytope2(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
mjtNum* v5 = pt->verts[v5i].vert;
// build hexahedron
- if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL) {
+ if (attachFace(pt, v1i, v3i, v4i, 1, 3, 2) < mjMINVAL2) {
replaceSimplex3(pt, status, v1i, v3i, v4i);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL) {
+ if (attachFace(pt, v1i, v5i, v3i, 2, 4, 0) < mjMINVAL2) {
replaceSimplex3(pt, status, v1i, v5i, v3i);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL) {
+ if (attachFace(pt, v1i, v4i, v5i, 0, 5, 1) < mjMINVAL2) {
replaceSimplex3(pt, status, v1i, v4i, v5i);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL) {
+ if (attachFace(pt, v2i, v4i, v3i, 5, 0, 4) < mjMINVAL2) {
replaceSimplex3(pt, status, v2i, v4i, v3i);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL) {
+ if (attachFace(pt, v2i, v3i, v5i, 3, 1, 5) < mjMINVAL2) {
replaceSimplex3(pt, status, v2i, v3i, v5i);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL) {
+ if (attachFace(pt, v2i, v5i, v4i, 4, 2, 3) < mjMINVAL2) {
replaceSimplex3(pt, status, v2i, v5i, v4i);
return polytope3(pt, status, obj1, obj2);
}
@@ -1120,22 +1122,22 @@ static int polytope3(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
}
// create hexahedron for EPA
- if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINVAL) {
+ if (attachFace(pt, v4i, v1i, v2i, 1, 3, 2) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
- if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINVAL) {
+ if (attachFace(pt, v4i, v3i, v1i, 2, 4, 0) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
- if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINVAL) {
+ if (attachFace(pt, v4i, v2i, v3i, 0, 5, 1) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
- if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINVAL) {
+ if (attachFace(pt, v5i, v2i, v1i, 5, 0, 4) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
- if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINVAL) {
+ if (attachFace(pt, v5i, v1i, v3i, 3, 1, 5) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
- if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINVAL) {
+ if (attachFace(pt, v5i, v3i, v2i, 4, 2, 3) < mjMINVAL2) {
return mjEPA_P3_ORIGIN_ON_FACE;
}
@@ -1160,19 +1162,19 @@ static int polytope4(Polytope* pt, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj
int v4 = insertVertex(pt, status->simplex + 3);
// if the origin is on a face, replace the 3-simplex with a 2-simplex
- if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL) {
+ if (attachFace(pt, v1, v2, v3, 1, 3, 2) < mjMINVAL2) {
replaceSimplex3(pt, status, v1, v2, v3);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL) {
+ if (attachFace(pt, v1, v4, v2, 2, 3, 0) < mjMINVAL2) {
replaceSimplex3(pt, status, v1, v4, v2);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL) {
+ if (attachFace(pt, v1, v3, v4, 0, 3, 1) < mjMINVAL2) {
replaceSimplex3(pt, status, v1, v3, v4);
return polytope3(pt, status, obj1, obj2);
}
- if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL) {
+ if (attachFace(pt, v4, v3, v2, 2, 0, 1) < mjMINVAL2) {
replaceSimplex3(pt, status, v4, v3, v2);
return polytope3(pt, status, obj1, obj2);
}
@@ -1222,7 +1224,7 @@ static inline int maxFaces(Polytope* pt) {
-// attach a face to the polytope with the given vertex indices; return distance to origin
+// attach a face to the polytope with the given vertex indices; return squared distance to origin
static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
int adj1, int adj2, int adj3) {
Face* face = &pt->faces[pt->nfaces++];
@@ -1240,10 +1242,10 @@ static inline mjtNum attachFace(Polytope* pt, int v1, int v2, int v3,
if (ret) {
return 0;
}
- face->dist = norm3(face->v);
+ face->dist2 = dot3(face->v, face->v);
face->index = -1;
- return face->dist;
+ return face->dist2;
}
@@ -1267,10 +1269,8 @@ static inline int getEdge(Face* face, int vertex) {
// recursive call to build horizon; return 1 if face is visible from w otherwise 0
static int horizonRec(Polytope* pt, Face* face, int e) {
- mjtNum dist2 = face->dist * face->dist;
-
// v is visible from w so it is deleted and adjacent faces are checked
- if (dot3(face->v, pt->horizon.w) > dist2) {
+ if (dot3(face->v, pt->horizon.w) - face->dist2 > mjMINVAL) {
deleteFace(pt, face);
// recursively search the adjacent faces on the next two edges
@@ -1350,7 +1350,7 @@ 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, lower, upper = mjMAX_LIMIT;
+ mjtNum tolerance = status->tolerance, lower2, upper = mjMAX_LIMIT, upper2 = mjMAX_LIMIT;
int k, kmax = status->max_iterations;
Face* face = NULL, *pface = NULL; // face closest to origin
@@ -1358,31 +1358,35 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
pface = face;
// find the face closest to the origin (lower bound for penetration depth)
- lower = mjMAX_LIMIT;
+ lower2 = mjMAX_LIMIT;
for (int i = 0; i < pt->nmap; i++) {
- if (pt->map[i]->dist < lower) {
+ if (pt->map[i]->dist2 < lower2) {
face = pt->map[i];
- lower = face->dist;
+ lower2 = face->dist2;
}
}
// face not valid, return previous face
- if (lower > upper || !face) {
+ if (lower2 > upper2 || !face) {
face = pface;
break;
}
// check if lower bound is 0
- if (lower <= 0) {
+ if (lower2 <= 0) {
mju_warning("EPA: origin lies on affine hull of face");
break;
}
// 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
- if (upper_k < upper) upper = upper_k;
+ if (upper_k < upper) {
+ upper = upper_k;
+ upper2 = upper * upper;
+ }
if (upper - lower < tolerance) {
break;
}
@@ -1411,16 +1415,16 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
int v1 = horFace->verts[horEdge],
v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = nfaces;
- mjtNum dist = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1);
+ mjtNum dist2 = attachFace(pt, wi, v2, v1, nfaces + nedges - 1, horIndex, nfaces + 1);
// unrecoverable numerical issue
- if (dist == 0) {
+ if (dist2 == 0) {
face = NULL;
break;
}
// store face in map
- if (dist >= lower && dist <= upper) {
+ if (dist2 >= lower2 && dist2 <= upper2) {
int i = pt->nmap++;
pt->map[i] = &pt->faces[pt->nfaces - 1];
pt->map[i]->index = i;
@@ -1436,16 +1440,16 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
v1 = horFace->verts[horEdge];
v2 = horFace->verts[(horEdge + 1) % 3];
horFace->adj[horEdge] = cur;
- dist = attachFace(pt, wi, v2, v1, cur - 1, horIndex, next);
+ dist2 = attachFace(pt, wi, v2, v1, cur - 1, horIndex, next);
// unrecoverable numerical issue
- if (dist == 0) {
+ if (dist2 == 0) {
face = NULL;
break;
}
// store face in map
- if (dist >= lower && dist <= upper) {
+ if (dist2 >= lower2 && dist2 <= upper2) {
int idx = pt->nmap++;
pt->map[idx] = &pt->faces[pt->nfaces - 1];
pt->map[idx]->index = idx;
@@ -1463,7 +1467,7 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob
if (face) {
epaWitness(pt, face, status->x1, status->x2);
status->nx = 1;
- status->dist = -face->dist;
+ status->dist = -mju_sqrt(face->dist2);
} else {
status->nx = 0;
status->dist = 0;
@@ -1565,7 +1569,7 @@ static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3],
// find what side of a plane a point p lies
static int halfspace(const mjtNum a[3], const mjtNum n[3], const mjtNum p[3]) {
mjtNum diff[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]};
- return dot3(diff, n) >= 0.0;
+ return dot3(diff, n) > -mjMINVAL;
}
@@ -1855,13 +1859,37 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj
+// try recovering box normal from collision normal
+static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const mjtNum n[3]) {
+ // list of box face normals
+ mjtNum normals[18] = {1, 0, 0, -1, 0, 0,
+ 0, 1, 0, 0, -1, 0,
+ 0, 0, 1, 0, 0, -1};
+
+ // get local coordinates of the normal
+ mjtNum local_n[3];
+ local_n[0] = mat[0]*n[0] + mat[3]*n[1] + mat[6]*n[2];
+ local_n[1] = mat[1]*n[0] + mat[4]*n[1] + mat[7]*n[2];
+ local_n[2] = mat[2]*n[0] + mat[5]*n[1] + mat[8]*n[2];
+ scl3(local_n, local_n, 1/mju_sqrt(dot3(local_n, local_n)));
+
+ // determine if there is a side close to the normal
+ for (int i = 0; i < 6; i++) {
+ if (dot3(local_n, normals + 3*i) > mjFACE_TOL) {
+ globalcoord(res, mat, NULL, normals[3*i], normals[3*i + 1], normals[3*i + 2]);
+ resind[0] = i;
+ return 1;
+ }
+ }
+ return 0;
+}
+
+
+
// compute possible face normals of a box given up to 3 vertices
static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
- int v1, int v2, int v3) {
- // box data
- int g = 3*obj->geom;
- const mjtNum* mat = obj->data->geom_xmat + 3*g;
-
+ int v1, int v2, int v3, const mjtNum dir[3]) {
+ const mjtNum* mat = obj->data->geom_xmat + 9*obj->geom;
if (dim == 3) {
int c = 0;
int x = ((v1 & 1) && (v2 & 1) && (v3 & 1)) - (!(v1 & 1) && !(v2 & 1) && !(v3 & 1));
@@ -1873,7 +1901,7 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
if (y) resind[c++] = 2;
if (z) resind[c++] = 4;
if (sgn == -1) resind[0]++;
- return c == 1 ? 1 : 0; // return 1 only if vertices make a valid face
+ return c == 1 ? 1 : boxNormals2(res, resind, mat, dir);
}
if (dim == 2) {
@@ -1893,8 +1921,7 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
globalcoord(res + 3, mat, NULL, 0, 0, z);
resind[c++] = (z > 0) ? 4 : 5;
}
- // TODO(kylebayes): Should be able to recover multiple contacts here.
- return c == 2 ? 2 : 0;
+ return c == 2 ? 2 : boxNormals2(res, resind, mat, dir);
}
if (dim == 1) {
@@ -2107,14 +2134,18 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
mjtNum n1[3 * mjMAX_POLYVERT], n2[3 * mjMAX_POLYVERT]; // normals of possible face collisions
int idx1[mjMAX_POLYVERT], idx2[mjMAX_POLYVERT]; // indices of faces
+ mjtNum dir[3], dir_neg[3];
+ sub3(dir, status->x2, status->x1);
+ sub3(dir_neg, status->x1, status->x2);
+
// get all possible face normals for each geom
if (obj1->geom_type == mjGEOM_BOX) {
- nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
+ nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i, dir_neg);
} else if (obj1->geom_type == mjGEOM_MESH) {
nnorms1 = meshNormals(n1, idx1, nface1, obj1, v11i, v12i, v13i);
}
if (obj2->geom_type == mjGEOM_BOX) {
- nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
+ nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i, dir);
} else if (obj2->geom_type == mjGEOM_MESH) {
nnorms2 = meshNormals(n2, idx2, nface2, obj2, v21i, v22i, v23i);
}
@@ -2181,25 +2212,24 @@ static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
// TODO(kylebayes): this approximates the contact direction, by scaling the face normal by the
// single contact direction's magnitude. This is effective, but polygonClip should compute
// this for each contact point.
- mjtNum diff[3], approx_dir[3];
- sub3(diff, status->x2, status->x1);
+ mjtNum approx_dir[3];
// face1 is an edge; clip face1 against face2
if (edgecon1) {
- scl3(approx_dir, n2 + 3*j, norm3(diff));
+ scl3(approx_dir, n2 + 3*j, norm3(dir));
polygonClip(status, face2, nface2, face1, nface1, n2 + 3*j, approx_dir);
return;
}
// face2 is an edge; clip face2 against face1
if (edgecon2) {
- scl3(approx_dir, n1 + 3*j, -norm3(diff));
+ scl3(approx_dir, n1 + 3*j, -norm3(dir));
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*j, approx_dir);
return;
}
// face-face collision
- scl3(approx_dir, n2 + 3*j, norm3(diff));
+ scl3(approx_dir, n2 + 3*j, norm3(dir));
polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir);
}
diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc
index f4fcb141..934a034e 100644
--- a/test/engine/engine_collision_gjk_test.cc
+++ b/test/engine/engine_collision_gjk_test.cc
@@ -1088,6 +1088,70 @@ TEST_F(MjGjkTest, BoxBoxMultiCCD11) {
mj_deleteModel(model);
}
+TEST_F(MjGjkTest, BoxBoxMultiCCD12) {
+ 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] = 1.0000000000000000000000000000000000000000;
+ xmat[1] = 0.0000000000000000000000000000000000000000;
+ xmat[2] = 0.0000000000000000000000000000000000000000;
+ xmat[3] = 0.0000000000000000000000000000000000000000;
+ xmat[4] = 1.0000000000000000000000000000000000000000;
+ xmat[5] = -0.0000000000000000032154383478277941584027;
+ xmat[6] = 0.0000000000000000000000000000000000000000;
+ xmat[7] = 0.0000000000000000032154383478277941584027;
+ xmat[8] = 1.0000000000000000000000000000000000000000;
+
+ xpos[0] = 0.0164299999999999862820843077315657865256;
+ xpos[1] = -0.0764300000000000256950016819246229715645;
+ xpos[2] = 0.1252706891962387103500731200256268493831;
+
+ xpos = data->geom_xpos + 3;
+ xmat = data->geom_xmat + 9;
+
+ xmat[0] = 1.0000000000000000000000000000000000000000;
+ xmat[1] = 0.0000000000000000000000000000000000000000;
+ xmat[2] = 0.0000000000000000000000000000000000000000;
+ xmat[3] = 0.0000000000000000000000000000000000000000;
+ xmat[4] = 1.0000000000000000000000000000000000000000;
+ xmat[5] = -0.0000000000000000018997602302052549055743;
+ xmat[6] = 0.0000000000000000000000000000000000000000;
+ xmat[7] = 0.0000000000000000018997602302052549055743;
+ xmat[8] = 1.0000000000000000000000000000000000000000;
+
+ xpos[0] = 0.0164299999999999862820843077315657865256;
+ xpos[1] = -0.0764300000000000256950016819246229715645;
+ xpos[2] = 0.1748374248948718623353215662064030766487;
+
+ 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"(