Yuval Tassa
a9ee497e33
PiperOrigin-RevId: 573620198 Change-Id: Icf295cc0dd381a4a7f0e2c94f2e12b499193e862
1268 lines
35 KiB
C
1268 lines
35 KiB
C
// 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 <math.h>
|
|
#include <stddef.h>
|
|
|
|
#include <ccd/ccd.h>
|
|
#include <ccd/vec3.h>
|
|
|
|
#include <mujoco/mjdata.h>
|
|
#include <mujoco/mjmacro.h>
|
|
#include <mujoco/mjmodel.h>
|
|
#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;
|
|
}
|