From 4fc6fb179b47f2f77c2a5d4af9acfb5f7ef49e98 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Fri, 6 Sep 2024 06:55:01 -0700 Subject: [PATCH] Remove libccd compatibility layer from native ccd. PiperOrigin-RevId: 671740643 Change-Id: If3c1097df5c505f70c2dc0c090a7f5a93a43c683 --- src/engine/engine_collision_convex.c | 67 +++++--- src/engine/engine_collision_convex.h | 1 - src/engine/engine_collision_gjk.c | 203 ++++++++--------------- src/engine/engine_collision_gjk.h | 36 ++-- test/engine/engine_collision_gjk_test.cc | 153 ++++++++--------- 5 files changed, 218 insertions(+), 242 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 39535d28..7a495f59 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -32,13 +32,40 @@ // call LibCCD or GJK to recover penetration info -static int mjc_penetration(const mjModel* m, const void *obj1, const void *obj2, const ccd_t *ccd, - ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) { +static int mjc_penetration(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2, + const ccd_t* ccd, ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) { if (mjENABLED(mjENBL_NATIVECCD)) { - return mj_gjkPenetration(obj1, obj2, ccd, depth, dir, pos); - } else { - return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos); + mjCCDConfig config; + mjCCDStatus status; + + // set config + config.max_iterations = ccd->max_iterations, + config.tolerance = ccd->mpr_tolerance, + config.contacts = 1; + config.distances = 0; // no geom distances needed + + mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); + if (dist < 0) { + if (depth) *depth = -dist; + if (dir) { + mju_sub3(dir->v, status.x1, status.x2); + mju_normalize3(dir->v); + } + if (pos) { + pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]); + pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]); + pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]); + } + return 0; + } + if (depth) *depth = 0; + if (dir) mju_zero3(dir->v); + if (pos) mju_zero3(dir->v); + return 1; } + + // fallback to MPR + return ccdMPRPenetration(obj1, obj2, ccd, depth, dir, pos); } @@ -298,8 +325,8 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { -// find single convex-convex collision, using libccd -static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, +// find single convex-convex collision +static int mjc_CCDIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, const mjModel* m, const mjData* d, mjContact* con, mjtNum margin) { ccd_vec3_t dir, pos; @@ -370,10 +397,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], int mjc_Convex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { ccd_t ccd; - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -384,7 +409,7 @@ int mjc_Convex(const mjModel* m, const mjData* d, ccd.support2 = mjccd_support; // find initial contact - int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin); + int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin); // look for additional contacts if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) @@ -433,7 +458,7 @@ int mjc_Convex(const mjModel* m, const mjData* d, mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2); // search for new contact - int new_contact = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin); + int new_contact = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin); // check new contact if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) { @@ -501,7 +526,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjGETINFO mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; ccd_vec3_t dir, vec; - mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; // get support point in -normal direction ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); @@ -683,8 +708,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, mjc_support}; ccd_t ccd; // point size1 to hfield size instead of geom1 size @@ -1118,10 +1142,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { ccd_t ccd; - mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; - mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; + mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -1132,7 +1154,7 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, ccd.support2 = mjccd_support; // find contacts - int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin); + int ncon = mjc_CCDIteration(&obj1, &obj2, &ccd, m, d, con, margin); return ncon; } @@ -1170,8 +1192,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, mjc_center, mjc_support}; ccd_t ccd; //------------------------------------- AABB computation, box-box test diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 9c27623f..94cbb1ed 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -51,7 +51,6 @@ struct _mjCCDObj { int vert; mjtNum margin; mjtNum rotate[4]; - mjtNum x0[3]; // initial guess of the witness point void (*center)(mjtNum res[3], const struct _mjCCDObj* obj); void (*support)(mjtNum res[3], struct _mjCCDObj* obj, const mjtNum dir[3]); mjtNum prism[6][3]; // for hfield diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 9a3c8b2b..058027ee 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -25,9 +25,6 @@ #include "engine/engine_util_errmem.h" #include "engine/engine_util_spatial.h" -#include -#include - // Computes the shortest distance between the origin and an n-simplex (n <= 3) and returns the // barycentric coordinates of the closest point in the simplex. This is the so called distance // sub-algorithm of the original 1988 GJK algorithm. @@ -48,8 +45,8 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, // support function tweaked for GJK by taking kth iteration point as input and setting both // support points to recover witness points -static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, - const mjtNum x_k[3]); +static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum x_k[3]); // linear algebra utility functions static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]); @@ -81,11 +78,9 @@ typedef struct { // generates a polytope from a 1-simplex, 2-simplex, or 3-simplex respectively // returns true if the polytope can be generated, false otherwise -static int polytope2(Polytope* pt, const mjtNum simplex1[6], const mjtNum simplex2[6], - mjCCDObj* obj1, mjCCDObj* obj2); -static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simplex2[9], - mjCCDObj* obj1, mjCCDObj* obj2); -static int polytope4(Polytope* pt, const mjtNum simplex1[12], const mjtNum simplex2[12]); +static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); +static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); +static int polytope4(Polytope* pt, const mjCCDStatus* status); // initializes the polytope (faces and vertices must be freed by caller) static void initPolytope(Polytope* pt); @@ -98,37 +93,31 @@ static void attachFace(Polytope* pt, int v1, int v2, int v3); // returns the penetration depth (negative distance) of the convex objects // witness points are stored in x1 and x2 -static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, - mjtNum dir[3]); +static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2); -// internal data structure for the returning simplex from GJK -typedef struct { - mjtNum verts[12]; - int nverts; -} Simplex; -// internal GJK with returned data for EPA -static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, - Simplex* ret1, Simplex* ret2) { + +// GJK algorithm +static mjtNum gjk(mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions int n = 0; // number of vertices in the simplex mjtNum x_k[3]; // the kth approximation point with initial value x_0 // segregated simplices and points for the two objects to recover witness points - mjtNum simplex1[12], simplex2[12]; - mjtNum* x1_k = obj1->x0; - mjtNum* x2_k = obj2->x0; + mjtNum *simplex1 = status->simplex1, *simplex2 = status->simplex2; + mjtNum* x1_k = status->x1; + mjtNum* x2_k = status->x2; mju_sub3(x_k, x1_k, x2_k); - mjtNum epsilon = config->tolerance * config->tolerance; + mjtNum epsilon = status->tolerance * status->tolerance; - int N = config->max_iterations; - for (size_t k = 0; k < N; k++) { + int k = 0, N = status->max_iterations; + for (; k < N; k++) { mjtNum s1[3], s2[3]; // the support points in obj1 and obj2 mjtNum s_k[3]; // the kth support point of Minkowski difference mjtNum lambda[4]; // barycentric coordinates for x_k // compute the kth support point - gjk_support(s1, s2, obj1, obj2, x_k); + gjkSupport(s1, s2, obj1, obj2, x_k); mju_sub3(s_k, s1, s2); // the stopping criteria relies on the Frank-Wolfe duality gap given by @@ -181,30 +170,17 @@ static mjtNum _gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, break; } } - if (ret1 && ret2) { - ret1->nverts = n; - ret2->nverts = n; - for (int i = 0; i < n; i++) { - mju_copy3(ret1->verts + 3*i, simplex1 + 3*i); - mju_copy3(ret2->verts + 3*i, simplex2 + 3*i); - } - } + + status->gjk_iterations = k; + status->nsimplex = n; return mju_norm3(x_k); } -// returns the distance between the two objects. The witness points are -// recoverable from the x_0 field in obj1 and obj2. -mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { - return _gjk(config, obj1, obj2, NULL, NULL); -} - - - // computes the support points in obj1 and obj2 for the kth approximation point -static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, - const mjtNum x_k[3]) { +static void gjkSupport(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum x_k[3]) { mjtNum dir[3], dir_neg[3]; mju_copy3(dir_neg, x_k); mju_normalize3(dir_neg); // mjc_support assumes a normalized direction @@ -648,11 +624,10 @@ static void rotmat(mjtNum R[9], const mjtNum axis[3]) { // creates a polytope from a 1-simplex (2 points i.e. line segment) -static int polytope2(Polytope* pt, const mjtNum simplex1[6], const mjtNum simplex2[6], - mjCCDObj* obj1, mjCCDObj* obj2) { +static int polytope2(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum v1[3], v2[3]; - mju_sub3(v1, simplex1 + 0, simplex2 + 0); - mju_sub3(v2, simplex1 + 3, simplex2 + 3); + mju_sub3(v1, status->simplex1 + 0, status->simplex2 + 0); + mju_sub3(v2, status->simplex1 + 3, status->simplex2 + 3); mjtNum diff[3]; mju_sub3(diff, v2, v1); @@ -701,8 +676,8 @@ static int polytope2(Polytope* pt, const mjtNum simplex1[6], const mjtNum simple } // save vertices and get indices for each one - int v1i = newVertex(pt, simplex1 + 0, simplex2 + 0); - int v2i = newVertex(pt, simplex1 + 3, simplex2 + 3); + int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); + int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); int v3i = newVertex(pt, v3a, v3b); int v4i = newVertex(pt, v4a, v4b); int v5i = newVertex(pt, v5a, v5b); @@ -795,13 +770,12 @@ static int triPointIntersect(const mjtNum v1[3], const mjtNum v2[3], const mjtNu // creates a polytope from a 2-simplex (3 points i.e. triangle) -static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simplex2[9], - mjCCDObj* obj1, mjCCDObj* obj2) { +static int polytope3(Polytope* pt, const mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { // get vertices of simplex from GJK mjtNum v1[3], v2[3], v3[3]; - mju_sub3(v1, simplex1 + 0, simplex2 + 0); - mju_sub3(v2, simplex1 + 3, simplex2 + 3); - mju_sub3(v3, simplex1 + 6, simplex2 + 6); + mju_sub3(v1, status->simplex1 + 0, status->simplex2 + 0); + mju_sub3(v2, status->simplex1 + 3, status->simplex2 + 3); + mju_sub3(v3, status->simplex1 + 6, status->simplex2 + 6); // get normals in both directions mjtNum diff1[3], diff2[3], n[3], nn[3]; @@ -842,15 +816,15 @@ static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simple // 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. mjtNum dir[3]; - mju_sub3(dir, obj1->x0, obj2->x0); + mju_sub3(dir, status->x1, status->x2); if (mju_norm3(dir) > mjMINVAL && !testTetra(v1, v2, v3, v4) && !testTetra(v1, v2, v3, v5)) { return 0; } // save vertices and get indices for each one - int v1i = newVertex(pt, simplex1 + 0, simplex2 + 0); - int v2i = newVertex(pt, simplex1 + 3, simplex2 + 3); - int v3i = newVertex(pt, simplex1 + 6, simplex2 + 6); + int v1i = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); + int v2i = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); + int v3i = newVertex(pt, status->simplex1 + 6, status->simplex2 + 6); int v5i = newVertex(pt, v5a, v5b); int v4i = newVertex(pt, v4a, v4b); @@ -875,11 +849,11 @@ static int polytope3(Polytope* pt, const mjtNum simplex1[9], const mjtNum simple // creates a polytope from a 3-simplex (4 points i.e. tetrahedron) -static int polytope4(Polytope* pt, const mjtNum simplex1[12], const mjtNum simplex2[12]) { - int v1 = newVertex(pt, simplex1 + 0, simplex2 + 0); - int v2 = newVertex(pt, simplex1 + 3, simplex2 + 3); - int v3 = newVertex(pt, simplex1 + 6, simplex2 + 6); - int v4 = newVertex(pt, simplex1 + 9, simplex2 + 9); +static int polytope4(Polytope* pt, const mjCCDStatus* status) { + int v1 = newVertex(pt, status->simplex1 + 0, status->simplex2 + 0); + int v2 = newVertex(pt, status->simplex1 + 3, status->simplex2 + 3); + int v3 = newVertex(pt, status->simplex1 + 6, status->simplex2 + 6); + int v4 = newVertex(pt, status->simplex1 + 9, status->simplex2 + 9); attachFace(pt, v1, v2, v3); attachFace(pt, v1, v2, v4); @@ -1035,16 +1009,15 @@ static void epaWitness(const Polytope* pt, int index, mjtNum x1[3], mjtNum x2[3] // returns the penetration depth (negative distance) of the convex objects -static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2, - mjtNum dir[3]) { +static mjtNum epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* obj2) { mjtNum dist = mjMAXVAL; int index; Horizon h; initHorizon(&h); - int N = config->max_iterations; - mjtNum tolerance = config->tolerance; + mjtNum tolerance = status->tolerance; - for (int j = 0; j < N; j++) { + int k = 0, N = status->max_iterations; + for (; k < N; k++) { // find the closest face to the origin dist = mjMAXVAL; index = -1; @@ -1100,79 +1073,49 @@ static mjtNum epa(const mjCCDConfig* config, Polytope* pt, mjCCDObj* obj1, mjCCD h.n = 0; // clear horizon } free(h.edges); - Face face = pt->faces[index]; - mju_copy3(dir, face.v); - epaWitness(pt, index, obj1->x0, obj2->x0); + epaWitness(pt, index, status->x1, status->x2); + status->epa_iterations = k; return dist; } -// runs both GJK and EPA (if needed) -static mjtNum _gjk_epa(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2, Polytope* pt, - mjtNum dir[3]) { - Simplex simplex1, simplex2; - mjtNum dist = _gjk(config, obj1, obj2, &simplex1, &simplex2); +// run general convex collision detection +mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { + // set up + obj1->center(status->x1, obj1); + obj2->center(status->x2, obj2); + status->epa_iterations = -1; + status->tolerance = config->tolerance; + status->max_iterations = config->max_iterations; - if (dist <= config->tolerance && simplex1.nverts > 1) { + mjtNum dist = gjk(status, obj1, obj2); + + // penetration recovery for contacts not needed + if (!config->contacts) { + return dist; + } + + if (dist <= config->tolerance && status->nsimplex > 1) { + Polytope pt; + initPolytope(&pt); int ret; - if (simplex1.nverts == 2) { - ret = polytope2(pt, simplex1.verts, simplex2.verts, obj1, obj2); - } else if (simplex1.nverts == 3) { - ret = polytope3(pt, simplex1.verts, simplex2.verts, obj1, obj2); + if (status->nsimplex == 2) { + ret = polytope2(&pt, status, obj1, obj2); + } else if (status->nsimplex == 3) { + ret = polytope3(&pt, status, obj1, obj2); } else { - ret = polytope4(pt, simplex1.verts, simplex2.verts); + ret = polytope4(&pt, status); } // simplex not on boundary (objects are penetrating) if (ret) { - dist = epa(config, pt, obj1, obj2, dir); - return -dist; + dist = -epa(status, &pt, obj1, obj2); + } else { + dist = 0; } - return 0; + mju_free(pt.faces); + mju_free(pt.verts); } return dist; } - -// --------------------------- LibCCD Compatibility Layer ----------------------------------------- - -// Penetration function with same signature as LibCCD's ccdMPRPenetration and ccdGJKPenetration -int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, - ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos) { - Polytope pt; - initPolytope(&pt); - mjCCDConfig config; - - mjCCDObj* o1 = (mjCCDObj*) obj1; - mjtNum* x1 = o1->x0; - mjCCDObj* o2 = (mjCCDObj*) obj2; - mjtNum* x2 = o2->x0; - - o1->center(x1, o1); - o2->center(x2, o2); - - config.max_iterations = ccd->max_iterations; - config.tolerance = ccd->mpr_tolerance; - mjtNum d[3]; - mjtNum dist = _gjk_epa(&config, o1, o2, &pt, d); - - if (dist < 0) { - if (depth) *depth = -dist; - if (dir) { - mju_normalize3(d); - mju_copy3(dir->v, d); - } - if (pos) { - mju_scl3(x1, x1, 0.5); - mju_scl3(x2, x2, 0.5); - mju_add3(pos->v, x1, x2); - } - } else { - if (depth) *depth = 0; - if (dir) mju_zero3(dir->v); - if (pos) mju_zero3(dir->v); - } - free(pt.faces); - free(pt.verts); - return dist >= 0; -} diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index aee89818..47e1e539 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -19,27 +19,39 @@ #include #include "engine/engine_collision_convex.h" -#include -#include - #ifdef __cplusplus extern "C" { #endif -// internal configuration for convex collision detection +// configuration for convex collision detection struct _mjCCDConfig { - int max_iterations; - mjtNum tolerance; + int max_iterations; // the maximum number of iterations for GJK and EPA + mjtNum tolerance; // tolerance used by GJK and EPA + int contacts; // set to true to recover contact (pendetration) info + int distances; // set to true to recover distance info }; typedef struct _mjCCDConfig mjCCDConfig; -// Returns the distance between the two objects. The witness points are -// recoverable from x_0 in obj1 and obj2. -MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2); +// data produced from running GJK and EPA +struct _mjCCDStatus { + mjtNum x1[3]; // witness point for geom 1 + mjtNum x2[3]; // witness point for geom 2 -// Penetration function with same signature as LibCCD's ccdMPRPenetration and ccdGJKPenetration -MJAPI int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, - ccd_real_t *depth, ccd_vec3_t *dir, ccd_vec3_t *pos); + // configurations used + int max_iterations; // the maximum number of iterations for GJK and EPA + mjtNum tolerance; // tolerance used by GJK and EPA + + // 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) + mjtNum simplex1[12]; // the simplex that GJK returned for obj1 + mjtNum simplex2[12]; // the simplex that GJK returned for obj2 + int nsimplex; // size of simplex 1 & 2 +}; +typedef struct _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); #ifdef __cplusplus } #endif diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index d41972a8..970405b0 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -47,28 +47,58 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjc_support(vec->v, (mjCCDObj*) obj, _dir->v); } -mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], - mjtNum x2[3]) { +mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], + mjtNum x2[3]) { mjCCDConfig config = {kMaxIterations, kTolerance}; - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; - mjc_center(obj1.x0, &obj1); - mjc_center(obj2.x0, &obj2); - mjtNum dist = mj_gjk(&config, &obj1, &obj2); - if (x1 != nullptr) mju_copy3(x1, obj1.x0); - if (x2 != nullptr) mju_copy3(x2, obj2.x0); + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, + mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, + mjc_support}; + mjCCDStatus status; + mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); + if (x1 != nullptr) mju_copy3(x1, status.x1); + if (x2 != nullptr) mju_copy3(x2, status.x2); return dist; } +// drop in replacement for ccdMPRPenetration taken from mjc_penetration +int PenetrationWrapper(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, + ccd_real_t* depth, ccd_vec3_t* dir, ccd_vec3_t* pos) { + mjCCDConfig config; + mjCCDStatus status; -mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2, - mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr) { - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, - mjc_center, mjc_support}; + // set config + config.max_iterations = ccd->max_iterations, + config.tolerance = ccd->mpr_tolerance, + config.contacts = 1; + config.distances = 0; // no geom distances needed + + mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); + if (dist < 0) { + if (depth) *depth = -dist; + if (dir) { + mju_sub3(dir->v, status.x1, status.x2); + mju_normalize3(dir->v); + } + if (pos) { + pos->v[0] = 0.5 * (status.x1[0] + status.x2[0]); + pos->v[1] = 0.5 * (status.x1[1] + status.x2[1]); + pos->v[2] = 0.5 * (status.x1[2] + status.x2[2]); + } + return 0; + } + if (depth) *depth = 0; + if (dir) mju_zero3(dir->v); + if (pos) mju_zero3(dir->v); + return 1; +} + +mjtNum Penetration(mjModel* m, mjData* d, int g1, int g2, + mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr) { + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, + mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, mjc_center, + mjc_support}; ccd_t ccd; // CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration ccd.mpr_tolerance = kTolerance; @@ -82,7 +112,7 @@ mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2, ccd_real_t depth; ccd_vec3_t ccd_dir, ccd_pos; - int ret = mj_gjkPenetration(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos); + int ret = PenetrationWrapper(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos); if (ret) return 0; // objects not colliding if (dir) mju_copy3(dir, ccd_dir.v); if (pos) mju_copy3(pos, ccd_pos.v); @@ -116,7 +146,7 @@ TEST_F(MjGjkTest, SphereSphere) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); mjtNum x1[3], x2[3]; - mjtNum dist = run_gjk(model, data, geom1, geom2, x1, x2); + mjtNum dist = GeomDist(model, data, geom1, geom2, x1, x2); EXPECT_EQ(dist, 1); EXPECT_THAT(x1, ElementsAre(-.5, 0, 0)); @@ -125,60 +155,6 @@ TEST_F(MjGjkTest, SphereSphere) { mj_deleteModel(model); } -TEST_F(MjGjkTest, BoxBox) { - 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 dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); - - EXPECT_EQ(dist, 1); - mj_deleteData(data); - mj_deleteModel(model); -} - -TEST_F(MjGjkTest, BoxBoxIntersect) { - 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 = run_gjkPenetration(model, data, geom1, geom2, dir, pos); - - EXPECT_NEAR(dist, -1, kTolerance); - EXPECT_NEAR(dir[0], 1, kTolerance); - EXPECT_NEAR(dir[1], 0, kTolerance); - EXPECT_NEAR(dir[2], 0, kTolerance); - mj_deleteData(data); - mj_deleteModel(model); -} - TEST_F(MjGjkTest, EllipsoidEllipsoid) { static constexpr char xml[] = R"( @@ -197,13 +173,38 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr); EXPECT_NEAR(dist, 0.7542, .0001); mj_deleteData(data); mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBox) { + 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 dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr); + + EXPECT_EQ(dist, 1); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { static constexpr char xml[] = R"( @@ -222,7 +223,7 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr); EXPECT_NEAR(dist, 0, kTolerance); mj_deleteData(data); @@ -247,7 +248,7 @@ TEST_F(MjGjkTest, CapsuleCapsule) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + mjtNum dist = GeomDist(model, data, geom1, geom2, nullptr, nullptr); EXPECT_NEAR(dist, 0.4711, .0001); mj_deleteData(data);