From aa505a78726912fe743787dfa3a2adb60248927a Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 22 Jan 2025 06:02:10 -0800 Subject: [PATCH] Add support for NativeCCD multiple contacts for box-box collision. PiperOrigin-RevId: 718352304 Change-Id: Icaa827e716a2d7aa7c0c644e0ccd2913b476e6fb --- src/engine/engine_collision_convex.c | 44 +- src/engine/engine_collision_gjk.c | 400 ++++++++++++++- src/engine/engine_collision_gjk.h | 3 + test/engine/engine_collision_gjk_test.cc | 600 ++++++++++++++++++++--- 4 files changed, 949 insertions(+), 98 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 712841a8..04f9ccc3 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -779,9 +779,9 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { -// find single convex-convex collision +// find convex-convex collision static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, mjCCDObj* obj2, - mjContact* con, mjtNum margin) { + mjContact* con, int max_contacts, mjtNum margin) { if (mjENABLED(mjENBL_NATIVECCD)) { mjCCDConfig config; mjCCDStatus status; @@ -789,19 +789,22 @@ static int mjc_CCDIteration(const mjModel* m, const mjData* d, mjCCDObj* obj1, m // set config config.max_iterations = m->opt.ccd_iterations; config.tolerance = m->opt.ccd_tolerance; - config.max_contacts = 1; + config.max_contacts = max_contacts; config.dist_cutoff = 0; // no geom distances needed mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); if (dist < 0) { - con->dist = margin + dist; - mju_sub3(con->frame, status.x1, status.x2); - mju_normalize3(con->frame); - con->pos[0] = 0.5 * (status.x1[0] + status.x2[0]); - con->pos[1] = 0.5 * (status.x1[1] + status.x2[1]); - con->pos[2] = 0.5 * (status.x1[2] + status.x2[2]); - mju_zero3(con->frame+3); - return 1; + for (int i = 0; i < status.nx; i++) { + mjContact* c = con++; + c->dist = margin + dist; + mju_sub3(c->frame, status.x1 + 3*i, status.x2 + 3*i); + mju_normalize3(c->frame); + c->pos[0] = 0.5 * (status.x1[0 + 3*i] + status.x2[0 + 3*i]); + c->pos[1] = 0.5 * (status.x1[1 + 3*i] + status.x2[1 + 3*i]); + c->pos[2] = 0.5 * (status.x1[2 + 3*i] + status.x2[2 + 3*i]); + mju_zero3(c->frame+3); + } + return status.nx; } return 0; } @@ -884,12 +887,23 @@ int mjc_Convex(const mjModel* m, const mjData* d, mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, margin); mjc_initCCDObj(&obj2, m, d, g2, margin); + int max_contacts = 1; + if (mjENABLED(mjENBL_MULTICCD)) { + // TODO(kylebayes): Support contact pruning. + max_contacts = 8; + } // find initial contact - int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); + int ncon = mjc_CCDIteration(m, d, &obj1, &obj2, con, max_contacts, margin); + + // nativeccd supports multi Box-Box collision directly + if (mjENABLED(mjENBL_NATIVECCD) && m->geom_type[g1] == mjGEOM_BOX + && m->geom_type[g2] == mjGEOM_BOX) { + return ncon; + } // look for additional contacts - if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) + if (ncon == 1 && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { // multiCCD parameters @@ -935,7 +949,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_CCDIteration(m, d, &obj1, &obj2, con+ncon, margin); + int new_contact = mjc_CCDIteration(m, d, &obj1, &obj2, con+ncon, 1, margin); // check new contact if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) { @@ -1583,7 +1597,7 @@ int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, mjc_setCCDObjFlex(&obj2, f2, e2, -1); // find contacts - return mjc_CCDIteration(m, d, &obj1, &obj2, con, margin); + return mjc_CCDIteration(m, d, &obj1, &obj2, con, 1, margin); } diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index ec81a722..543d3f95 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -87,13 +87,23 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob // -------------------------------- inlined 3D vector utils -------------------------------------- -// v1 == v2 +// v1 == v2 up to 1e-15 static inline int equal3(const mjtNum v1[3], const mjtNum v2[3]) { return mju_abs(v1[0] - v2[0]) < mjMINVAL && mju_abs(v1[1] - v2[1]) < mjMINVAL && mju_abs(v1[2] - v2[2]) < mjMINVAL; } +// v1 == v2 +static inline int equalexact3(const mjtNum v1[3], const mjtNum v2[3]) { + return v1[0] == v2[0] && v1[1] == v2[1] && v1[2] == v2[2]; +} + +// res = v1 + v2 +static inline void add3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { + res[0] = v1[0] + v2[0], res[1] = v1[1] + v2[1], res[2] = v1[2] + v2[2]; +} + // res = v1 - v2 static inline void sub3(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { res[0] = v1[0] - v2[0], res[1] = v1[1] - v2[1], res[2] = v1[2] - v2[2]; @@ -501,10 +511,19 @@ static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mj -// return true only when a and b are both strictly positive or both strictly negative -static inline int sameSign(mjtNum a, mjtNum b) { +// return 1 if both numbers are positive, -1 if both negative and 0 otherwise +static inline int sameSign2(mjtNum a, mjtNum b) { if (a > 0 && b > 0) return 1; - if (a < 0 && b < 0) return 1; + if (a < 0 && b < 0) return -1; + return 0; +} + + + +// return 1 if all three numbers are positive, -1 if all negative and 0 otherwise +static inline int sameSign3(mjtNum a, mjtNum b, mjtNum c) { + if (a > 0 && b > 0 && c > 0) return 1; + if (a < 0 && b < 0 && c < 0) return -1; return 0; } @@ -553,10 +572,10 @@ static void S3D(mjtNum lambda[4], const mjtNum s1[3], const mjtNum s2[3], const // with vertices {s1, s2, s3, 0} - si mjtNum m_det = C41 + C42 + C43 + C44; - int comp1 = sameSign(m_det, C41), - comp2 = sameSign(m_det, C42), - comp3 = sameSign(m_det, C43), - comp4 = sameSign(m_det, C44); + int comp1 = sameSign2(m_det, C41), + comp2 = sameSign2(m_det, C42), + comp3 = sameSign2(m_det, C43), + comp4 = sameSign2(m_det, C44); // if all signs are the same then the origin is inside the simplex if (comp1 && comp2 && comp3 && comp4) { @@ -706,9 +725,9 @@ static void S2D(mjtNum lambda[3], const mjtNum s1[3], const mjtNum s2[3], const mjtNum C33 = p_o_2D[0]*s1_2D[1] + p_o_2D[1]*s2_2D[0] + s1_2D[0]*s2_2D[1] - p_o_2D[0]*s2_2D[1] - p_o_2D[1]*s1_2D[0] - s2_2D[0]*s1_2D[1]; - int comp1 = sameSign(M_max, C31), - comp2 = sameSign(M_max, C32), - comp3 = sameSign(M_max, C33); + int comp1 = sameSign2(M_max, C31), + comp2 = sameSign2(M_max, C32), + comp3 = sameSign2(M_max, C33); // all the same sign, p_o is inside the 2-simplex if (comp1 && comp2 && comp3) { @@ -780,7 +799,7 @@ static void S1D(mjtNum lambda[2], const mjtNum s1[3], const mjtNum s2[3]) { mjtNum C2 = s1[index] - p_o[index]; // inside the simplex - if (sameSign(mu_max, C1) && sameSign(mu_max, C2)) { + if (sameSign2(mu_max, C1) && sameSign2(mu_max, C2)) { lambda[0] = C1 / mu_max; lambda[1] = C2 / mu_max; } else { @@ -1434,6 +1453,358 @@ static Face* epa(mjCCDStatus* status, Polytope* pt, mjCCDObj* obj1, mjCCDObj* ob } +// ------------------------------------- MultiCCD ------------------------------------------------- + +// find the normal of a plane perpendicular to the face (given by its normal n) and intersecting the +// face edge (v1, v2) +static mjtNum planeNormal(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3], + const mjtNum n[3]) { + mjtNum v3[3], diff1[3], diff2[3]; + add3(v3, v1, n); + sub3(diff1, v2, v1); + sub3(diff2, v3, v1); + cross3(res, diff1, diff2); + return dot3(res, v1); +} + + + +// 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; +} + + + +// compute the intersection of a plane with a line segment (a, b) +static mjtNum planeIntersect(mjtNum res[3], const mjtNum pn[3], mjtNum pd, + const mjtNum a[3], const mjtNum b[3]) { + mjtNum ab[3]; + sub3(ab, b, a); + mjtNum temp = dot3(pn, ab); + if (temp == 0.0) return mjMAXVAL; // parallel; no intersection + mjtNum t = (pd - dot3(pn, a)) / temp; + if (t >= 0.0 && t <= 1.0) { + res[0] = a[0] + t*ab[0]; + res[1] = a[1] + t*ab[1]; + res[2] = a[2] + t*ab[2]; + } + return t; +} + + + +// clip a polygon against another polygon +static void polygonClip(mjCCDStatus* status, const mjtNum face1[3 * mjMAX_SIDES], int nface1, + const mjtNum face2[3 * mjMAX_SIDES], int nface2, const mjtNum n[3], + const mjtNum dir[3]) { + // compute plane normal and distance to plane for each vertex + mjtNum pn[3 * mjMAX_SIDES], pd[mjMAX_SIDES]; + for (int i = 0; i < nface1 - 1; i++) { + pd[i] = planeNormal(&pn[3*i], &face1[3*i], &face1[3*i + 3], n); + } + pd[nface1 - 1] = planeNormal(&pn[3*(nface1 - 1)], &face1[3*(nface1 - 1)], &face1[0], n); + + // reserve 2 * max_sides as max sides for a clipped polygon + mjtNum polygon1[6 * mjMAX_SIDES], polygon2[6 * mjMAX_SIDES], *polygon, *clipped; + int npolygon = nface2, nclipped = 0; + polygon = polygon1; + clipped = polygon2; + + for (int i = 0; i < nface2; i++) { + copy3(polygon + 3*i, face2 + 3*i); + } + + // clip the polygon by one edge e at a time + for (int e = 0; e < (3 * nface1); e += 3) { + for (int i = 0; i < npolygon; i++) { + // get edge PQ of the polygon + mjtNum *P = polygon + 3*i; + mjtNum *Q = (i < npolygon - 1) ? polygon + 3*(i+1) : polygon; + + // determine if P and Q are in the halfspace of the clipping edge + int inside1 = halfspace(face1 + e, pn + e, P); + int inside2 = halfspace(face1 + e, pn + e, Q); + + // PQ entirely outside the clipping edge, skip + if (!inside1 && !inside2) { + continue; + } + + // edge PQ is inside the clipping edge, add Q + if (inside1 && inside2) { + copy3(clipped + 3*nclipped++, Q); + continue; + } + + // add new vertex to clipped polygon where PQ intersects the clipping edge + mjtNum t = planeIntersect(clipped + 3*nclipped++, pn + e, pd[e/3], P, Q); + if (t < 0.0 || t > 1.0) { + nclipped--; // no intersection in PQ + } + + // add Q as PQ is now back inside the clipping edge + if (inside2) { + copy3(clipped + 3*nclipped++, Q); + } + } + + // swap clipped and polygon + mjtNum* tmp = polygon; + polygon = clipped; + clipped = tmp; + npolygon = nclipped; + nclipped = 0; + } + + // copy final clipped polygon to status + if (npolygon > 0) { + status->nx = npolygon; + for (int i = 0; i < 3*npolygon; i += 3) { + copy3(status->x2 + i, polygon + i); + sub3(status->x1 + i, status->x2 + i, dir); + } + } +} + + + +// compute local coordinates of a global point (g1, g2, g3) +static inline void localcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum pos[3], + mjtNum g1, mjtNum g2, mjtNum g3) { + // perform matT * ((g1, g2, g3) - pos) + if (pos) { + g1 -= pos[0]; + g2 -= pos[1]; + g3 -= pos[2]; + } + res[0] = mat[0]*g1 + mat[3]*g2 + mat[6]*g3; + res[1] = mat[1]*g1 + mat[4]*g2 + mat[7]*g3; + res[2] = mat[2]*g1 + mat[5]*g2 + mat[8]*g3; +} + + + +// compute global coordinates of a local point (l1, l2, l3) +static inline void globalcoord(mjtNum res[3], const mjtNum mat[9], const mjtNum pos[3], + mjtNum l1, mjtNum l2, mjtNum l3) { + // perform mat * (l1, l2, l3) + pos + res[0] = mat[0]*l1 + mat[1]*l2 + mat[2]*l3; + res[1] = mat[3]*l1 + mat[4]*l2 + mat[5]*l3; + res[2] = mat[6]*l1 + mat[7]*l2 + mat[8]*l3; + if (pos) { + res[0] += pos[0]; + res[1] += pos[1]; + res[2] += pos[2]; + } +} + + + +// 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, + const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { + // box data + int g = 3*obj->geom; + const mjtNum* mat = obj->data->geom_xmat + 3*g; + const mjtNum* pos = obj->data->geom_xpos + g; + + // rotate global coordinates to geom local frame + mjtNum v1_local[3], v2_local[3], v3_local[3]; + if (dim > 0) localcoord(v1_local, mat, pos, v1[0], v1[1], v1[2]); + if (dim > 1) localcoord(v2_local, mat, pos, v2[0], v2[1], v2[2]); + if (dim > 2) localcoord(v3_local, mat, pos, v3[0], v3[1], v3[2]); + + if (dim == 3) { + int x = sameSign3(v1_local[0], v2_local[0], v3_local[0]); + int y = sameSign3(v1_local[1], v2_local[1], v3_local[1]); + int z = sameSign3(v1_local[2], v2_local[2], v3_local[2]); + globalcoord(res, mat, NULL, x, y, z); + int sgn = x + y + z; + if (x) resind[0] = 0; + if (y) resind[0] = 2; + if (z) resind[0] = 4; + if (sgn == -1) resind[0]++; + return 1; + } + + if (dim == 2) { + int x = sameSign2(v1_local[0], v2_local[0]); + int y = sameSign2(v1_local[1], v2_local[1]); + int z = sameSign2(v1_local[2], v2_local[2]); + if (x) { + globalcoord(res, mat, NULL, x, 0, 0); + resind[0] = (x > 0) ? 0 : 1; + } + if (y) { + int i = (x ? 1 : 0); + globalcoord(res + 3*i, mat, NULL, 0, y, 0); + resind[i] = (y > 0) ? 2 : 3; + } + if (z) { + globalcoord(res + 3, mat, NULL, 0, 0, z); + resind[1] = (z > 0) ? 4 : 5; + } + return 2; + } + + if (dim == 1) { + mjtNum x = (v1_local[0] > 0) ? 1 : -1; + mjtNum y = (v1_local[1] > 0) ? 1 : -1; + mjtNum z = (v1_local[2] > 0) ? 1 : -1; + globalcoord(res + 0, mat, NULL, x, 0, 0); + globalcoord(res + 3, mat, NULL, 0, y, 0); + globalcoord(res + 6, mat, NULL, 0, 0, z); + resind[0] = (x > 0) ? 0 : 1; + resind[1] = (y > 0) ? 2 : 3; + resind[2] = (z > 0) ? 4 : 5; + return 3; + } + return 0; +} + + + +// recover face of a box from its index +static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) { + // box data + int g = 3*obj->geom; + const mjtNum* mat = obj->data->geom_xmat + 3*g; + const mjtNum* pos = obj->data->geom_xpos + g; + const mjtNum* size = obj->model->geom_size + g; + + // compute global coordinates of the box face and face normal + switch (idx) { + case 0: // right + globalcoord(res + 0, mat, pos, size[0], size[1], size[2]); + globalcoord(res + 3, mat, pos, size[0], size[1], -size[2]); + globalcoord(res + 6, mat, pos, size[0], -size[1], -size[2]); + globalcoord(res + 9, mat, pos, size[0], -size[1], size[2]); + return 4; + case 1: // left + globalcoord(res + 0, mat, pos, -size[0], size[1], -size[2]); + globalcoord(res + 3, mat, pos, -size[0], size[1], size[2]); + globalcoord(res + 6, mat, pos, -size[0], -size[1], size[2]); + globalcoord(res + 9, mat, pos, -size[0], -size[1], -size[2]); + return 4; + case 2: // top + globalcoord(res + 0, mat, pos, -size[0], size[1], -size[2]); + globalcoord(res + 3, mat, pos, size[0], size[1], -size[2]); + globalcoord(res + 6, mat, pos, size[0], size[1], size[2]); + globalcoord(res + 9, mat, pos, -size[0], size[1], size[2]); + return 4; + case 3: // bottom + globalcoord(res + 0, mat, pos, -size[0], -size[1], size[2]); + globalcoord(res + 3, mat, pos, size[0], -size[1], size[2]); + globalcoord(res + 6, mat, pos, size[0], -size[1], -size[2]); + globalcoord(res + 9, mat, pos, -size[0], -size[1], -size[2]); + return 4; + case 4: // front + globalcoord(res + 0, mat, pos, -size[0], size[1], size[2]); + globalcoord(res + 3, mat, pos, size[0], size[1], size[2]); + globalcoord(res + 6, mat, pos, size[0], -size[1], size[2]); + globalcoord(res + 9, mat, pos, -size[0], -size[1], size[2]); + return 4; + case 5: // back + globalcoord(res + 0, mat, pos, size[0], size[1], -size[2]); + globalcoord(res + 3, mat, pos, -size[0], size[1], -size[2]); + globalcoord(res + 6, mat, pos, -size[0], -size[1], -size[2]); + globalcoord(res + 9, mat, pos, size[0], -size[1], -size[2]); + return 4; + } + return 0; +} + + + +static inline int compareNorms(int res[2], const mjtNum* v, int nv, + const mjtNum* w, int nw) { + for (int i = 0; i < nv; i++) { + for (int j = 0; j < nw; j++) { + if (dot3(v + 3*i, w + 3*j) < -0.99999872) { + res[0] = i; + res[1] = j; + return 1; + } + } + } + return 0; +} + + + +// return number of dimensions of a feature (1, 2 or 3) +static inline int simplexDim(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { + int i = 1; + int same1 = equalexact3(v1, v2); + int same2 = equalexact3(v1, v3); + int same3 = equalexact3(v2, v3); + if (!same1) i++; + if (!same3 && !same2) i++; + return i; +} + + + +// recover multiple contacts from EPA polytope +static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status, + mjCCDObj* obj1, mjCCDObj* obj2) { + mjtNum face1[mjMAX_SIDES * 3], face2[mjMAX_SIDES * 3]; + + // get vertices of faces from EPA + const mjtNum* v11 = pt->verts1 + face->verts[0]; + const mjtNum* v12 = pt->verts1 + face->verts[1]; + const mjtNum* v13 = pt->verts1 + face->verts[2]; + const mjtNum* v21 = pt->verts2 + face->verts[0]; + const mjtNum* v22 = pt->verts2 + face->verts[1]; + const mjtNum* v23 = pt->verts2 + face->verts[2]; + + // get dimensions of features of geoms 1 and 2 + int nface1 = simplexDim(v11, v12, v13); + int nface2 = simplexDim(v21, v22, v23); + int nnorms1 = 0, nnorms2 = 0; + mjtNum n1[9], n2[9]; // normals of possible face collisions + int idx1[3], idx2[3]; // indices of faces, so they can be recovered later + + // get all possible face normals for each geom + if (obj1->geom_type == mjGEOM_BOX) { + nnorms1 = boxNormals(n1, idx1, nface1, obj1, v11, v12, v13); + } + if (obj2->geom_type == mjGEOM_BOX) { + nnorms2 = boxNormals(n2, idx2, nface2, obj2, v21, v22, v23); + } + + // determine if any two normals match + int res[2]; + if (!compareNorms(res, n1, nnorms1, n2, nnorms2)) { + return; + } + int i = res[0], j = res[1]; + + // recover matching faces + if (obj1->geom_type == mjGEOM_BOX) { + nface1 = boxFace(face1, obj1, idx1[i]); + } + if (obj2->geom_type == mjGEOM_BOX) { + nface2 = boxFace(face2, obj2, idx2[j]); + } + + if (nface1 >= 3 && nface2 >= 3) { + // 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); + scl3(approx_dir, n2 + 3*j, mju_sqrt(dot3(diff, diff))); + + // clip the faces and store the results in status + polygonClip(status, face1, nface1, face2, nface2, n1 + 3*i, approx_dir); + } +} + + + // inflate a contact by margin static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) { @@ -1574,7 +1945,10 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m // simplex not on boundary (objects are penetrating) if (!ret) { - epa(status, &pt, obj1, obj2); + Face* face = epa(status, &pt, obj1, obj2); + if (config->max_contacts > 1 && face) { + multicontact(&pt, face, status, obj1, obj2); + } } mj_freeStack(d); } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 5c0601c6..78e1e135 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -25,6 +25,9 @@ extern "C" { #endif +// max sides of a face of mesh supported for multiple contacts +#define mjMAX_SIDES 10 + // Status of an EPA run typedef enum { mjEPA_NOCONTACT = -1, diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index bb8daf65..234fdce2 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -17,6 +17,7 @@ #include "src/engine/engine_collision_gjk.h" #include +#include #include #include @@ -28,11 +29,16 @@ #include #include +// uncomment to run tests with libccd +// #define TEST_WITH_LIBCCD + namespace mujoco { namespace { using ::testing::NotNull; using ::testing::ElementsAre; +using ::testing::Pointwise; +using ::testing::DoubleNear; constexpr mjtNum kTolerance = 1e-6; constexpr int kMaxIterations = 1000; @@ -78,64 +84,70 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], 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) { +int Penetration(mjtNum& depth, std::vector& dir, + std::vector& pos, mjModel* model, mjData* data, + int g1, int g2, mjtNum margin = 0, int max_contacts = 1) { + mjCCDObj obj1, obj2; + mjc_initCCDObj(&obj1, model, data, g1, margin); + mjc_initCCDObj(&obj2, model, data, g2, margin); + +#if defined(TEST_WITH_LIBCCD) + if (max_contacts == 1) { + ccd_t ccd; + CCD_INIT(&ccd); + ccd.mpr_tolerance = kTolerance; + ccd.epa_tolerance = kTolerance; + ccd.max_iterations = kMaxIterations; + ccd.center1 = mjccd_center; + ccd.center2 = mjccd_center; + ccd.support1 = mjccd_support; + ccd.support2 = mjccd_support; + + ccd_real_t ccd_depth; + ccd_vec3_t ccd_dir, ccd_pos; + + int ret = ccdMPRPenetration(&obj1, &obj2, &ccd, &ccd_depth, &ccd_dir, + &ccd_pos); + if (ret) return 0; + dir.resize(3); + pos.resize(3); + depth = -ccd_depth; + mju_copy3(dir.data(), ccd_dir.v); + mju_copy3(pos.data(), ccd_pos.v); + return 1; + } +#endif + mjCCDConfig config; mjCCDStatus status; // set config - config.max_iterations = ccd->max_iterations, - config.tolerance = ccd->mpr_tolerance, - config.max_contacts = 1; + config.max_iterations = kMaxIterations; + config.tolerance = kTolerance; + config.max_contacts = max_contacts; config.dist_cutoff = 0; // no geom distances needed + config.max_contacts = max_contacts; - mjtNum dist = mjc_ccd(&config, &status, obj1, obj2); + 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); + dir.resize(3 * status.nx); + pos.resize(3 * status.nx); + for (int i = 0; i < status.nx; ++i) { + // compute direction + mju_sub3(&dir[3 * i], status.x1 + 3 * i, status.x2 + 3 * i); + mju_normalize3(&dir[3 * i]); + + // compute position + pos[3 * i + 0] = 0.5 * (status.x1[0 + 3 * i] + status.x2[0 + 3 * i]); + pos[3 * i + 1] = 0.5 * (status.x1[1 + 3 * i] + status.x2[1 + 3 * i]); + pos[3 * i + 2] = 0.5 * (status.x1[2 + 3 * i] + status.x2[2 + 3 * i]); } - 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; + depth = dist; + return status.nx; } - 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, - mjtNum margin = 0) { - mjCCDObj obj1, obj2; - mjc_initCCDObj(&obj1, m, d, g1, margin); - mjc_initCCDObj(&obj2, m, d, g2, margin); - - ccd_t ccd; - // CCD_INIT(&ccd); // uncomment to run ccdMPRPenetration - ccd.mpr_tolerance = kTolerance; - ccd.epa_tolerance = kTolerance; - ccd.max_iterations = kMaxIterations; - ccd.center1 = mjccd_center; - ccd.center2 = mjccd_center; - ccd.support1 = mjccd_support; - ccd.support2 = mjccd_support; - - ccd_real_t depth; - ccd_vec3_t ccd_dir, ccd_pos; - - int ret = PenetrationWrapper(&obj1, &obj2, &ccd, &depth, &ccd_dir, &ccd_pos); - // objects not colliding, return max value as geom distance was never computed - if (ret) return mjMAXVAL; - if (dir) mju_copy3(dir, ccd_dir.v); - if (pos) mju_copy3(pos, ccd_pos.v); - return -depth; + // no contacts + return 0; } using MjGjkTest = MujocoTest; @@ -211,10 +223,11 @@ TEST_F(MjGjkTest, SphereSphereNoDist) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); - EXPECT_EQ(dist, mjMAXVAL); + EXPECT_EQ(ncons, 0); mj_deleteData(data); mj_deleteModel(model); } @@ -237,8 +250,11 @@ TEST_F(MjGjkTest, SphereSphereIntersect) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + + EXPECT_EQ(ncons, 1); // penetration depth EXPECT_NEAR(dist, -2, kTolerance); @@ -275,13 +291,18 @@ TEST_F(MjGjkTest, BoxBoxDepth) { 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); + + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + + EXPECT_EQ(ncons, 1); 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); } @@ -321,10 +342,11 @@ TEST_F(MjGjkTest, BoxBoxDepth2) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); - if (dist < 0) { + if (ncons == 1) { EXPECT_NEAR(dist, -0.033401579411886845, kTolerance); EXPECT_NEAR(dir[0], 0, kTolerance); EXPECT_NEAR(dir[1], 0, kTolerance); @@ -387,9 +409,11 @@ TEST_F(MjGjkTest, BoxBoxDepth3) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.003066, kTolerance); EXPECT_NEAR(dir[0], 0, kTolerance); EXPECT_NEAR(dir[1], 0, kTolerance); @@ -417,15 +441,442 @@ TEST_F(MjGjkTest, BoxBoxTouching) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); - EXPECT_EQ(dist, mjMAXVAL); + EXPECT_EQ(ncons, 0); mj_deleteData(data); mj_deleteModel(model); } +TEST_F(MjGjkTest, BoxBoxMultiCCD) { + 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"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + EXPECT_NEAR(dist, -.1, kTolerance); + + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + + EXPECT_THAT(pos, Pointwise(DoubleNear(kTolerance), {-1.0, 1.0, 0.95, + 1.0, 1.0, 0.95, + 1.0, -1.0, 0.95, + -1.0, -1.0, 0.95})); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD2) { + 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"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + EXPECT_NEAR(dist, -.1, kTolerance); + + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + + EXPECT_THAT(pos, Pointwise(DoubleNear(kTolerance), { 8.5, 10.0, 0.95, + 10.0, 10.0, 0.95, + 10.0, 8.5, 0.95, + 8.5, 8.5, 0.95})); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD3) { + 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* xmat = data->geom_xmat + 9; + mjtNum* xpos = data->geom_xpos + 3; + + xmat[0] = 0.999999806540386004805043285160; + xmat[1] = -0.000014738590672566122784237219; + xmat[2] = 0.000621853651764864637230267874; + xmat[3] = -0.000621853434269146370175218586; + xmat[4] = 0.000014756878555191479777952690; + xmat[5] = 0.999999806540251667819063641218; + xmat[6] = -0.000014747764440060310685981504; + xmat[7] = -0.999999999782504311873765345808; + xmat[8] = 0.000014747710457105431443303178; + + xpos[0] = -0.941218618591869393696924817050; + xpos[1] = 2.209729011624415928594089564285; + xpos[2] = 1.095456702630382306296041861060; + + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD4) { + 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* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 0.500063246694118501700643264485; + xmat[1] = -0.865988885078582182330819705385; + xmat[2] = -0.000015036290463686326402846169; + xmat[3] = 0.865988885208801795201338791230; + xmat[4] = 0.500063246603650646271432833601; + xmat[5] = 0.000009541064416582982810641038; + xmat[6] = -0.000000743359510433135621196039; + xmat[7] = -0.000017792396065397684211655677; + xmat[8] = 0.999999999841438502734547455475; + + xpos[0] = -0.015346718925143524800414063236; + xpos[1] = -0.023500448793229846561336771060; + xpos[2] = -4.859382717259980388746498647379; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 0.999999999448633714038692232862; + xmat[1] = -0.000033207420761195452995305499; + xmat[2] = -0.000000044925527333868828730462; + xmat[3] = 0.000033207420790006526530903364; + xmat[4] = 0.999999999448428988912951353996; + xmat[5] = 0.000000641458652741046316968134; + xmat[6] = 0.000000044904226121706672357864; + xmat[7] = -0.000000641460144248257277838641; + xmat[8] = 0.999999999999794386695839421009; + + xpos[0] = -0.015347749710384111718197708285; + xpos[1] = -0.023500601273213628239489025873; + xpos[2] = -4.958782854594746325460619118530; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 8); + EXPECT_NEAR(dist, -0.00060425119242707459, kTolerance); + + EXPECT_NEAR(dir[0], 0, kTolerance); + EXPECT_NEAR(dir[1], 0, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD5) { + 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* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 0.965955045562010394810670277366; + xmat[1] = -0.258709898141739669252814337597; + xmat[2] = -0.000196358811267032467391333017; + xmat[3] = 0.258709919231419560592399875532; + xmat[4] = 0.965955055634174608591990818240; + xmat[5] = 0.000090476785846218643442895324; + xmat[6] = 0.000166266546411239724218358860; + xmat[7] = -0.000138196479997660070767120932; + xmat[8] = 0.999999976628582865068040064216; + + xpos[0] = -0.015381524498156991936914650410; + xpos[1] = -0.023527931890396581310342938309; + xpos[2] = -4.559214004409498421921398403356; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 0.866076536677693908927722077351; + xmat[1] = -0.499911388413602053581996642606; + xmat[2] = -0.000190658753729162216972170540; + xmat[3] = 0.499911409912061843741071243130; + xmat[4] = 0.866076540935322825021103199106; + xmat[5] = 0.000086494189368211055798235654; + xmat[6] = 0.000121885643632020743577087929; + xmat[7] = -0.000170223074359586521841353202; + xmat[8] = 0.999999978083999430111816764111; + + xpos[0] = -0.015358668590921718474784363195; + xpos[1] = -0.023542070504611382203430380855; + xpos[2] = -4.659108354876987156956147373421; + + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 8); + EXPECT_NEAR(dist, -0.0001077858631973211, kTolerance); + + EXPECT_NEAR(dir[0], 0.00019065, kTolerance); + EXPECT_NEAR(dir[1], -8.6494189274575805e-05, kTolerance); + EXPECT_NEAR(dir[2], -1, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD6) { + 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* xmat = data->geom_xmat + 9; + mjtNum* xpos = data->geom_xpos + 3; + + xmat[0] = -0.412617528992808124677083014831; + xmat[1] = -0.910903939143411389700588642881; + xmat[2] = -0.000887930675351447824816819576; + xmat[3] = 0.910904370383107120368038067681; + xmat[4] = -0.412617275794986082537718630192; + xmat[5] = -0.000460143975736545586020798115; + xmat[6] = 0.000052771423713213129642884969; + xmat[7] = -0.000998683403024198425301793947; + xmat[8] = 0.999999499923193035932911243435; + + xpos[0] = 0.413029898172642018217004533653; + xpos[1] = 0.190777715293135141649827346555; + xpos[2] = 0.100006658017411736993906856696; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 5); + EXPECT_NEAR(dist, -0.00009843, kTolerance); + + EXPECT_NEAR(dir[0], -0.0008879306751646528, kTolerance); + EXPECT_NEAR(dir[1], -0.00046014397575771832, kTolerance); + EXPECT_NEAR(dir[2], 1, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD7) { + 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* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 0.482851932827058627495375731087; + xmat[1] = -0.875697459006381406787511423317; + xmat[2] = 0.002823341095436950488190008812; + xmat[3] = 0.875701084072774249555948244961; + xmat[4] = 0.482853601927766051815638093103; + xmat[5] = -0.000102269990141710693382082198; + xmat[6] = -0.001273702846902712276094815635; + xmat[7] = 0.002521784120391480209927292933; + xmat[8] = 0.999996009134990648803409385437; + + xpos[0] = -0.002020740254618143012105280221; + xpos[1] = -0.022654384848980465422263463893; + xpos[2] = -4.858542902144324493463045655517; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 0.999985133805306514176436394337; + xmat[1] = -0.005293845271528460454113496070; + xmat[2] = 0.001306663930443651821383665990; + xmat[3] = 0.005293871114312041943616993223; + xmat[4] = 0.999985987232967277194006783247; + xmat[5] = -0.000016319793417504115210251922; + xmat[6] = -0.001306559226005186893221354794; + xmat[7] = 0.000023236861241766870316309210; + xmat[8] = 0.999999146181155484924829579541; + + xpos[0] = -0.011066235018223425159988870803; + xpos[1] = -0.023114696036485724711662115283; + xpos[2] = -4.958375812037025376355359185254; + + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 8); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBoxMultiCCD8) { + 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* xmat = data->geom_xmat; + mjtNum* xpos = data->geom_xpos; + + xmat[0] = 1.000000000000000000000000000000; + xmat[1] = 0.000000000000000000000000000000; + xmat[2] = 0.000000000000000000000000000000; + xmat[3] = 0.000000000000000000000000000000; + xmat[4] = 1.000000000000000000000000000000; + xmat[5] = 0.000000000000000000000000000000; + xmat[6] = 0.000000000000000000000000000000; + xmat[7] = 0.000000000000000000000000000000; + xmat[8] = 1.000000000000000000000000000000; + + xpos[0] = -0.015346500000000000765720820084; + xpos[1] = -0.023505499999999998617106200527; + xpos[2] = -4.859662640000005140450412000064; + + xmat = data->geom_xmat + 9; + xpos = data->geom_xpos + 3; + + xmat[0] = 1.000000000000000000000000000000; + xmat[1] = 0.000000000000000000000000000000; + xmat[2] = 0.000000000000000000000000000000; + xmat[3] = 0.000000000000000000000000000000; + xmat[4] = 1.000000000000000000000000000000; + xmat[5] = -0.000000000000000015361939765351; + xmat[6] = 0.000000000000000000000000000000; + xmat[7] = 0.000000000000000015361939765351; + xmat[8] = 1.000000000000000000000000000000; + + xpos[0] = -0.015346500000000000765720820084; + xpos[1] = -0.023505499999999998617106200527; + xpos[2] = -4.958574289672835533338002278470; + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 0, 1000); + + EXPECT_EQ(ncons, 4); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, SmallBoxMesh) { static constexpr char xml[] = R"( @@ -465,9 +916,11 @@ TEST_F(MjGjkTest, SmallBoxMesh) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, 0, kTolerance); // direction @@ -495,9 +948,11 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidPenetrating) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.00022548856248122027, kTolerance); mj_deleteData(data); mj_deleteModel(model); @@ -576,9 +1031,11 @@ TEST_F(MjGjkTest, LongBox) { 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); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -0.01, kTolerance); EXPECT_NEAR(dir[0], 0, kTolerance); @@ -611,8 +1068,11 @@ TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); - mjtNum dist = Penetration(model, data, geom1, geom2, nullptr, nullptr, 15); + std::vector dir, pos; + mjtNum dist; + int ncons = Penetration(dist, dir, pos, model, data, geom1, geom2, 15); + EXPECT_EQ(ncons, 1); EXPECT_NEAR(dist, -14.245732934582151, kTolerance); mj_deleteData(data); mj_deleteModel(model);