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

Remove model and data from ccd object. Each geom gets a copy of pos and mat, which is editable without modifying mjData.

Browse Source 
PiperOrigin-RevId: 888028695
Change-Id: Ic9bbea4bcd7c2dbe62326798eab73d14da0aaa13
This commit is contained in:
Kyle Bayes
2026-03-23 04:51:02 -07:00
committed by Copybara-Service
parent babcfd6575
commit 5c0de1f84d
3 changed files with 240 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_convex.c
+145 -180
View File
@@ -51,11 +51,10 @@ static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) {
}
// wrapper around libccd; returns number of collisions found
static inline int _libccd_wrapper(mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con,
mjtNum margin) {
static inline int _libccd_wrapper(const mjModel* m, mjCCDObj* obj1, mjCCDObj* obj2,
mjContact* con, mjtNum margin) {
ccd_t ccd;
CCD_INIT(&ccd);
const mjModel* m = obj1->model;
ccd.mpr_tolerance = m->opt.ccd_tolerance;
ccd.epa_tolerance = m->opt.ccd_tolerance; // use MPR tolerance for EPA
ccd.max_iterations = m->opt.ccd_iterations;
@@ -87,11 +86,10 @@ static inline int _libccd_wrapper(mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con
// find penetration info between two geoms; returns number of collisions found
static int mjc_penetration(mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con, int ncon,
mjtNum margin) {
const mjModel* m = obj1->model;
static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj* obj2,
mjContact* con, int ncon, mjtNum margin) {
if (mjDISABLED(mjDSBL_NATIVECCD)) {
return _libccd_wrapper(obj1, obj2, con, margin);
return _libccd_wrapper(m, obj1, obj2, con, margin);
}
// nativeccd
@@ -104,7 +102,7 @@ static int mjc_penetration(mjCCDObj* obj1, mjCCDObj* obj2, mjContact* con, int n
config.tolerance = m->opt.ccd_tolerance;
config.max_contacts = ncon;
config.dist_cutoff = 0; // no geom distances needed
config.context = (void*)obj1->data;
config.context = (void*)d;
config.alloc = ccd_allocate;
config.free = ccd_free;
@@ -140,7 +138,7 @@ void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
if (obj->geom_type == mjGEOM_HFIELD) {
mju_zero3(res);
for (int i=0; i < 6; i++) {
mji_addTo3(res, obj->prism[i]);
mji_addTo3(res, obj->data.hfield.prism[i]);
}
mju_scl3(res, res, 1.0/6.0);
return;
@@ -148,19 +146,19 @@ void mjc_center(mjtNum res[3], const mjCCDObj *obj) {
// return geom position
if (g >= 0) {
mji_copy3(res, obj->data->geom_xpos + 3*g);
mji_copy3(res, obj->pos);
return;
}
// return flex element position
if (e >= 0) {
mji_copy3(res, obj->data->flexelem_aabb + 6*(obj->model->flex_elemadr[f]+e));
mji_copy3(res, obj->data.flex.aabb + 6*(obj->data.flex.elemadr[f]+e));
return;
}
// return flex vertex position
if (f >= 0) {
mji_copy3(res, obj->data->flexvert_xpos + 3*(obj->model->flex_vertadr[f]+v));
mji_copy3(res, obj->data.flex.vert_xpos + 3*(obj->data.flex.vertadr[f]+v));
return;
}
}
@@ -191,19 +189,15 @@ static inline void localToGlobal(mjtNum res[3], const mjtNum mat[9], const mjtNu
// point support function
void mjc_pointSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjtNum* pos = obj->data->geom_xpos + 3*obj->geom;
mji_copy3(res, pos);
mji_copy3(res, obj->pos);
}
// sphere support function
static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// sphere data
const mjtNum* pos = d->geom_xpos + 3*obj->geom;
mjtNum radius = m->geom_size[3*obj->geom];
const mjtNum* pos = obj->pos;
mjtNum radius = obj->size[0];
res[0] = radius*dir[0] + pos[0];
res[1] = radius*dir[1] + pos[1];
@@ -213,14 +207,10 @@ static void mjc_sphereSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3])
// line support function (capsule)
void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// capsule data
int i = 3*obj->geom;
const mjtNum* mat = d->geom_xmat + 3*i;
const mjtNum* pos = d->geom_xpos + i;
mjtNum length = m->geom_size[i+1];
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
mjtNum length = obj->size[1];
mjtNum dot = mat[2]*dir[0] + mat[5]*dir[1] + mat[8]*dir[2];
mjtNum scl = dot >= 0 ? length : -length;
@@ -234,15 +224,11 @@ void mjc_lineSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
// capsule support function
static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// capsule data
int i = 3*obj->geom;
const mjtNum* mat = d->geom_xmat + 3*i;
const mjtNum* pos = d->geom_xpos + i;
mjtNum radius = m->geom_size[i];
mjtNum length = m->geom_size[i+1];
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
mjtNum radius = obj->size[0];
mjtNum length = obj->size[1];
// rotate dir to geom local frame
mjtNum local_dir[3], local_supp[3];
@@ -263,14 +249,10 @@ static void mjc_capsuleSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]
// ellipsoid support function
static void mjc_ellipsoidSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// ellipsoid data
int i = 3*obj->geom;
const mjtNum* mat = d->geom_xmat + 3*i;
const mjtNum* pos = d->geom_xpos + i;
const mjtNum* size = m->geom_size + i;
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
const mjtNum* size = obj->size;
// rotate dir to geom local frame
mjtNum local_dir[3], local_supp[3];
@@ -304,14 +286,10 @@ static void mjc_ellipsoidSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
// cylinder support function
static void mjc_cylinderSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// cylinder data
int i = 3*obj->geom;
const mjtNum* mat = d->geom_xmat + 3*i;
const mjtNum* pos = d->geom_xpos + i;
const mjtNum* size = m->geom_size + i;
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
const mjtNum* size = obj->size;
// rotate dir to geom local frame
mjtNum local_dir[3], local_supp[3];
@@ -332,14 +310,10 @@ static void mjc_cylinderSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3
// box support function
static void mjc_boxSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// box data
int i = 3*obj->geom;
const mjtNum* mat = d->geom_xmat + 3*i;
const mjtNum* pos = d->geom_xpos + i;
const mjtNum* size = m->geom_size + i;
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
const mjtNum* size = obj->size;
// rotate dir to geom local frame
mjtNum local_dir[3], local_supp[3];
@@ -368,15 +342,10 @@ static inline mjtNum dot3f(const mjtNum a[3], const float b[3]) {
// mesh support function via exhaustive search
static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// mesh data
int g = obj->geom;
const mjtNum* mat = d->geom_xmat+9*g;
const mjtNum* pos = d->geom_xpos+3*g;
float* verts = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
int nverts = m->mesh_vertnum[m->geom_dataid[g]];
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
const float* verts = obj->data.mesh.vert;
int nverts = obj->data.mesh.nvert;
mjtNum local_dir[3];
mulMatTVec3(local_dir, mat, dir);
@@ -415,19 +384,13 @@ static void mjc_meshSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
// mesh support function via hill climbing
static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
// get mesh info
int g = obj->geom;
int graphadr = m->mesh_graphadr[m->geom_dataid[g]];
int numvert = m->mesh_graph[graphadr];
int* vert_edgeadr = m->mesh_graph + graphadr + 2;
int* vert_globalid = m->mesh_graph + graphadr + 2 + numvert;
int* edge_localid = m->mesh_graph + graphadr + 2 + 2*numvert;
float* verts = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
const mjtNum* pos = d->geom_xpos + 3*g;
const mjtNum* mat = d->geom_xmat + 9*g;
int numvert = obj->data.mesh.graph[0];
const int* vert_edgeadr = obj->data.mesh.graph + 2;
const int* vert_globalid = obj->data.mesh.graph + 2 + numvert;
const int* edge_localid = obj->data.mesh.graph + 2 + 2*numvert;
const float* verts = obj->data.mesh.vert;
const mjtNum* pos = obj->pos;
const mjtNum* mat = obj->mat;
// rotate dir to geom local frame
mjtNum local_dir[3];
@@ -468,35 +431,34 @@ static void mjc_hillclimbSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[
static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
int istart, ibest;
mjtNum best, tmp;
mjtNum (*prism)[3] = obj->data.hfield.prism;
// find best vertex in halfspace determined by dir.z
istart = dir[2] < 0 ? 0 : 3;
ibest = istart;
best = mju_dot3(obj->prism[istart], dir);
for (int i=istart+1; i < istart+3; i++) {
if ((tmp = mju_dot3(obj->prism[i], dir)) > best) {
ibest = i;
best = mju_dot3(prism[istart], dir);
for (int i=1; i < 3; i++) {
if ((tmp = mju_dot3(prism[istart + i], dir)) > best) {
ibest = istart + i;
best = tmp;
}
}
// copy best point
mji_copy3(res, obj->prism[ibest]);
mji_copy3(res, prism[ibest]);
}
// flex support function
static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
int f = obj->flex;
int dim = m->flex_dim[f];
int dim = obj->data.flex.dim[f];
// flex element
if (obj->elem >= 0) {
int e = obj->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];
const int* edata = obj->data.flex.elem + obj->data.flex.elemdataadr[f] + e*(dim+1);
const mjtNum* vert = obj->data.flex.vert_xpos + 3*obj->data.flex.vertadr[f];
// find element vertex with largest projection along dir
mji_copy3(res, vert+3*edata[0]);
@@ -512,14 +474,14 @@ static void mjc_flexSupport(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) {
}
// add radius and margin/2
mji_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin);
mji_addToScl3(res, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
return;
}
// flex vertex
else {
const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert);
mji_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin);
const mjtNum* vert = obj->data.flex.vert_xpos + 3*(obj->data.flex.vertadr[f] + obj->vert);
mji_addScl3(res, vert, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
return;
}
}
@@ -530,19 +492,17 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
mjCCDObj *obj = (mjCCDObj *)_obj;
mjtNum *res = vec->v;
const mjtNum *dir = _dir->v;
const mjModel* m = obj->model;
const mjData* d = obj->data;
int g = obj->geom;
if (g < 0) {
int f = obj->flex;
int dim = m->flex_dim[f];
int dim = obj->data.flex.dim[f];
// flex element
if (obj->elem >= 0) {
int e = obj->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];
const int* edata = obj->data.flex.elem + obj->data.flex.elemdataadr[f] + e*(dim+1);
const mjtNum* vert = obj->data.flex.vert_xpos + 3*obj->data.flex.vertadr[f];
// find element vertex with largest projection along dir
mji_copy3(res, vert+3*edata[0]);
@@ -558,28 +518,27 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
}
// add radius and margin/2
mji_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->margin);
mji_addToScl3(res, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
return;
}
// flex vertex
else {
const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert);
mji_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin);
const mjtNum* vert = obj->data.flex.vert_xpos + 3*(obj->data.flex.vertadr[f] + obj->vert);
mji_addScl3(res, vert, dir, obj->data.flex.xradius[f] + 0.5*obj->margin);
return;
}
}
float* vertdata;
int ibest, graphadr, numvert, change, locid;
int *vert_edgeadr, *vert_globalid, *edge_localid;
const float* vertdata;
int ibest, numvert, change, locid;
mjtNum tmp, vdot;
const mjtNum* size = m->geom_size+3*g; // geom sizes
const mjtNum* size = obj->size; // geom sizes
mjtNum local_dir[3]; // direction in geom local frame
// rotate dir to geom local frame
mju_mulMatTVec3(local_dir, d->geom_xmat+9*g, dir);
mju_mulMatTVec3(local_dir, obj->mat, dir);
// compute result according to geom type
switch ((mjtGeom) obj->geom_type) {
@@ -631,14 +590,14 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
case mjGEOM_MESH:
case mjGEOM_SDF:
// init search
vertdata = m->mesh_vert + 3*m->mesh_vertadr[m->geom_dataid[g]];
vertdata = obj->data.mesh.vert;
tmp = -1E+10;
ibest = -1;
// no graph data: exhaustive search
if (m->mesh_graphadr[m->geom_dataid[g]] < 0) {
if (obj->data.mesh.graph == NULL) {
// search all vertices, find best
for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) {
for (int i=0; i < obj->data.mesh.nvert; i++) {
// vdot = dot(vertex, dir)
vdot = local_dir[0] * (mjtNum)vertdata[3*i] +
local_dir[1] * (mjtNum)vertdata[3*i+1] +
@@ -658,11 +617,10 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
// 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;
numvert = obj->data.mesh.graph[0];
const int* vert_edgeadr = obj->data.mesh.graph + 2;
const int* vert_globalid = obj->data.mesh.graph + 2 + numvert;
const int* edge_localid = obj->data.mesh.graph + 2 + 2*numvert;
// init with first vertex in convex hull or warmstart
ibest = obj->meshindex < 0 ? 0 : obj->meshindex;
@@ -720,7 +678,7 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
return;
default:
mjERROR("ccd support function is undefined for geom type %d", m->geom_type[g]);
mjERROR("ccd support function is undefined for geom type %d", obj->geom_type);
}
// add local_dir*margin/2 to result
@@ -729,18 +687,17 @@ void mjccd_support(const void *_obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
}
// rotate result to global frame
mju_mulMatVec3(res, d->geom_xmat+9*g, res);
mju_mulMatVec3(res, obj->mat, res);
// add geom position
mji_addTo3(res, d->geom_xpos+3*g);
mji_addTo3(res, obj->pos);
}
// ------------------------------------------------------------------------------------------------
// initialize a CCD object
void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjtNum margin) {
obj->model = m;
obj->data = d;
int graphadr, vertadr, polyadr;
obj->geom = g;
obj->margin = margin;
obj->center = mjc_center;
@@ -752,6 +709,9 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
mju_zero4(obj->rotate);
obj->rotate[0] = 1;
if (g >= 0) {
mju_copy(obj->size, m->geom_size+3*g, 3);
mju_copy(obj->pos, d->geom_xpos+3*g, 3);
mju_copy(obj->mat, d->geom_xmat+9*g, 9);
obj->geom_type = m->geom_type[g];
switch ((mjtGeom) obj->geom_type) {
case mjGEOM_ELLIPSOID:
@@ -759,12 +719,26 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
break;
case mjGEOM_MESH:
case mjGEOM_SDF:
if (m->mesh_graphadr[m->geom_dataid[g]] < 0 ||
m->mesh_vertnum[m->geom_dataid[g]] < mjMESH_HILLCLIMB_MIN) {
graphadr = m->mesh_graphadr[m->geom_dataid[g]];
vertadr = m->mesh_vertadr[m->geom_dataid[g]];
polyadr = m->mesh_polyadr[m->geom_dataid[g]];
if (graphadr < 0 || m->mesh_vertnum[m->geom_dataid[g]] < mjMESH_HILLCLIMB_MIN) {
obj->data.mesh.graph = NULL;
obj->support = mjc_meshSupport;
} else {
obj->data.mesh.graph = m->mesh_graph + graphadr;
obj->support = mjc_hillclimbSupport;
}
obj->data.mesh.vert = m->mesh_vert + 3*vertadr;
obj->data.mesh.nvert = m->mesh_vertnum[m->geom_dataid[g]];
obj->data.mesh.mpolymapadr = m->mesh_polymapadr + vertadr;
obj->data.mesh.mpolymapnum = m->mesh_polymapnum + vertadr;
obj->data.mesh.polymap = m->mesh_polymap;
obj->data.mesh.polynormal = m->mesh_polynormal + 3*polyadr;
obj->data.mesh.polyvertadr = m->mesh_polyvertadr + polyadr;
obj->data.mesh.polyvertnum = m->mesh_polyvertnum + polyadr;
obj->data.mesh.polyvert = m->mesh_polyvert;
obj->data.mesh.mesh_polynum = m->mesh_polynum[m->geom_dataid[g]];
break;
case mjGEOM_SPHERE:
obj->support = mjc_sphereSupport;
@@ -783,10 +757,10 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
obj->support = mjc_prism_support;
int hid = m->geom_dataid[g];
obj->hfield_nrow = m->hfield_nrow[hid];
obj->hfield_ncol = m->hfield_ncol[hid];
obj->size = m->hfield_size + 4*hid;
obj->hfield_data = m->hfield_data + m->hfield_adr[hid];
obj->data.hfield.hfield_nrow = m->hfield_nrow[hid];
obj->data.hfield.hfield_ncol = m->hfield_ncol[hid];
mju_copy(obj->size, m->hfield_size + 4*hid, 4);
obj->data.hfield.hfield_data = m->hfield_data + m->hfield_adr[hid];
break;
default:
obj->support = NULL;
@@ -794,7 +768,15 @@ void mjc_initCCDObj(mjCCDObj* obj, const mjModel* m, const mjData* d, int g, mjt
}
} else {
obj->geom_type = mjGEOM_FLEX;
obj->data.flex.dim = m->flex_dim;
obj->support = mjc_flexSupport;
obj->data.flex.aabb = d->flexelem_aabb;
obj->data.flex.elemadr = m->flex_elemadr;
obj->data.flex.vert_xpos = d->flexvert_xpos;
obj->data.flex.vertadr = m->flex_vertadr;
obj->data.flex.xradius = m->flex_radius;
obj->data.flex.elemdataadr = m->flex_elemdataadr;
obj->data.flex.elem = m->flex_elem;
}
}
@@ -840,17 +822,15 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
// return number of contacts supported by a single pass of narrowphase
static int maxContacts(const mjCCDObj* obj1, const mjCCDObj* obj2) {
const mjModel* m = obj1->model;
static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* obj2) {
// single pass not supported for margins
if (obj1->margin > 0 || obj2->margin > 0) {
return 1;
}
// can return 8 contacts for box-box collision in one pass
int type1 = m->geom_type[obj1->geom];
int type2 = m->geom_type[obj2->geom];
int type1 = obj1->geom_type;
int type2 = obj2->geom_type;
if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) {
return 8;
}
@@ -873,10 +853,10 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
mjCCDObj obj1, obj2;
mjc_initCCDObj(&obj1, m, d, g1, margin);
mjc_initCCDObj(&obj2, m, d, g2, margin);
int max_contacts = maxContacts(&obj1, &obj2);
int max_contacts = maxContacts(m, &obj1, &obj2);
// find initial contact
int ncon = mjc_penetration(&obj1, &obj2, con, max_contacts, margin);
int ncon = mjc_penetration(m, d, &obj1, &obj2, con, max_contacts, margin);
if (mjDISABLED(mjDSBL_NATIVECCD) && ncon && g1 >= 0 && g2 >= 0) {
mjc_fixNormal(m, d, con, g1, g2);
}
@@ -894,13 +874,6 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
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];
mji_copy3(xpos1, d->geom_xpos+3*g1);
mji_copy9(xmat1, d->geom_xmat+9*g1);
mji_copy3(xpos2, d->geom_xpos+3*g2);
mji_copy9(xmat2, d->geom_xmat+9*g2);
// complete frame of initial contact
mjtNum frame[9];
mji_copy9(frame, con[0].frame);
@@ -925,15 +898,15 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
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);
mju_rotateFrame(con[0].pos, rot, obj1.mat, obj1.pos);
// 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);
mju_rotateFrame(con[0].pos, invrot, obj2.mat, obj2.pos);
// search for new contact
int n = mjc_penetration(&obj1, &obj2, con + ncon, 1, margin);
int n = mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, margin);
if (mjDISABLED(mjDSBL_NATIVECCD) && n && g1 >= 0 && g2 >= 0) {
mjc_fixNormal(m, d, con + ncon, g1, g2);
}
@@ -947,10 +920,10 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN
}
// reset positions and orientations of g1 and g2
mji_copy3(d->geom_xpos+3*g1, xpos1);
mji_copy9(d->geom_xmat+9*g1, xmat1);
mji_copy3(d->geom_xpos+3*g2, xpos2);
mji_copy9(d->geom_xmat+9*g2, xmat2);
mji_copy3(obj1.pos, d->geom_xpos+3*g1);
mji_copy9(obj1.mat, d->geom_xmat+9*g1);
mji_copy3(obj2.pos, d->geom_xpos+3*g2);
mji_copy9(obj2.mat, d->geom_xmat+9*g2);
}
}
}
@@ -1101,35 +1074,35 @@ int mjc_PlaneConvex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2,
// add vertex to prism
static inline void addVert(mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) {
// move old data
mji_copy3(obj->prism[0], obj->prism[1]);
mji_copy3(obj->prism[1], obj->prism[2]);
mji_copy3(obj->prism[3], obj->prism[4]);
mji_copy3(obj->prism[4], obj->prism[5]);
mji_copy3(obj->data.hfield.prism[0], obj->data.hfield.prism[1]);
mji_copy3(obj->data.hfield.prism[1], obj->data.hfield.prism[2]);
mji_copy3(obj->data.hfield.prism[3], obj->data.hfield.prism[4]);
mji_copy3(obj->data.hfield.prism[4], obj->data.hfield.prism[5]);
// add new vertex at last position
obj->prism[2][0] = obj->prism[5][0] = x;
obj->prism[2][1] = obj->prism[5][1] = y;
obj->prism[5][2] = z;
obj->data.hfield.prism[2][0] = obj->data.hfield.prism[5][0] = x;
obj->data.hfield.prism[2][1] = obj->data.hfield.prism[5][1] = y;
obj->data.hfield.prism[5][2] = z;
}
// add vertex to prism
static inline void addPrismVert(mjCCDObj* obj, int r, int c, int i, mjtNum dx, mjtNum dy, mjtNum margin) {
// move old data
mji_copy3(obj->prism[0], obj->prism[1]);
mji_copy3(obj->prism[1], obj->prism[2]);
mji_copy3(obj->prism[3], obj->prism[4]);
mji_copy3(obj->prism[4], obj->prism[5]);
mji_copy3(obj->data.hfield.prism[0], obj->data.hfield.prism[1]);
mji_copy3(obj->data.hfield.prism[1], obj->data.hfield.prism[2]);
mji_copy3(obj->data.hfield.prism[3], obj->data.hfield.prism[4]);
mji_copy3(obj->data.hfield.prism[4], obj->data.hfield.prism[5]);
int dr = 1 - i;
// add new vertex at last position
obj->prism[2][0] = obj->prism[5][0] = dx*c - obj->size[0];
obj->prism[2][1] = obj->prism[5][1] = dy*(r + dr) - obj->size[1];
obj->prism[5][2] = obj->hfield_data[(r + dr)*obj->hfield_ncol + c]*obj->size[2];
obj->data.hfield.prism[2][0] = obj->data.hfield.prism[5][0] = dx*c - obj->size[0];
obj->data.hfield.prism[2][1] = obj->data.hfield.prism[5][1] = dy*(r + dr) - obj->size[1];
obj->data.hfield.prism[5][2] = obj->data.hfield.hfield_data[(r + dr)*obj->data.hfield.hfield_ncol + c]*obj->size[2];
// factor in margin
obj->prism[5][2] += margin;
obj->data.hfield.prism[5][2] += margin;
}
@@ -1178,12 +1151,8 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
mjtNum mat[9];
mji_mulMatTMat3(mat, mat1, mat2);
// save mat2 and pos2, replace with relative frame
mjtNum savemat2[9], savepos2[3];
mji_copy9(savemat2, mat2);
mji_copy3(savepos2, pos2);
mji_copy9(mat2, mat);
mji_copy3(pos2, pos);
mji_copy9(obj2.mat, mat);
mji_copy3(obj2.pos, pos);
mjtNum dir[3] = {0, 0, 0}, res[3];
@@ -1225,8 +1194,6 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
if ((xmin - margin > size0) || (xmax + margin < -size0) ||
(ymin - margin > size1) || (ymax + margin < -size1) ||
(zmin - margin > size2) || (zmax + margin < -size3)) {
mji_copy9(mat2, savemat2);
mji_copy3(pos2, savepos2);
return 0;
}
@@ -1248,7 +1215,8 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
mjtNum dy = (2.0*size1) / (nrow-1);
// set zbottom value using base size
obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -size3;
mjtNum (*prism)[3] = obj1.data.hfield.prism;
prism[0][2] = prism[1][2] = prism[2][2] = -size3;
// process all prisms in subgrid
int ncon = 0;
@@ -1261,12 +1229,12 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
addPrismVert(&obj1, r, c, i, dx, dy, margin);
// prism height test
if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin && obj1.prism[5][2] < zmin) {
if (prism[3][2] < zmin && prism[4][2] < zmin && prism[5][2] < zmin) {
continue;
}
// run penetration function, save contact
if (mjc_penetration(&obj1, &obj2, con + ncon, 1, 0.0)) {
if (mjc_penetration(m, d, &obj1, &obj2, con + ncon, 1, 0.0)) {
// transform to global coordinates
mji_copy3(dir, con[ncon].frame);
mji_copy3(pos, con[ncon].pos);
@@ -1286,10 +1254,6 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjContact* con, int g1, int g2
}
}
// restore mat2 and pos2
mji_copy9(mat2, savemat2);
mji_copy3(pos2, savepos2);
if (mjDISABLED(mjDSBL_NATIVECCD)) {
// fix contact normals
for (int i=0; i < ncon; i++) {
@@ -1571,7 +1535,7 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1,
mjc_setCCDObjFlex(&obj2, f2, e2, -1);
// find contacts
int ncon = mjc_penetration(&obj1, &obj2, con, 1, margin);
int ncon = mjc_penetration(m, d, &obj1, &obj2, con, 1, margin);
// fix normals for 2D flex
if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) {
@@ -1605,9 +1569,9 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in
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 mjtNum* hpos = d->geom_xpos + 3*g;
const mjtNum* hmat = d->geom_xmat + 9*g;
const mjtNum* hsize = m->hfield_size + 4*hid;
const float* hdata = m->hfield_data + m->hfield_adr[hid];
// get elem indo
@@ -1684,7 +1648,8 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in
dr[1] = 0;
// set zbottom value using base size
obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -hsize[3];
mjtNum (*prism)[3] = obj1.data.hfield.prism;
prism[0][2] = prism[1][2] = prism[2][2] = -hsize[3];
// process all prisms in sub-grid
cnt = 0;
@@ -1699,12 +1664,12 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in
// check for enough vertices
if (++nvert > 2) {
// prism height test
if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin && obj1.prism[5][2] < zmin) {
if (prism[3][2] < zmin && prism[4][2] < zmin && prism[5][2] < zmin) {
continue;
}
// run ccd, save contact
if (mjc_penetration(&obj1, &obj2, con + cnt, 1, 0.0)) {
if (mjc_penetration(m, d, &obj1, &obj2, con + cnt, 1, 0.0)) {
// transform to global coordinates
mjtNum dir[3], pos[3];
mji_copy3(dir, con[cnt].frame);
src/engine/engine_collision_convex.h
+39 -9
View File
@@ -37,10 +37,11 @@ extern "C" {
// internal object type for convex collision detection
struct _mjCCDObj {
const mjModel* model;
const mjData* data;
int geom;
int geom_type;
mjtNum size[4];
mjtNum pos[3];
mjtNum mat[9];
int vertindex;
int meshindex;
int flex;
@@ -48,15 +49,44 @@ struct _mjCCDObj {
int vert;
mjtNum margin;
mjtNum rotate[4];
union {
// mesh data from mjModel
struct {
int nvert;
int mesh_polynum;
const float* vert;
const int* mpolymapadr;
const int* mpolymapnum;
const int* polymap;
const int* polyvertadr;
const int* polyvertnum;
const int* polyvert;
const mjtNum* polynormal;
const int*graph;
} mesh;
// hfield prism data
struct {
mjtNum prism[6][3];
const float* hfield_data;
int hfield_nrow;
int hfield_ncol;
} hfield;
// flex data from mjModel and mjData
struct {
const int* elem;
const int* dim;
const mjtNum* aabb;
const int* elemadr;
const int* elemdataadr;
const mjtNum* vert_xpos;
const int* vertadr;
const mjtNum* xradius;
} flex;
} data;
void (*center)(mjtNum res[3], const struct _mjCCDObj* obj);
void (*support)(mjtNum res[3], struct _mjCCDObj* obj, const mjtNum dir[3]);
// for hfield
mjtNum prism[6][3];
const mjtNum* size;
const float* hfield_data;
int hfield_nrow;
int hfield_ncol;
};
typedef struct _mjCCDObj mjCCDObj;
src/engine/engine_collision_gjk.c
+56 -82
View File
@@ -1710,65 +1710,61 @@ static int intersect(int res[2], const int* arr1, const int* arr2, int n, int m)
// compute possible polygon normals of a mesh given up to 3 vertices
static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj,
int v1, int v2, int v3) {
const mjModel* m = obj->model;
const mjData* d = obj->data;
int g = obj->geom;
int polyadr = m->mesh_polyadr[m->geom_dataid[g]];
int vertadr = m->mesh_vertadr[m->geom_dataid[g]];
const mjtNum* mat = d->geom_xmat + 9*g;
const int* polymap = obj->data.mesh.polymap;
const mjtNum* polynormal = obj->data.mesh.polynormal;
if (dim == 3) {
int v1_adr = m->mesh_polymapadr[vertadr + v1];
int v1_num = m->mesh_polymapnum[vertadr + v1];
int v1_adr = obj->data.mesh.mpolymapadr[v1];
int v1_num = obj->data.mesh.mpolymapnum[v1];
int v2_adr = m->mesh_polymapadr[vertadr + v2];
int v2_num = m->mesh_polymapnum[vertadr + v2];
int v2_adr = obj->data.mesh.mpolymapadr[v2];
int v2_num = obj->data.mesh.mpolymapnum[v2];
int v3_adr = m->mesh_polymapadr[vertadr + v3];
int v3_num = m->mesh_polymapnum[vertadr + v3];
int v3_adr = obj->data.mesh.mpolymapadr[v3];
int v3_num = obj->data.mesh.mpolymapnum[v3];
int edgeset[2], faceset[2];
int n = intersect(edgeset, m->mesh_polymap + v1_adr, m->mesh_polymap + v2_adr, v1_num, v2_num);
int n = intersect(edgeset, polymap + v1_adr, polymap + v2_adr, v1_num, v2_num);
if (n == 0) return 0;
n = intersect(faceset, edgeset, m->mesh_polymap + v3_adr, n, v3_num);
n = intersect(faceset, edgeset, polymap + v3_adr, n, v3_num);
if (n == 0) return 0;
// three vertices on mesh define a unique face
mjtNum* normal = m->mesh_polynormal + 3*(polyadr + faceset[0]);
globalcoord(res, mat, NULL, normal[0], normal[1], normal[2]);
const mjtNum* normal = polynormal + 3*faceset[0];
globalcoord(res, obj->mat, NULL, normal[0], normal[1], normal[2]);
resind[0] = faceset[0];
return 1;
}
if (dim == 2) {
int v1_adr = m->mesh_polymapadr[vertadr + v1];
int v1_num = m->mesh_polymapnum[vertadr + v1];
int v1_adr = obj->data.mesh.mpolymapadr[v1];
int v1_num = obj->data.mesh.mpolymapnum[v1];
int v2_adr = m->mesh_polymapadr[vertadr + v2];
int v2_num = m->mesh_polymapnum[vertadr + v2];
int v2_adr = obj->data.mesh.mpolymapadr[v2];
int v2_num = obj->data.mesh.mpolymapnum[v2];
// up to two faces as vertices on mesh define an edge
int edgeset[2];
int n = intersect(edgeset, m->mesh_polymap + v1_adr, m->mesh_polymap + v2_adr, v1_num, v2_num);
int n = intersect(edgeset, polymap + v1_adr, polymap + v2_adr, v1_num, v2_num);
if (n == 0) return 0;
for (int i = 0; i < n; i++) {
mjtNum* normal = m->mesh_polynormal + 3*(polyadr + edgeset[i]);
globalcoord(res + 3*i, mat, NULL, normal[0], normal[1], normal[2]);
const mjtNum* normal = polynormal + 3*edgeset[i];
globalcoord(res + 3*i, obj->mat, NULL, normal[0], normal[1], normal[2]);
resind[i] = edgeset[i];
}
return n;
}
if (dim == 1) {
int v1_adr = m->mesh_polymapadr[vertadr + v1];
int v1_num = m->mesh_polymapnum[vertadr + v1];
int v1_adr = obj->data.mesh.mpolymapadr[v1];
int v1_num = obj->data.mesh.mpolymapnum[v1];
// cap number of possible faces intersecting at a vertex
if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT;
for (int i = 0; i < v1_num; i++) {
int index = m->mesh_polymap[v1_adr + i];
mjtNum* normal = m->mesh_polynormal + 3*(polyadr + index);
globalcoord(res + 3*i, mat, NULL, normal[0], normal[1], normal[2]);
int index = polymap[v1_adr + i];
const mjtNum* normal = polynormal + 3*index;
globalcoord(res + 3*i, obj->mat, NULL, normal[0], normal[1], normal[2]);
resind[i] = index;
}
return v1_num;
@@ -1781,9 +1777,10 @@ static int meshNormals(mjtNum* res, int resind[3], int dim, mjCCDObj* obj,
static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj,
const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
// mesh data
int g = obj->geom;
const mjtNum* mat = obj->data->geom_xmat + 9*g;
const mjtNum* pos = obj->data->geom_xpos + 3*g;
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
const float* vert = obj->data.mesh.vert;
const int* polyvert = obj->data.mesh.polyvert;
// only one edge
if (dim == 2) {
@@ -1794,25 +1791,15 @@ static int meshEdgeNormals(mjtNum* res, mjtNum* endverts, int dim, mjCCDObj* obj
}
if (dim == 1) {
const mjModel* m = obj->model;
int polyadr = m->mesh_polyadr[m->geom_dataid[g]];
int vertadr = m->mesh_vertadr[m->geom_dataid[g]];
int v1_adr = m->mesh_polymapadr[vertadr + v1i];
int v1_num = m->mesh_polymapnum[vertadr + v1i];
int v1_adr = obj->data.mesh.mpolymapadr[v1i];
int v1_num = obj->data.mesh.mpolymapnum[v1i];
if (v1_num > mjMAX_POLYVERT) v1_num = mjMAX_POLYVERT;
const int* polymap = m->mesh_polymap + v1_adr;
const int* polyvert = m->mesh_polyvert;
const int* polyvertadr = m->mesh_polyvertadr + polyadr;
const int* polyvertnum = m->mesh_polyvertnum + polyadr;
const float* vert = m->mesh_vert + 3*vertadr;
// loop through all faces with vertex v1
for (int i = 0; i < v1_num; i++) {
int idx = polymap[i];
int adr = polyvertadr[idx];
int nvert = polyvertnum[idx];
int idx = obj->data.mesh.polymap[v1_adr + i];
int adr = obj->data.mesh.polyvertadr[idx];
int nvert = obj->data.mesh.polyvertnum[idx];
// find previous vertex in polygon to form edge
for (int j = 0; j < nvert; j++) {
if (polyvert[adr + j] == v1i) {
@@ -1860,7 +1847,7 @@ static int boxNormals2(mjtNum res[9], int resind[3], const mjtNum mat[9], const
// 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,
int v1, int v2, int v3, const mjtNum dir[3]) {
const mjtNum* mat = obj->data->geom_xmat + 9*obj->geom;
const mjtNum* mat = obj->mat;
if (dim == 3) {
int c = 0;
int x = ((v1 & 1) && (v2 & 1) && (v3 & 1)) - (!(v1 & 1) && !(v2 & 1) && !(v3 & 1));
@@ -1915,11 +1902,9 @@ static int boxNormals(mjtNum res[9], int resind[3], int dim, mjCCDObj* obj,
static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj* obj,
const mjtNum v1[3], const mjtNum v2[3], int v1i, int v2i) {
// 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;
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
const mjtNum* size = obj->size;
if (dim == 2) {
copy3(endverts, v2);
sub3(res, v2, v1);
@@ -1953,10 +1938,9 @@ static int boxEdgeNormals(mjtNum res[9], mjtNum endverts[9], int dim, mjCCDObj*
// 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;
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
const mjtNum* size = obj->size;
// compute global coordinates of the box face and face normal
switch (idx) {
@@ -2003,22 +1987,18 @@ static int boxFace(mjtNum res[12], mjCCDObj* obj, int idx) {
// recover mesh polygon from its index, return number of edges
static int meshFace(mjtNum* res, mjCCDObj* obj, int idx) {
const mjModel* m = obj->model;
const mjtNum* mat = obj->mat;
const mjtNum* pos = obj->pos;
// mesh data
int g = 3*obj->geom;
const mjtNum* mat = obj->data->geom_xmat + 3*g;
const mjtNum* pos = obj->data->geom_xpos + g;
int polyadr = m->mesh_polyadr[m->geom_dataid[obj->geom]];
int vertadr = m->mesh_vertadr[m->geom_dataid[obj->geom]];
const float* vert = m->mesh_vert + 3*vertadr;
const int* polyvert = m->mesh_polyvert;
int adr = m->mesh_polyvertadr[polyadr + idx], j = 0;
int nvert = m->mesh_polyvertnum[polyadr + idx];
if (nvert > mjMAX_POLYVERT) nvert = mjMAX_POLYVERT;
int adr = obj->data.mesh.polyvertadr[idx], j = 0;
int nvert = obj->data.mesh.polyvertnum[idx];
if (nvert > mjMAX_POLYVERT) {
nvert = mjMAX_POLYVERT;
}
const float* vert = obj->data.mesh.vert;
const int* polyvert = obj->data.mesh.polyvert + adr;
for (int i = nvert - 1; i >= 0; i--) {
const float* v = vert + 3*polyvert[adr + i];
const float* v = vert + 3*polyvert[i];
globalcoord(res + 3*j++, mat, pos, v[0], v[1], v[2]);
}
return nvert;
@@ -2078,12 +2058,10 @@ static inline int simplexDim(int* v1i, int* v2i, int* v3i, mjtNum** v1, mjtNum**
// recover multiple contacts from EPA polytope
static void multicontact(Polytope* pt, Face* face, mjCCDStatus* status,
mjCCDObj* obj1, mjCCDObj* obj2) {
const int* polynum = obj1->model->mesh_polynum;
const int* geom_dataid = obj1->model->geom_dataid;
if (obj1->geom_type == mjGEOM_MESH && !polynum[geom_dataid[obj1->geom]]) {
if (obj1->geom_type == mjGEOM_MESH && !obj1->data.mesh.mesh_polynum) {
return;
}
if (obj2->geom_type == mjGEOM_MESH && !polynum[geom_dataid[obj2->geom]]) {
if (obj2->geom_type == mjGEOM_MESH && !obj2->data.mesh.mesh_polynum) {
return;
}
@@ -2260,25 +2238,21 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m
mjtNum margin1 = obj1->margin, margin2 = obj2->margin;
if (obj1->geom_type == mjGEOM_SPHERE) {
const mjModel* m = obj1->model;
full_margin1 = m->geom_size[3*obj1->geom] + 0.5*margin1;
full_margin1 = obj1->size[0] + 0.5*margin1;
obj1->support = mjc_pointSupport;
obj1->margin = 0;
} else if (obj1->geom_type == mjGEOM_CAPSULE) {
const mjModel* m = obj1->model;
full_margin1 = m->geom_size[3*obj1->geom] + 0.5*margin1;
full_margin1 = obj1->size[0] + 0.5*margin1;
obj1->support = mjc_lineSupport;
obj1->margin = 0;
}
if (obj2->geom_type == mjGEOM_SPHERE) {
const mjModel* m = obj2->model;
full_margin2 = m->geom_size[3*obj2->geom] + 0.5*margin2;
full_margin2 = obj2->size[0] + 0.5*margin2;
obj2->support = mjc_pointSupport;
obj2->margin = 0;
} else if (obj2->geom_type == mjGEOM_CAPSULE) {
const mjModel* m = obj2->model;
full_margin2 = m->geom_size[3*obj2->geom] + 0.5*margin2;
full_margin2 = obj2->size[0] + 0.5*margin2;
obj2->support = mjc_lineSupport;
obj2->margin = 0;
}