// Copyright 2021 DeepMind Technologies Limited // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. #include "engine/engine_collision_convex.h" #include #include #include #include #include #include #include #include "engine/engine_collision_primitive.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "engine/engine_util_spatial.h" // ccd center function void mjccd_center(const void *obj, ccd_vec3_t *center) { const mjtCCD* ccd = (const mjtCCD*)obj; int g = ccd->geom; int f = ccd->flex; int e = ccd->elem; int v = ccd->vert; // return geom position if (g >= 0) { mju_copy3(center->v, ccd->data->geom_xpos + 3*g); } // return flex element position else if (e >= 0) { mju_copy3(center->v, ccd->data->flexelem_aabb + 6*(ccd->model->flex_elemadr[f]+e)); } // return flex vertex position else { mju_copy3(center->v, ccd->data->flexvert_xpos + 3*(ccd->model->flex_vertadr[f]+v)); } } // ccd support function void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { const mjtCCD* ccd = (const mjtCCD*)obj; const mjModel* m = ccd->model; const mjData* d = ccd->data; int g = ccd->geom; //-------------------------- flex element or vertex ----------------------------- if (g < 0) { int f = ccd->flex; int dim = m->flex_dim[f]; mjtNum *res = vec->v; const mjtNum *dir = _dir->v; // flex element if (ccd->elem >= 0) { int e = ccd->elem; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f]; // find element vertex with largest projection along dir mju_copy3(res, vert+3*edata[0]); mjtNum best = mju_dot3(res, dir); for (int i=1; i <= dim; i++) { mjtNum dot = mju_dot3(vert+3*edata[i], dir); // better vertex found: assign if (dot > best) { best = dot; mju_copy3(res, vert+3*edata[i]); } } // add radius and margin/2 mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*ccd->margin); return; } // flex vertex else { const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + ccd->vert); mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*ccd->margin); return; } } //-------------------------- geom ------------------------------------------- float* vertdata; int ibest, graphadr, numvert, change, locid; int *vert_edgeadr, *vert_globalid, *edge_localid; mjtNum tmp, vdot; const mjtNum* size = m->geom_size+3*g; // geom sizes mjtNum dir[3]; // direction in geom local frame mjtNum res[3]; // result in geom local frame // rotate dir to geom local frame mju_rotVecMatT(dir, _dir->v, d->geom_xmat+9*g); // compute result according to geom type switch ((mjtGeom) m->geom_type[g]) { case mjGEOM_SPHERE: mju_scl3(res, dir, size[0]); break; case mjGEOM_CAPSULE: // start with sphere mju_scl3(res, dir, size[0]); // add cylinder contribution res[2] += mju_sign(dir[2]) * size[1]; break; case mjGEOM_ELLIPSOID: // find support point on unit sphere: scale dir by ellipsoid sizes and renormalize for (int i=0; i < 3; i++) { res[i] = dir[i] * size[i]; } mju_normalize3(res); // transform to ellipsoid for (int i=0; i < 3; i++) { res[i] *= size[i]; } break; case mjGEOM_CYLINDER: // set result in XY plane: support on circle tmp = mju_sqrt(dir[0]*dir[0] + dir[1]*dir[1]); if (tmp > mjMINVAL) { res[0] = dir[0]/tmp*size[0]; res[1] = dir[1]/tmp*size[0]; } else { res[0] = res[1] = 0; } // set result in Z direction res[2] = mju_sign(dir[2]) * size[1]; break; case mjGEOM_BOX: for (int i=0; i < 3; i++) { res[i] = mju_sign(dir[i]) * size[i]; } break; case mjGEOM_MESH: // init search vertdata = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]]; tmp = -1E+10; ibest = -1; // no graph data: exhaustive search if (m->mesh_graphadr[m->geom_dataid[g]] < 0) { // search all vertices, find best for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) { // vdot = dot(vertex, dir) vdot = dir[0] * (mjtNum)vertdata[3*i] + dir[1] * (mjtNum)vertdata[3*i+1] + dir[2] * (mjtNum)vertdata[3*i+2]; // update best if (vdot > tmp) { tmp = vdot; ibest = i; } } // record best vertex index, in globalid format ((mjtCCD*)ccd)->meshindex = ibest; } // hill-climb using graph data else { // get info graphadr = m->mesh_graphadr[m->geom_dataid[g]]; numvert = m->mesh_graph[graphadr]; vert_edgeadr = m->mesh_graph + graphadr + 2; vert_globalid = m->mesh_graph + graphadr + 2 + numvert; edge_localid = m->mesh_graph + graphadr + 2 + 2*numvert; // init with first vertex in convex hull ibest = 0; tmp = dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] + dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] + dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2]; // hill-climb until no change change = 1; while (change) { // look for improvement in ibest neighborhood change = 0; int i = vert_edgeadr[ibest]; while ((locid=edge_localid[i]) >= 0) { // vdot = dot(vertex, dir) vdot = dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; // update best if (vdot > tmp) { tmp = vdot; ibest = locid; change = 1; } // advance to next edge i++; } } // record best vertex index, in locid format ((mjtCCD*)ccd)->meshindex = ibest; // map best index to globalid ibest = vert_globalid[ibest]; } // sanity check, SHOULD NOT OCCUR if (ibest < 0) { mju_warning("mesh_support could not find support vertex"); mju_zero3(res); } // copy best vertex else { for (int i=0; i < 3; i++) { res[i] = (mjtNum)vertdata[3*ibest + i]; } } break; default: mjERROR("ccd support function is undefined for geom type %d", m->geom_type[g]); } // add dir*margin/2 to result for (int i=0; i < 3; i++) { res[i] += dir[i] * ccd->margin/2; } // rotate result to global frame mju_rotVecMat(vec->v, res, d->geom_xmat+9*g); // add geom position mju_addTo3(vec->v, d->geom_xpos+3*g); } // find single convex-convex collision, using libccd static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd, const mjModel* m, const mjData* d, mjContact* con, mjtNum margin) { ccd_vec3_t dir, pos; ccd_real_t depth; if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) { // contact is found but normal is undefined if (ccdVec3Eq(&dir, ccd_vec3_origin)) { return 0; } // fill in contact data con->dist = margin-depth; mju_copy3(con->frame, dir.v); mju_copy3(con->pos, pos.v); mju_zero3(con->frame+3); // both geoms: fix contact frame normal if (obj1->geom >= 0 && obj2->geom >= 0) { mjc_fixNormal(m, d, con, obj1->geom, obj2->geom); } return 1; } // no contact found else { return 0; } } // compare new contact to previous contacts, return 1 if it is far from all of them static int mjc_isDistinctContact(mjContact* con, int ncon, mjtNum tolerance) { for (int i=0; i < ncon-1; i++) { if (mju_dist3(con[i].pos, con[ncon - 1].pos) <= tolerance) { return 0; } } return 1; } // in-place rotation of spatial frame around given point of origin static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], mjtNum xmat[9], mjtNum xpos[3]) { mjtNum mat[9], vec[3], rel[3]; // rotate frame: xmat = rot*xmat mju_mulMatMat(mat, rot, xmat, 3, 3, 3); mju_copy(xmat, mat, 9); // vector to rotation origin: rel = origin - xpos mju_sub3(rel, origin, xpos); // displacement of origin due to rotation: vec = rot*rel - rel mju_rotVecMat(vec, rel, rot); mju_subFrom3(vec, rel); // correct xpos by subtracting displacement: xpos = xpos - vec mju_subFrom3(xpos, vec); } // multi-point convex-convex collision, using libccd int mjc_Convex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { ccd_t ccd; mjtCCD obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; // init ccd structure ccd.first_dir = ccdFirstDirDefault; ccd.center1 = mjccd_center; ccd.center2 = mjccd_center; ccd.support1 = mjccd_support; ccd.support2 = mjccd_support; // set ccd parameters ccd.max_iterations = m->opt.mpr_iterations; ccd.mpr_tolerance = m->opt.mpr_tolerance; // find initial contact int ncon = mjc_MPRIteration(&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 (?) && 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 const mjtNum relative_tolerance = 1e-3; const mjtNum perturbation_angle = 1e-3; // save positions and orientations of g1 and g2 mjtNum xpos1[3], xmat1[9], xpos2[3], xmat2[9]; mju_copy3(xpos1, d->geom_xpos+3*g1); mju_copy(xmat1, d->geom_xmat+9*g1, 9); mju_copy3(xpos2, d->geom_xpos+3*g2); mju_copy(xmat2, d->geom_xmat+9*g2, 9); // complete frame of initial contact mjtNum frame[9]; mju_copy(frame, con[0].frame, 9); mju_makeFrame(frame); // tolerance for determining if newly found contacts are distinct const mjtNum tolerance = relative_tolerance * mju_min(m->geom_rbound[g1], m->geom_rbound[g2]); // axes and rotation angles for perturbation test mjtNum* axes[2] = {frame+3, frame+6}; mjtNum angles[2] = {-perturbation_angle, perturbation_angle}; // rotate both geoms, search for new contacts for (int axis_id = 0; axis_id < 2; ++axis_id) { for (int angle_id = 0; angle_id < 2; ++angle_id) { mjtNum* axis = axes[axis_id]; mjtNum angle = angles[angle_id]; // make rotation matrix rot mjtNum quat[4], rot[9]; mju_axisAngle2Quat(quat, axis, angle); mju_quat2Mat(rot, quat); // rotate g1 around initial contact point mju_rotateFrame(con[0].pos, rot, d->geom_xmat+9*g1, d->geom_xpos+3*g1); // inversely rotate g2 around initial contact point mjtNum invrot[9]; mju_transpose(invrot, rot, 3, 3); 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); // check new contact if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) { // set penetration of new point to equal that of initial point con[ncon].dist = con[0].dist; // add new point ncon += 1; } // reset positions and orientations of g1 and g2 mju_copy3(d->geom_xpos+3*g1, xpos1); mju_copy(d->geom_xmat+9*g1, xmat1, 9); mju_copy3(d->geom_xpos+3*g2, xpos2); mju_copy(d->geom_xmat+9*g2, xmat2, 9); } } } return ncon; } // parameters for plane-mesh extra contacts const int maxplanemesh = 3; const mjtNum tolplanemesh = 0.3; // add one plane-mesh contact static int addplanemesh(mjContact* con, const float vertex[3], const mjtNum pos1[3], const mjtNum normal1[3], const mjtNum pos2[3], const mjtNum mat2[9], const mjtNum first[3], mjtNum rbound) { // compute point in global coordinates mjtNum pnt[3], v[3] = {vertex[0], vertex[1], vertex[2]}; mju_rotVecMat(pnt, v, mat2); mju_addTo3(pnt, pos2); // skip if too close to first contact if (mju_dist3(pnt, first) < tolplanemesh*rbound) { return 0; } // pnt-pos difference vector mjtNum dif[3]; mju_sub3(dif, pnt, pos1); // set distance con->dist = mju_dot3(normal1, dif); // set position mju_copy3(con->pos, pnt); mju_addToScl3(con->pos, normal1, -0.5*con->dist); // set frame mju_copy3(con->frame, normal1); mju_zero3(con->frame+3); return 1; } // plane-convex collision, using libccd int mjc_PlaneConvex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { mjGETINFO mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]}; ccd_vec3_t dir, vec; mjtCCD 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]); mjccd_support(&obj, &dir, &vec); // compute normal distance, return if too far mju_sub3(dif, vec.v, pos1); dist = mju_dot3(normal, dif); if (dist > margin) { return 0; } // fill in contact data con->dist = dist; mju_copy3(con->pos, vec.v); mju_addToScl3(con->pos, normal, -0.5*dist); mju_copy3(con->frame, normal); mju_zero3(con->frame+3); //--------------- add all/connected vertices below margin float* vertdata; int graphadr, numvert, locid; int *vert_edgeadr, *vert_globalid, *edge_localid; mjtNum vdot; int count = 1, g = g2; // g is an ellipsoid: no need for further mesh-specific processing if (m->geom_dataid[g] == -1) { return count; } // init vertdata = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]]; // express dir in geom local frame mjtNum locdir[3]; mju_rotVecMatT(locdir, dir.v, d->geom_xmat+9*g); // inclusion threshold along locdir, relative to geom2 center mju_sub3(dif, pos2, pos1); mjtNum threshold = mju_dot3(normal, dif) - margin; // no graph data: exhaustive search if (m->mesh_graphadr[m->geom_dataid[g]] < 0) { // search all vertices, find best for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]] && count < maxplanemesh; i++) { // vdot = dot(vertex, dir) vdot = locdir[0] * (mjtNum)vertdata[3*i] + locdir[1] * (mjtNum)vertdata[3*i+1] + locdir[2] * (mjtNum)vertdata[3*i+2]; // detect contact, skip best if (vdot > threshold && i != obj.meshindex) { count += addplanemesh(con+count, vertdata+3*i, pos1, normal, pos2, mat2, con->pos, m->geom_rbound[g2]); } } } // use graph data else if (obj.meshindex >= 0) { // get info graphadr = m->mesh_graphadr[m->geom_dataid[g]]; numvert = m->mesh_graph[graphadr]; vert_edgeadr = m->mesh_graph + graphadr + 2; vert_globalid = m->mesh_graph + graphadr + 2 + numvert; edge_localid = m->mesh_graph + graphadr + 2 + 2*numvert; // look for contacts in ibest neighborhood int i = vert_edgeadr[obj.meshindex]; while ((locid=edge_localid[i]) >= 0 && count < maxplanemesh) { // vdot = dot(vertex, dir) vdot = locdir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + locdir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + locdir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; // detect contact if (vdot > threshold) { count += addplanemesh(con+count, vertdata+3*vert_globalid[locid], pos1, normal, pos2, mat2, con->pos, m->geom_rbound[g2]); } // advance to next edge i++; } } return count; } //---------------------------- heightfield collisions --------------------------------------------- // ccd prism object type struct _mjtPrism { mjtNum v[6][3]; }; typedef struct _mjtPrism mjtPrism; // ccd prism support function static void prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) { int istart, ibest; mjtNum best, tmp; const mjtPrism* p = (const mjtPrism*)obj; // find best vertex in halfspace determined by dir.z istart = dir->v[2] < 0 ? 0 : 3; ibest = istart; best = mju_dot3(p->v[istart], dir->v); for (int i=istart+1; i < istart+3; i++) { if ((tmp = mju_dot3(p->v[i], dir->v)) > best) { ibest = i; best = tmp; } } // copy best point mju_copy3(vec->v, p->v[ibest]); } // ccd prism center function static void prism_center(const void *obj, ccd_vec3_t *center) { const mjtPrism* p = (const mjtPrism*)obj; // compute mean mju_zero3(center->v); for (int i=0; i < 6; i++) { mju_addTo3(center->v, p->v[i]); } mju_scl3(center->v, center->v, 1.0/6.0); } // ccd prism first dir static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) { ccdVec3Set(vec, 0, 0, 1); } // add vertex to prism, count vertices static void addVert(int* nvert, mjtPrism* prism, mjtNum x, mjtNum y, mjtNum z) { // move old data mju_copy3(prism->v[0], prism->v[1]); mju_copy3(prism->v[1], prism->v[2]); mju_copy3(prism->v[3], prism->v[4]); mju_copy3(prism->v[4], prism->v[5]); // add new vertex at last position prism->v[2][0] = prism->v[5][0] = x; prism->v[2][1] = prism->v[5][1] = y; prism->v[5][2] = z; // count (*nvert)++; } // entry point for heightfield collisions int mjc_ConvexHField(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { mjGETINFO_HFIELD mjtNum mat[9], savemat2[9], savepos2[3], pos[3], vec[3], r2, dx, dy; mjtNum xmin, xmax, ymin, ymax, zmin, zmax; int hid = m->geom_dataid[g1]; int nrow = m->hfield_nrow[hid]; int ncol = m->hfield_ncol[hid]; int dr[2], cnt, rmin, rmax, cmin, cmax; const float* data = m->hfield_data + m->hfield_adr[hid]; mjtPrism prism; // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; ccd_t ccd; // point size1 to hfield size instead of geom1 size size1 = m->hfield_size + 4*hid; //------------------------------------- frame alignment, box-sphere test // express geom2 pos in heightfield frame mju_sub3(vec, pos2, pos1); mju_mulMatTVec(pos, mat1, vec, 3, 3); // get geom2 rbound r2 = m->geom_rbound[g2]; // box-sphere test: horizontal plane for (int i=0; i < 2; i++) { if ((size1[i] < pos[i]-r2-margin) || (-size1[i] > pos[i]+r2+margin)) { return 0; } } // box-sphere test in: vertical direction if (size1[2] < pos[2]-r2-margin) { // up return 0; } if (-size1[3] > pos[2]+r2+margin) { // down return 0; } // express geom2 mat in heightfield frame mju_mulMatTMat(mat, mat1, mat2, 3, 3, 3); //------------------------------------- AABB computation, box-box test // save mat2 and pos2, replace with relative frame mju_copy(savemat2, mat2, 9); mju_copy3(savepos2, pos2); mju_copy(mat2, mat, 9); mju_copy3(pos2, pos); // get support point in +X ccdVec3Set(&dirccd, 1, 0, 0); mjccd_support(&obj, &dirccd, &vecccd); xmax = vecccd.v[0]; // get support point in -X ccdVec3Set(&dirccd, -1, 0, 0); mjccd_support(&obj, &dirccd, &vecccd); xmin = vecccd.v[0]; // get support point in +Y ccdVec3Set(&dirccd, 0, 1, 0); mjccd_support(&obj, &dirccd, &vecccd); ymax = vecccd.v[1]; // get support point in -Y ccdVec3Set(&dirccd, 0, -1, 0); mjccd_support(&obj, &dirccd, &vecccd); ymin = vecccd.v[1]; // get support point in +Z ccdVec3Set(&dirccd, 0, 0, 1); mjccd_support(&obj, &dirccd, &vecccd); zmax = vecccd.v[2]; // get support point in -Z ccdVec3Set(&dirccd, 0, 0, -1); mjccd_support(&obj, &dirccd, &vecccd); zmin = vecccd.v[2]; // box-box test if ((xmin-margin > size1[0]) || (xmax+margin < -size1[0]) || (ymin-margin > size1[1]) || (ymax+margin < -size1[1]) || (zmin-margin > size1[2]) || (zmax+margin < -size1[3])) { // restore mat2 and pos2 mju_copy(mat2, savemat2, 9); mju_copy3(pos2, savepos2); return 0; } // compute sub-grid bounds cmin = (int) floor((xmin + size1[0]) / (2*size1[0]) * (ncol-1)); cmax = (int) ceil ((xmax + size1[0]) / (2*size1[0]) * (ncol-1)); rmin = (int) floor((ymin + size1[1]) / (2*size1[1]) * (nrow-1)); rmax = (int) ceil ((ymax + size1[1]) / (2*size1[1]) * (nrow-1)); cmin = mjMAX(0, cmin); cmax = mjMIN(ncol-1, cmax); rmin = mjMAX(0, rmin); rmax = mjMIN(nrow-1, rmax); //------------------------------------- collision testing // init ccd structure ccd.first_dir = prism_firstdir; ccd.center1 = prism_center; ccd.center2 = mjccd_center; ccd.support1 = prism_support; ccd.support2 = mjccd_support; // set ccd parameters ccd.max_iterations = m->opt.mpr_iterations; ccd.mpr_tolerance = m->opt.mpr_tolerance; // geom margin needed for actual collision test obj.margin = margin; // compute real-valued grid step, and triangulation direction dx = (2.0*size1[0]) / (ncol-1); dy = (2.0*size1[1]) / (nrow-1); dr[0] = 1; dr[1] = 0; // set zbottom value using base size prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -size1[3]; // process all prisms in sub-grid cnt = 0; for (int r=rmin; r < rmax; r++) { int nvert = 0; for (int c=cmin; c <= cmax; c++) { for (int i=0; i < 2; i++) { // send vertex to prism constructor addVert(&nvert, &prism, dx*c-size1[0], dy*(r+dr[i])-size1[1], data[(r+dr[i])*ncol+c]*size1[2]+margin); // check for enough vertices if (nvert > 2) { // prism height test if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) { continue; } // run MPR, save contact if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 && !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; mju_rotVecMat(con[cnt].frame, dirccd.v, mat1); mju_rotVecMat(con[cnt].pos, vecccd.v, mat1); mju_addTo3(con[cnt].pos, pos1); mju_zero3(con[cnt].frame+3); // count, stop if max number reached cnt++; if (cnt >= mjMAXCONPAIR) { r = rmax+1; c = cmax+1; i = 3; break; } } } } } } // restore mat2 and pos2 mju_copy(mat2, savemat2, 9); mju_copy3(pos2, savepos2); // fix contact normals for (int i=0; i < cnt; i++) { mjc_fixNormal(m, d, con+i, g1, g2); } return cnt; } //--------------------------- fix contact frame normal --------------------------------------------- // compute normal for point outside ellipsoid, using ray-projection SQP static int mjc_ellipsoidInside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum size[3]) { // algorithm constants const int maxiter = 30; const mjtNum tolerance = 1e-6; // precompute quantities mjtNum S2inv[3] = {1/(size[0]*size[0]), 1/(size[1]*size[1]), 1/(size[2]*size[2])}; mjtNum C = pos[0]*pos[0]*S2inv[0] + pos[1]*pos[1]*S2inv[1] + pos[2]*pos[2]*S2inv[2] - 1; if (C > 0) { return 0; } // normalize initial normal (just in case) mju_normalize3(nrm); // main iteration int iter; for (iter=0; iter < maxiter; iter++) { // coefficients and determinant of quadratic mjtNum A = nrm[0]*nrm[0]*S2inv[0] + nrm[1]*nrm[1]*S2inv[1] + nrm[2]*nrm[2]*S2inv[2]; mjtNum B = pos[0]*nrm[0]*S2inv[0] + pos[1]*nrm[1]*S2inv[1] + pos[2]*nrm[2]*S2inv[2]; mjtNum det = B*B - A*C; if (det < mjMINVAL || A < mjMINVAL) { return (iter > 0); } // ray intersection with ellipse: pos + x*nrm, x>=0 mjtNum x = (-B + mju_sqrt(det))/A; if (x < 0) { return (iter > 0); } // new point on ellipsoid mjtNum pnt[3]; mju_addScl3(pnt, pos, nrm, x); // normal at new point mjtNum newnrm[3] = {pnt[0]*S2inv[0], pnt[1]*S2inv[1], pnt[2]*S2inv[2]}; mju_normalize3(newnrm); // save change and assign mjtNum change = mju_dist3(nrm, newnrm); mju_copy3(nrm, newnrm); // terminate if converged if (change < tolerance) { break; } } return 1; } // compute normal for point inside ellipsoid, using diagonal QCQP static int mjc_ellipsoidOutside(mjtNum nrm[3], const mjtNum pos[3], const mjtNum size[3]) { // algorithm constants const int maxiter = 30; const mjtNum tolerance = 1e-6; // precompute quantities mjtNum S2[3] = {size[0]*size[0], size[1]*size[1], size[2]*size[2]}; mjtNum PS2[3] = {pos[0]*pos[0]*S2[0], pos[1]*pos[1]*S2[1], pos[2]*pos[2]*S2[2]}; // main iteration mjtNum la = 0; int iter; for (iter=0; iter < maxiter; iter++) { // precompute 1/(s^2+la) mjtNum R[3] = {1/(S2[0]+la), 1/(S2[1]+la), 1/(S2[2]+la)}; // value mjtNum val = PS2[0]*R[0]*R[0] + PS2[1]*R[1]*R[1] + PS2[2]*R[2]*R[2] - 1; if (val < tolerance) { break; } // derivative mjtNum deriv = -2*(PS2[0]*R[0]*R[0]*R[0] + PS2[1]*R[1]*R[1]*R[1] + PS2[2]*R[2]*R[2]*R[2]); if (deriv > -mjMINVAL) { break; } // delta mjtNum delta = -val/deriv; if (delta < tolerance) { break; } // update la += delta; } // compute normal given lambda nrm[0] = pos[0]/(S2[0]+la); nrm[1] = pos[1]/(S2[1]+la); nrm[2] = pos[2]/(S2[2]+la); mju_normalize3(nrm); return 1; } // entry point void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2) { mjtNum dst1, dst2; // get geom ids and types int gid[2] = {g1, g2}; mjtGeom type[2]; for (int i=0; i < 2; i++) { type[i] = m->geom_type[gid[i]]; // set to mjGEOM_NONE if type cannot be processed if (type[i] != mjGEOM_SPHERE && type[i] != mjGEOM_CAPSULE && type[i] != mjGEOM_ELLIPSOID && type[i] != mjGEOM_CYLINDER) { type[i] = mjGEOM_NONE; } } // neither type can be processed: nothing to do if (type[0] == mjGEOM_NONE && type[1] == mjGEOM_NONE) { return; } // init normals mjtNum normal[2][3] = { {con->frame[0], con->frame[1], con->frame[2]}, {-con->frame[0], -con->frame[1], -con->frame[2]} }; // process geoms in type range int processed[2] = {0, 0}; for (int i=0; i < 2; i++) { if (type[i] != mjGEOM_NONE) { // get geom mat and size mjtNum* mat = d->geom_xmat + 9*gid[i]; mjtNum* size = m->geom_size + 3*gid[i]; // map contact point and normal to local frame mjtNum dif[3], pos[3], nrm[3]; mju_sub3(dif, con->pos, d->geom_xpos+3*gid[i]); mju_rotVecMatT(pos, dif, mat); mju_rotVecMatT(nrm, normal[i], mat); // process according to type switch (type[i]) { case mjGEOM_SPHERE: mju_copy3(nrm, pos); processed[i] = 1; break; case mjGEOM_CAPSULE: // Z: bottom cap if (pos[2] < -size[1]) { nrm[2] = pos[2]+size[1]; } // Z: top cap else if (pos[2] > size[1]) { nrm[2] = pos[2]-size[1]; } // Z: cylinder else { nrm[2] = 0; } // copy XY nrm[0] = pos[0]; nrm[1] = pos[1]; processed[i] = 1; break; case mjGEOM_ELLIPSOID: // guard against invalid ellipsoid size (just in case) if (size[0] < mjMINVAL || size[1] < mjMINVAL || size[2] < mjMINVAL) { break; } // compute elliptic distance^2 dst1 = pos[0]*pos[0]/(size[0]*size[0]) + pos[1]*pos[1]/(size[1]*size[1]) + pos[2]*pos[2]/(size[2]*size[2]); // dispatch to inside or outside solver if (dst1 <= 1) { processed[i] = mjc_ellipsoidInside(nrm, pos, size); } else { processed[i] = mjc_ellipsoidOutside(nrm, pos, size); } break; case mjGEOM_CYLINDER: // skip if within 5% length of flat wall if (mju_abs(pos[2]) > 0.95*size[1]) { break; } // compute distances to flat and round wall dst1 = mju_abs(size[1]-mju_abs(pos[2])); dst2 = mju_abs(size[0]-mju_norm(pos, 2)); // require 4x closer to round than flat wall if (dst1 < 0.25*dst2) { break; } // set normal for round wall nrm[0] = pos[0]; nrm[1] = pos[1]; nrm[2] = 0; processed[i] = 1; break; default: // do nothing: only sphere, capsule, ellipsoid and cylinder are processed break; } // normalize and map normal to global frame if (processed[i]) { mju_normalize3(nrm); mju_rotVecMat(normal[i], nrm, mat); } } } // both processed: average if (processed[0] && processed[1]) { mju_sub3(con->frame, normal[0], normal[1]); mju_normalize3(con->frame); } // first processed: copy else if (processed[0]) { mju_copy3(con->frame, normal[0]); } // second processed: copy reverse else if (processed[1]) { mju_scl3(con->frame, normal[1], -1); } // clear second frame axis if processed, just in case if (processed[0] || processed[1]) { mju_zero3(con->frame+3); } } //---------------------------- flex collisions --------------------------------------------- // geom-elem or elem-elem or vert-elem convex collision using ccd 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; mjtCCD obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}}; mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}}; // init ccd structure ccd.first_dir = ccdFirstDirDefault; ccd.center1 = mjccd_center; ccd.center2 = mjccd_center; ccd.support1 = mjccd_support; ccd.support2 = mjccd_support; // set ccd parameters ccd.max_iterations = m->opt.mpr_iterations; ccd.mpr_tolerance = m->opt.mpr_tolerance; // find contacts int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin); return ncon; } // test a heighfield geom and a flex flex element for collision int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, int g, int f, int e, mjtNum margin) { mjtNum vec[3], dx, dy; mjtNum xmin, xmax, ymin, ymax, zmin, zmax; int dr[2], cnt, rmin, rmax, cmin, cmax; mjtPrism prism; // get hfield info int hid = m->geom_dataid[g]; int nrow = m->hfield_nrow[hid]; int ncol = m->hfield_ncol[hid]; mjtNum* hpos = d->geom_xpos + 3*g; mjtNum* hmat = d->geom_xmat + 9*g; mjtNum* hsize = m->hfield_size + 4*hid; const float* hdata = m->hfield_data + m->hfield_adr[hid]; // get elem indo int dim = m->flex_dim[f]; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); mjtNum* evert[4] = {NULL, NULL, NULL, NULL}; for (int i=0; i <= dim; i++) { evert[i] = d->flexvert_xpos + 3*(m->flex_vertadr[f] + edata[i]); } mjtNum* ecenter = d->flexelem_aabb + 6*(m->flex_elemadr[f]+e); // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; mjtCCD obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}}; ccd_t ccd; //------------------------------------- AABB computation, box-box test // save elem vertices, transform to hfield frame mjtNum savevert[4][3]; for (int i=0; i <= dim; i++) { mju_copy3(savevert[i], evert[i]); mju_sub3(vec, evert[i], hpos); mju_mulMatTVec(evert[i], hmat, vec, 3, 3); } // save elem center, transform to hfield frame mjtNum savecenter[3]; mju_copy3(savecenter, ecenter); mju_sub3(vec, ecenter, hpos); mju_mulMatTVec(ecenter, hmat, vec, 3, 3); // compute elem bounding box (in hfield frame) xmin = xmax = evert[0][0]; ymin = ymax = evert[0][1]; zmin = zmax = evert[0][2]; for (int i=1; i <= dim; i++) { xmin = mju_min(xmin, evert[i][0]); xmax = mju_max(xmax, evert[i][0]); ymin = mju_min(ymin, evert[i][1]); ymax = mju_max(ymax, evert[i][1]); zmin = mju_min(zmin, evert[i][2]); zmax = mju_max(zmax, evert[i][2]); } // box-box test if ((xmin-margin > hsize[0]) || (xmax+margin < -hsize[0]) || (ymin-margin > hsize[1]) || (ymax+margin < -hsize[1]) || (zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) { // restore vertices and center for (int i=0; i <= dim; i++) { mju_copy3(evert[i], savevert[i]); } mju_copy3(ecenter, savecenter); return 0; } // compute sub-grid bounds cmin = (int) floor((xmin + hsize[0]) / (2*hsize[0]) * (ncol-1)); cmax = (int) ceil ((xmax + hsize[0]) / (2*hsize[0]) * (ncol-1)); rmin = (int) floor((ymin + hsize[1]) / (2*hsize[1]) * (nrow-1)); rmax = (int) ceil ((ymax + hsize[1]) / (2*hsize[1]) * (nrow-1)); cmin = mjMAX(0, cmin); cmax = mjMIN(ncol-1, cmax); rmin = mjMAX(0, rmin); rmax = mjMIN(nrow-1, rmax); //------------------------------------- collision testing // init ccd structure ccd.first_dir = prism_firstdir; ccd.center1 = prism_center; ccd.center2 = mjccd_center; ccd.support1 = prism_support; ccd.support2 = mjccd_support; // set ccd parameters ccd.max_iterations = m->opt.mpr_iterations; ccd.mpr_tolerance = m->opt.mpr_tolerance; // compute real-valued grid step, and triangulation direction dx = (2.0*hsize[0]) / (ncol-1); dy = (2.0*hsize[1]) / (nrow-1); dr[0] = 1; dr[1] = 0; // set zbottom value using base size prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -hsize[3]; // process all prisms in sub-grid cnt = 0; for (int r=rmin; r < rmax; r++) { int nvert = 0; for (int c=cmin; c <= cmax; c++) { for (int k=0; k < 2; k++) { // send vertex to prism constructor addVert(&nvert, &prism, dx*c-hsize[0], dy*(r+dr[k])-hsize[1], hdata[(r+dr[k])*ncol+c]*hsize[2]+margin); // check for enough vertices if (nvert > 2) { // prism height test if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) { continue; } // run MPR, save contact if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) { if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; mju_rotVecMat(con[cnt].frame, dirccd.v, hmat); mju_rotVecMat(con[cnt].pos, vecccd.v, hmat); mju_addTo3(con[cnt].pos, hpos); mju_zero3(con[cnt].frame+3); // count, stop if max number reached cnt++; if (cnt >= mjMAXCONPAIR) { r = rmax+1; c = cmax+1; k = 3; break; } } } } } } } // restore elem vertices and center for (int i=0; i <= dim; i++) { mju_copy3(evert[i], savevert[i]); } mju_copy3(ecenter, savecenter); return cnt; }