diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 30dd4b08..6fe17b4e 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -1532,7 +1532,7 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjPreContact* con, int g1, //---------------------------- flex collisions --------------------------------------------- // geom-elem or elem-elem or vert-elem convex collision using ccd -int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1, +int mjc_ConvexElem(const mjModel* m, mjData* d, mjPreContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, margin); @@ -1541,8 +1541,7 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, mjc_setCCDObjFlex(&obj2, f2, e2, -1); // find contacts - mjPreContact precon[8]; - int ncon = mjc_penetration(m, d, &obj1, &obj2, precon, 1, margin); + int ncon = mjc_penetration(m, d, &obj1, &obj2, con, 1, margin); // fix normals for 2D flex if (ncon && !mjDISABLED(mjDSBL_NATIVECCD)) { @@ -1553,25 +1552,16 @@ int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, if (isflex2d) { for (int i = 0; i < ncon; i++) { - mjc_fixNormal(m, d, precon + i, g1, -1); + mjc_fixNormal(m, d, con + i, g1, -1); } } } - - // copy to con - for (int i = 0; i < ncon; i++) { - con[i].dist = precon[i].dist; - mji_copy3(con[i].pos, precon[i].pos); - mji_copy3(con[i].frame, precon[i].normal); - mju_zero3(con[i].frame + 3); - } - return ncon; } // test a height field and a flex element for collision -int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e, +int mjc_HFieldElem(const mjModel* m, mjData* d, mjPreContact* con, int g, int f, int e, mjtNum margin) { mjtNum vec[3], dx, dy; mjtNum xmin, xmax, ymin, ymax, zmin, zmax; @@ -1684,14 +1674,12 @@ int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, in } // run ccd, save contact - mjContact* ccon = con + cnt; - mjPreContact precon; - if (mjc_penetration(m, d, &obj1, &obj2, &precon, 1, 0.0)) { + if (mjc_penetration(m, d, &obj1, &obj2, con + cnt, 1, 0.0)) { // transform to global coordinates - mji_zero3(ccon->frame + 3); - mji_mulMatVec3(ccon->frame, hmat, precon.normal); - mji_mulMatVec3(ccon->pos, hmat, precon.pos); - mji_addTo3(ccon->pos, hpos); + mji_zero3(con[cnt].tangent); + mju_mulMatVec3(con[cnt].normal, hmat, con[cnt].normal); + mju_mulMatVec3(con[cnt].pos, hmat, con[cnt].pos); + mji_addTo3(con[cnt].pos, hpos); // count, stop if max number reached cnt++; diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 969151cc..9a653172 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -114,11 +114,11 @@ int mjc_ConvexHField(const mjModel* m, mjData* d, mjPreContact* con, int g1, int MJAPI int mjc_Convex(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // geom-elem or elem-elem or vert-elem collision function using ccd -int mjc_ConvexElem(const mjModel* m, mjData* d, mjContact* con, int g1, int f1, int e1, int v1, +int mjc_ConvexElem(const mjModel* m, mjData* d, mjPreContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin); // heightfield-elem collision function using ccd -int mjc_HFieldElem(const mjModel* m, mjData* d, mjContact* con, int g, int f, int e, +int mjc_HFieldElem(const mjModel* m, mjData* d, mjPreContact* con, int g, int f, int e, mjtNum margin); // fix contact frame normal diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 5ce39f72..dc8f8da8 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -350,6 +350,13 @@ int mj_isElemActive(const mjModel* m, int f, int e) { //----------------------------- collision detection entry point ------------------------------------ +// internal structure for storing pairs meant for narrowphase collision detection +typedef struct { + int g1; + int g2; + int ipair; +} mjcPair; + // binary search between two bodyflex trees static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, int merged, int startadr, int pairadr); @@ -357,7 +364,7 @@ static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, // compute contacts for a batch of collision pairs contained in a buffer of // stride 3 ints (g1, g2, ipair) // if buffer is NULL, results are read from arena starting at parena -static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, +static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, int npair, size_t parena); // test a plane geom and a flex for collision, add to contact list @@ -481,19 +488,19 @@ static void filterFlexContacts(mjData* d, int ncon_before) { // push a candidate collision pair onto the arena static void pushPairArena(const mjModel* m, mjData* d, int g1, int g2, int ipair) { // allocate geom pair on the arena - int* pair = (int*) mj_arenaAllocByte(d, 3 * sizeof(int), _Alignof(int)); + mjcPair* pair = (mjcPair*) mj_arenaAllocByte(d, sizeof(mjcPair), _Alignof(mjcPair)); if (!pair) { mjERROR("arena too small to allocate geom pair"); } - if (m->geom_type[g1] > m->geom_type[g2]) { - pair[0] = g2; - pair[1] = g1; + if (g1 >= 0 && g2 >= 0 && m->geom_type[g1] > m->geom_type[g2]) { + pair->g1 = g2; + pair->g2 = g1; } else { - pair[0] = g1; - pair[1] = g2; + pair->g1 = g1; + pair->g2 = g2; } - pair[2] = ipair; + pair->ipair = ipair; } @@ -1040,8 +1047,11 @@ static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, n1 = nodeid2; n2 = nodeid1; } - int pair[3] = {n1, n2, -1}; - mj_narrowphase(m, d, pair, 1, 0); + mjcPair pair; + pair.g1 = n1; + pair.g2 = n2; + pair.ipair = -1; + mj_narrowphase(m, d, &pair, 1, 0); } if (mark_active) { d->bvh_active[node1 + bvhadr1] = true; @@ -1874,7 +1884,7 @@ static void collisionTask(const mjModel* m, mjData* d, void* arg, int thread_id, // compute contacts for a batch of collision pairs contained in a buffer of // stride 3 ints (g1, g2, ipair) // if buffer is NULL, results are read from arena starting at parena -static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, +static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, int npair, size_t parena) { int nthread = mju_numThread(d); int ccd_size = mjc_ccdSize(m->opt.ccd_iterations); @@ -1887,7 +1897,7 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int n // set buffer and arena pointer if (!buffer) { - buffer = (const int*) ((char*) d->arena + parena); + buffer = (const mjcPair*) ((char*) d->arena + parena); } else { parena = d->parena; } @@ -1898,9 +1908,9 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int n int* pairbuffer = mj_stackAllocInt(d, 4 * npair); int maxcon = 0; for (int i = 0; i < npair; i++) { - int g1 = buffer[3*i + 0]; - int g2 = buffer[3*i + 1]; - int ipair = buffer[3*i + 2]; + int g1 = buffer[i].g1; + int g2 = buffer[i].g2; + int ipair = buffer[i].ipair; pairbuffer[4*i + 0] = g1; pairbuffer[4*i + 1] = g2; @@ -2313,13 +2323,9 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { } } - // allocate mjContact[mjMAXCONPAIR] on the arena - mjContact* con = - (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); - if (!con) { - mj_warning(d, mjWARN_CONTACTFULL, d->ncon); - return; - } + // allocate mjPreContact[mjMAXCONPAIR] on the stack + mj_markStack(d); + mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact); // sphere/capsule/box : capsule if (dim == 1 && (type == mjGEOM_SPHERE || type == mjGEOM_CAPSULE || type == mjGEOM_BOX)) { @@ -2329,7 +2335,6 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { pos, mat, size); // call raw primitive for corresponding geom type - mjPreContact precon[2]; switch (type) { case mjGEOM_SPHERE: num = mjraw_SphereCapsule(precon, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g, @@ -2346,29 +2351,17 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { default: num = 0; } - - for (int i=0; i < num; i++) { - con[i].dist = precon[i].dist; - mju_copy3(con[i].pos, precon[i].pos); - mju_copy3(con[i].frame + 0, precon[i].normal); - mju_copy3(con[i].frame + 3, precon[i].tangent); - - // reverse contact normals, since box geom is second - if (type == mjGEOM_BOX) { - mju_scl3(con[i].frame, con[i].frame, -1); - } - } } // heightfield : elem else if (type == mjGEOM_HFIELD) { - num = mjc_HFieldElem(m, d, con, g, f, e, margin + gap); + num = mjc_HFieldElem(m, d, precon, g, f, e, margin + gap); } // sphere : triangle else if (type == mjGEOM_SPHERE && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; - num = mjraw_SphereTriangle(con, margin + gap, + num = mjraw_SphereTriangle(precon, margin + gap, d->geom_xpos+3*g, m->geom_size[3*g], vertxpos + 3*edata[0], vertxpos + 3*edata[1], vertxpos + 3*edata[2], m->flex_radius[f]); @@ -2377,7 +2370,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // box : triangle else if (type == mjGEOM_BOX && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f]; - num = mjraw_BoxTriangle(con, margin + gap, d->geom_xpos + 3 * g, + num = mjraw_BoxTriangle(precon, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1], vertxpos + 3 * edata[2], m->flex_radius[f]); @@ -2387,19 +2380,19 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { else if (type == mjGEOM_CAPSULE && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f]; num = mjraw_CapsuleTriangle( - con, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, + precon, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1], vertxpos + 3 * edata[2], m->flex_radius[f]); } // general geom : elem else { - num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin + gap); + num = mjc_ConvexElem(m, d, precon, g, -1, -1, -1, f, e, margin + gap); } // check contacts if (!num) { - resetArena(d); + mj_freeStack(d); return; } @@ -2409,6 +2402,15 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { mjtNum solreffriction[mjNREF] = {0}; mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); + // allocate mjContact[num] on the arena + mjContact* con = + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact)); + if (!con) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + mj_freeStack(d); + return; + } + // add contacts for (int i=0; i < num; i++) { // set contact ids @@ -2421,15 +2423,23 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { con[i].vert[0] = -1; con[i].vert[1] = -1; + con[i].dist = precon[i].dist; + mju_copy3(con[i].pos, precon[i].pos); + mju_copy3(con[i].frame + 0, precon[i].normal); + mju_copy3(con[i].frame + 3, precon[i].tangent); + + // reverse contact normals, since box geom is second + if (dim == 1 && type == mjGEOM_BOX) { + mju_scl3(con[i].frame, con[i].frame, -1); + } + // set remaining contact parameters mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon d->ncon += num; - - // move arena pointer back to the end of the contact array - resetArena(d); + mj_freeStack(d); } @@ -2466,13 +2476,9 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 } } - // allocate mjContact[mjMAXCONPAIR] on the arena - mjContact* con = - (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); - if (!con) { - mj_warning(d, mjWARN_CONTACTFULL, d->ncon); - return; - } + // allocate mjPreContact[mjMAXCONPAIR] on the stack + mj_markStack(d); + mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact); // capsule : capsule if (dim1 == 1 && dim2 == 1) { @@ -2485,24 +2491,17 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 pos2, mat2, size2); // raw primitive - mjPreContact precon[mjMAXCONPAIR]; num = mjraw_CapsuleCapsule(precon, margin + gap, pos1, mat1, size1, pos2, mat2, size2); - for (int i=0; i < num; i++) { - con[i].dist = precon[i].dist; - mju_copy3(con[i].pos, precon[i].pos); - mju_copy3(con[i].frame + 0, precon[i].normal); - mju_copy3(con[i].frame + 3, precon[i].tangent); - } } // general convex collision else { - num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin + gap); + num = mjc_ConvexElem(m, d, precon, -1, f1, e1, -1, f2, e2, margin + gap); } // check contacts if (!num) { - resetArena(d); + mj_freeStack(d); return; } @@ -2512,6 +2511,13 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 mjtNum solreffriction[mjNREF] = {0}; mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f1, f2); + mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact)); + if (!con) { + mj_freeStack(d); + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + return; + } + // add contacts for (int i=0; i < num; i++) { // set contact ids @@ -2524,15 +2530,18 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 con[i].vert[0] = -1; con[i].vert[1] = -1; + con[i].dist = precon[i].dist; + mju_copy3(con[i].pos, precon[i].pos); + mju_copy3(con[i].frame + 0, precon[i].normal); + mju_copy3(con[i].frame + 3, precon[i].tangent); + // set remaining contact parameters mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon d->ncon += num; - - // move arena pointer back to the end of the contact array - resetArena(d); + mj_freeStack(d); } @@ -2555,43 +2564,34 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { if (aabb[1]+aabb[4] < vert[1]-rbound) return; if (aabb[2]+aabb[5] < vert[2]-rbound) return; - // allocate mjContact[mjMAXCONPAIR] on the arena - mjContact* con = - (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); - if (!con) { - mj_warning(d, mjWARN_CONTACTFULL, d->ncon); - return; - } + // allocate mjPreContact[mjMAXCONPAIR] on the stack + mj_markStack(d); + mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact); // sphere : capsule if (dim == 1) { mjtNum pos[3], mat[9], size[2]; mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; mj_makeCapsule(m, d, f, edata, pos, mat, size); - mjPreContact precon; - num = mjraw_SphereCapsule(&precon, 0, vert, I, &radius, pos, mat, size); - con->dist = precon.dist; - mju_copy3(con->pos, precon.pos); - mju_copy3(con->frame + 0, precon.normal); - mju_copy3(con->frame + 3, precon.tangent); + num = mjraw_SphereCapsule(precon, 0, vert, I, &radius, pos, mat, size); } // sphere : triangle else if (dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; - num = mjraw_SphereTriangle(con, 0, vert, radius, + num = mjraw_SphereTriangle(precon, 0, vert, radius, vertxpos + 3*edata[0], vertxpos + 3*edata[1], vertxpos + 3*edata[2], radius); } // sphere : tetrahdron else { - num = mjc_ConvexElem(m, d, con, -1, f, -1, v, f, e, 0); + num = mjc_ConvexElem(m, d, precon, -1, f, -1, v, f, e, 0); } // check contacts if (!num) { - resetArena(d); + mj_freeStack(d); return; } @@ -2601,6 +2601,17 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { mjtNum solreffriction[mjNREF] = {0}; mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f); + + // allocate mjContact[num] on the arena + mjContact* con = + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * num, _Alignof(mjContact)); + if (!con) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + mj_freeStack(d); + return; + } + + // add contacts for (int i=0; i < num; i++) { // set contact ids @@ -2613,6 +2624,11 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { con[i].vert[0] = v; con[i].vert[1] = -1; + con[i].dist = precon[i].dist; + mju_copy3(con[i].pos, precon[i].pos); + mju_copy3(con[i].frame + 0, precon[i].normal); + mju_copy3(con[i].frame + 3, precon[i].tangent); + // set remaining contact parameters mj_setContact(m, con + i, condim, 0, solref, solreffriction, solimp, friction); } @@ -2620,6 +2636,5 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { // add to ncon d->ncon += num; - // move arena pointer back to the end of the contact array - resetArena(d); + mj_freeStack(d); } diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index a8823008..314de4e6 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -563,7 +563,7 @@ static mjtNum pointSegment(mjtNum res[2], const mjtNum p[2], // sphere : triangle with radius -int mjraw_SphereTriangle(mjContact* con, mjtNum margin, +int mjraw_SphereTriangle(mjPreContact* con, mjtNum margin, const mjtNum* s, mjtNum rs, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt) { mjtNum rbound = margin + rs + rt; @@ -642,15 +642,15 @@ int mjraw_SphereTriangle(mjContact* con, mjtNum margin, // construct contact con[0].dist = dst - rs - rt; mji_addScl3(con[0].pos, s, nrm, rs + con[0].dist / 2); - mji_copy3(con[0].frame, nrm); - mju_zero3(con[0].frame+3); + mji_copy3(con[0].normal, nrm); + mju_zero3(con[0].tangent); return 1; } // box : triangle with radius -int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, +int mjraw_BoxTriangle(mjPreContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt) { int cnt = 0; @@ -698,17 +698,17 @@ int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, nrm_local[maxaxis] = (local[maxaxis] > 0 ? 1 : -1); // normal in global frame (from Box to Triangle) - mju_mulMatVec3(con[cnt].frame, mat, nrm_local); + mju_mulMatVec3(con[cnt].normal, mat, nrm_local); // distance con[cnt].dist = maxval - rt; // position: v - nrm * (rt + dist/2) mjtNum offset = rt + con[cnt].dist * 0.5; - mji_addScl3(con[cnt].pos, vert[i], con[cnt].frame, -offset); + mji_addScl3(con[cnt].pos, vert[i], con[cnt].normal, -offset); // frame details - mju_zero3(con[cnt].frame + 3); + mju_zero3(con[cnt].tangent); cnt++; } @@ -743,7 +743,7 @@ int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, // capsule : triangle with radius -int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, +int mjraw_CapsuleTriangle(mjPreContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt) { @@ -793,8 +793,8 @@ int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, con[cnt].dist = dist - radius - rt; // Frame: normal from Capsule to Triangle. 'vec' points Closest->Vert. - mji_copy3(con[cnt].frame, vec); - mju_zero3(con[cnt].frame + 3); + mji_copy3(con[cnt].normal, vec); + mju_zero3(con[cnt].tangent); // Position: midway between surfaces mji_add3(con[cnt].pos, closest, vert[i]); diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h index e2df2966..f68d81bd 100644 --- a/src/engine/engine_collision_primitive.h +++ b/src/engine/engine_collision_primitive.h @@ -34,12 +34,12 @@ int mjraw_CapsuleCapsule(mjPreContact* con, mjtNum margin, const mjtNum* pos1, c int mjraw_CapsuleBox(mjPreContact* con, mjtNum margin, const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1, const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2); -int mjraw_SphereTriangle(mjContact* con, mjtNum margin, const mjtNum* s, mjtNum rs, +int mjraw_SphereTriangle(mjPreContact* con, mjtNum margin, const mjtNum* s, mjtNum rs, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); -int mjraw_BoxTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, +int mjraw_BoxTriangle(mjPreContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt); -int mjraw_CapsuleTriangle(mjContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, +int mjraw_CapsuleTriangle(mjPreContact* con, mjtNum margin, const mjtNum* pos, const mjtNum* mat, const mjtNum* size, const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt);