chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 8 Packages Projects Releases Wiki Activity

Add support for NativeCCD multiple contacts for box-box collision.

Browse Source 
PiperOrigin-RevId: 718352304
Change-Id: Icaa827e716a2d7aa7c0c644e0ccd2913b476e6fb
This commit is contained in:
Kyle Bayes
2025-01-22 06:02:10 -08:00
committed by Copybara-Service
parent 0090e1e908
commit aa505a7872
4 changed files with 949 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+29 -15
View File
@@ -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);
}
src/engine/engine_collision_gjk.c
+387 -13
View File
@@ -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);
}
src/engine/engine_collision_gjk.h
+3
View File
@@ -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,
test/engine/engine_collision_gjk_test.cc
+530 -70
View File
@@ -17,6 +17,7 @@
#include "src/engine/engine_collision_gjk.h"
#include <array>
#include <vector>
#include <ccd/ccd.h>
#include <ccd/vec3.h>
@@ -28,11 +29,16 @@
#include <gmock/gmock.h>
#include <gtest/gtest.h>
// 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<mjtNum>& dir,
std::vector<mjtNum>& 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<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="0 0 1.9" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 0 0" size="10 10 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" pos="9.5 9.5 1.9" size="1 1 1"/>
<geom name="geom2" type="box" pos="0 0 0" size="10 10 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom type="box" name="geom1" size="5 5 .1" pos="0 0 0"/>
<geom type="box" name="geom2" size="1 1 1"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom name="geom1" size="0.25 0.25 0.05" type="box"/>
<geom name="geom2" size="0.25 0.25 0.05" type="box"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom name="geom1" size="0.25 0.25 0.05" type="box"/>
<geom name="geom2" size="0.25 0.25 0.05" type="box"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".5 .5 .1" pos="0 0 -.1"/>
<geom name="geom2" type="box" size=".1 .1 .1" pos="0 0 0"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".25 .25 .05" pos="0 0 0"/>
<geom name="geom2" type="box" size=".25 .25 .05" pos="0 0 0"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
<worldbody>
<geom name="geom1" type="box" size=".25 .25 .05" pos="0 0 0"/>
<geom name="geom2" type="box" size=".25 .25 .05" pos="0 0 0"/>
</worldbody>
</mujoco>)";
std::array<char, 1000> 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<mjtNum> 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"(
<mujoco>
@@ -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<mjtNum> 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<mjtNum> 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<mjtNum> 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<mjtNum> 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);