diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 5ec7ea5d..100a76f2 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -84,6 +85,9 @@ static int newVertex(Polytope* pt, const mjtNum v1[3], const mjtNum v2[3]); // attaches a face to the polytope with the given vertex indices; returns non-zero on error static int attachFace(Polytope* pt, int v1, int v2, int v3, int adj1, int adj2, int adj3); +// returns 1 if objects are in contact, 0 otherwise; status must have initial tetrahedrons +static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2); + // returns the penetration depth of two convex objects; witness points are in status->{x1, x2} static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); @@ -107,7 +111,7 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { int get_dist = status->has_distances; // need to recover geom distances if not in contact mjtNum *simplex1 = status->simplex1; // simplex for obj1 mjtNum *simplex2 = status->simplex2; // simplex for obj2 - mjtNum simplex[12]; // simplex in Minkowski difference + mjtNum *simplex = status->simplex; // simplex in Minkowski difference int n = 0; // number of vertices in the simplex int k = 0; // current iteration int kmax = status->max_iterations; // max number of iterations @@ -145,6 +149,12 @@ static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { return mjMAXVAL; } + // tetrahedron is generated and only need contact info; fallback to gjkIntersect to + // determine contact + if (!get_dist && n == 3) { + return gjkIntersect(status, k, obj1, obj2) ? 0 : mjMAXVAL; + } + // run the distance subalgorithm to compute the barycentric coordinates // of the closest point to the origin in the simplex subdistance(lambda, simplex, n + 1); @@ -229,6 +239,93 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, +// helper function to compute the support point in the Minkowski difference (without normalization) +// TODO(kylebayes): combine support functions +void support2(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, const mjtNum dir[3]) { + mjtNum dir_neg[3]; + mju_scl3(dir_neg, dir, -1); + + // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) + obj1->support(s1, obj1, dir); + obj2->support(s2, obj2, dir_neg); +} + + + +// compute the signed distance of a face along with the normal +static inline mjtNum signedDistance(mjtNum normal[3], const mjtNum v1[3], const mjtNum v2[3], + const mjtNum v3[3]) { + mjtNum diff1[3], diff2[3]; + mju_sub3(diff1, v3, v1); + mju_sub3(diff2, v2, v1); + mju_cross(normal, diff1, diff2); + mjtNum norm = mju_norm3(normal); + if (norm > mjMINVAL && norm < mjMAXVAL) { + mjtNum invnorm = 1/norm; + normal[0] *= invnorm; + normal[1] *= invnorm; + normal[2] *= invnorm; + return mju_dot3(normal, v1); + } + return mjMAXVAL; // cannot recover normal (ignore face) +} + + + +// returns 0 if objects are in contact, mjMAXVAL otherwise; status must have initial tetrahedrons +static int gjkIntersect(mjCCDStatus* status, int start, mjCCDObj* obj1, mjCCDObj* obj2) { + mjtNum simplex1[12], simplex2[12], simplex[12]; + memcpy(simplex1, status->simplex1, sizeof(mjtNum) * 12); + memcpy(simplex2, status->simplex2, sizeof(mjtNum) * 12); + memcpy(simplex, status->simplex, sizeof(mjtNum) * 12); + int s[4] = {0, 3, 6, 9}; + + int kmax = status->max_iterations; + for (int k = start; k < kmax; k++) { + // compute the signed distance to each face in the simplex along with normals + mjtNum dist[4], normals[12]; + dist[0] = signedDistance(&normals[0], simplex + s[2], simplex + s[1], simplex + s[3]); + dist[1] = signedDistance(&normals[3], simplex + s[0], simplex + s[2], simplex + s[3]); + dist[2] = signedDistance(&normals[6], simplex + s[1], simplex + s[0], simplex + s[3]); + dist[3] = signedDistance(&normals[9], simplex + s[0], simplex + s[1], simplex + s[2]); + + // find the face with the smallest distance to the origin + int i = (dist[0] < dist[1]) ? 0 : 1; + int j = (dist[2] < dist[3]) ? 2 : 3; + int index = (dist[i] < dist[j]) ? i : j; + + // origin inside of simplex (run EPA for contact information) + if (dist[index] > 0) { + status->nsimplex = 4; + for (int n = 0; n < 4; n++) { + mju_copy3(status->simplex + 3*n, simplex + s[n]); + mju_copy3(status->simplex1 + 3*n, simplex1 + s[n]); + mju_copy3(status->simplex2 + 3*n, simplex2 + s[n]); + } + return 1; + } + + // replace worst vertex (farthest from origin) with new candidate + support2(simplex1 + s[index], simplex2 + s[index], obj1, obj2, normals + 3*index); + mju_sub3(simplex + s[index], simplex1 + s[index], simplex2 + s[index]); + + // found origin outside the Minkowski difference (return no collision) + if (mju_dot3(&normals[3*index], simplex + s[index]) < 0) { + return 0; + } + + // swap vertices in the simplex to retain orientation + i = (index + 1) & 3; + j = (index + 2) & 3; + int swap = s[i]; + s[i] = s[j]; + s[j] = swap; + } + return 0; // never found origin +} + + + // linear combination of n 3D vectors static inline void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n) { mju_zero3(res); diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index fcb11ace..ad8d272a 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -48,6 +48,7 @@ struct _mjCCDStatus { int epa_iterations; // number of iterations that EPA ran (negative if EPA did not 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;