From 5c0de1f84d401de88854cfc274a3ad34640a81eb Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 23 Mar 2026 04:51:02 -0700 Subject: [PATCH] Remove model and data from ccd object. Each geom gets a copy of pos and mat, which is editable without modifying mjData. PiperOrigin-RevId: 888028695 Change-Id: Ic9bbea4bcd7c2dbe62326798eab73d14da0aaa13 --- src/engine/engine_collision_convex.c | 325 ++++++++++++--------------- src/engine/engine_collision_convex.h | 48 +++- src/engine/engine_collision_gjk.c | 138 +++++------- 3 files changed, 240 insertions(+), 271 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index a232ba1f..f9c60231 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -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); diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 4f94b80c..d9212fbd 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -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; diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 40881cec..2d97eece 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -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; }