From b469144a81b44b272a98f45896ceb9237c53424b Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 16 Jun 2026 07:03:45 -0700 Subject: [PATCH] Modify mjc_FlexSDF to support mjPreContact directly. PiperOrigin-RevId: 933068825 Change-Id: I328d64bb05270067ce146908b0df3ca83f10ed9b --- src/engine/engine_collision_driver.c | 98 +++++++++++++++++++--------- src/engine/engine_collision_driver.h | 9 --- src/engine/engine_collision_sdf.c | 39 +++-------- src/engine/engine_collision_sdf.h | 2 +- 4 files changed, 79 insertions(+), 69 deletions(-) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index dd70d99e..5ce39f72 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -351,14 +351,23 @@ int mj_isElemActive(const mjModel* m, int f, int e) { //----------------------------- collision detection entry point ------------------------------------ // binary search between two bodyflex trees -void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, - int merged, int startadr, int pairadr); +static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, + int merged, int startadr, int pairadr); // 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 -void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, size_t parena); +static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, + size_t parena); +// test a plane geom and a flex for collision, add to contact list +static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f); + +// test an SDF geom and a flex for collision, add to contact list +static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f); + +// test for internal flex collisions, add to contact list +static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f); // compare contact pairs by their geom/elem/vert IDs static inline int contactcompare(const mjContact* c1, const mjContact* c2, void* context) { @@ -937,8 +946,8 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], // binary search between two bodyflex trees -void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, - int merged, int startadr, int pairadr) { +static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, + int merged, int startadr, int pairadr) { int nbody = m->nbody, nbvhstatic = m->nbvhstatic; mjtBool isbody1 = (bf1 < nbody); mjtBool isbody2 = (bf2 < nbody); @@ -1865,7 +1874,8 @@ 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 -void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, size_t parena) { +static void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, + size_t parena) { int nthread = mju_numThread(d); int ccd_size = mjc_ccdSize(m->opt.ccd_iterations); mjtNum margin, gap; @@ -1992,7 +2002,7 @@ void mj_narrowphase(const mjModel* m, mjData* d, const int* buffer, int npair, s // test a plane geom and a flex for collision, add to contact list -void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { +static void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { mjContact con; mjtNum radius = m->flex_radius[f]; mjtNum* pos = d->geom_xpos + 3*g; @@ -2049,7 +2059,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { // test an SDF geom and a flex for collision, add to contact list -void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { +static void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { // only support dim==2 (triangular elements) if (m->flex_dim[f] != 2) { return; @@ -2065,29 +2075,37 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { // allocate temporary contact array on stack (zero-initialized) mj_markStack(d); - mjContact* con = mjSTACKALLOC(d, mjMAXCONPAIR, mjContact); - memset(con, 0, mjMAXCONPAIR * sizeof(mjContact)); + mjPreContact* precon = mjSTACKALLOC(d, mjMAXCONPAIR, mjPreContact); + int* elem = mjSTACKALLOC(d, mjMAXCONPAIR, int); // call batched flex-SDF collision - int num = mjc_FlexSDF(m, d, con, g, f, margin + gap); + int num = mjc_FlexSDF(m, d, precon, elem, g, f, margin + gap); // add contacts to mjData + mjContact con; + memset(&con, 0, sizeof(mjContact)); for (int i = 0; i < num; i++) { + // copy properties from mjPreContact to mjContact + con.dist = precon[i].dist; + mju_copy3(con.pos, precon[i].pos); + mju_copy3(con.frame, precon[i].normal); + mju_copy3(con.frame + 3, precon[i].tangent); + // set contact ids - con[i].geom[0] = g; - con[i].geom[1] = -1; - con[i].flex[0] = -1; - con[i].flex[1] = f; - con[i].elem[0] = -1; - // con[i].elem[1] already set by mjc_FlexSDF with actual element index - con[i].vert[0] = -1; - con[i].vert[1] = -1; + con.geom[0] = g; + con.geom[1] = -1; + con.flex[0] = -1; + con.flex[1] = f; + con.elem[0] = -1; + con.elem[1] = elem[i]; + con.vert[0] = -1; + con.vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction); // add to mjData, abort if too many contacts - if (mj_addContact(m, d, con + i)) { + if (mj_addContact(m, d, &con)) { mj_freeStack(d); return; } @@ -2098,7 +2116,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { // test single triangle plane : vertex -static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad, +static int planeVertex(mjPreContact* con, const mjtNum* pos, mjtNum rad, int t0, int t1, int t2, int v) { // make t0 the origin mjtNum e1[3], e2[3], ev[3]; @@ -2119,17 +2137,16 @@ static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad, // construct contact con->dist = -dst-2*rad; - mju_scl3(con->frame, nrm, -1); - mju_zero3(con->frame+3); + mju_scl3(con->normal, nrm, -1); + mju_zero3(con->tangent); mju_addScl3(con->pos, pos+3*v, nrm, -0.5*dst); - con->vert[1] = v; return 1; } // test for internal flex collisions, add to contact list // ignore margin to avoid permament self-collision -void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { +static void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { int flex_evpairnum = m->flex_evpairnum[f]; // predefined element-vertex @@ -2161,29 +2178,50 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { // process all elements const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; for (int e=0; e < flex_elemnum; e++) { + mjPreContact precon; const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*4; con.elem[0] = e; // face (0,1,2) - if (planeVertex(&con, vertxpos, radius, edata[0], edata[1], edata[2], edata[3])) { + if (planeVertex(&precon, vertxpos, radius, edata[0], edata[1], edata[2], edata[3])) { + con.vert[1] = edata[3]; + con.dist = precon.dist; + mju_copy3(con.pos, precon.pos); + mju_copy3(con.frame, precon.normal); + mju_copy3(con.frame + 3, precon.tangent); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); if (mj_addContact(m, d, &con)) return; } // face (0,2,3) - if (planeVertex(&con, vertxpos, radius, edata[0], edata[2], edata[3], edata[1])) { + if (planeVertex(&precon, vertxpos, radius, edata[0], edata[2], edata[3], edata[1])) { + con.vert[1] = edata[1]; + con.dist = precon.dist; + mju_copy3(con.pos, precon.pos); + mju_copy3(con.frame, precon.normal); + mju_copy3(con.frame + 3, precon.tangent); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); if (mj_addContact(m, d, &con)) return; } // face (0,3,1) - if (planeVertex(&con, vertxpos, radius, edata[0], edata[3], edata[1], edata[2])) { + if (planeVertex(&precon, vertxpos, radius, edata[0], edata[3], edata[1], edata[2])) { + con.vert[1] = edata[2]; + con.dist = precon.dist; + mju_copy3(con.pos, precon.pos); + mju_copy3(con.frame, precon.normal); + mju_copy3(con.frame + 3, precon.tangent); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); if (mj_addContact(m, d, &con)) return; } // face (1,3,2) - if (planeVertex(&con, vertxpos, radius, edata[1], edata[3], edata[2], edata[0])) { + if (planeVertex(&precon, vertxpos, radius, edata[1], edata[3], edata[2], edata[0])) { + con.vert[1] = edata[0]; + con.dist = precon.dist; + mju_copy3(con.pos, precon.pos); + mju_copy3(con.frame, precon.normal); + mju_copy3(con.frame + 3, precon.tangent); mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); if (mj_addContact(m, d, &con)) return; } diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index e3692f8f..d41f1ccf 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -47,15 +47,6 @@ MJAPI int mj_isElemActive(const mjModel* m, int f, int e); // broad phase collision detection; return list of bodyflex pairs int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair); -// test a plane geom and a flex for collision, add to contact list -void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f); - -// test an SDF geom and a flex for collision, add to contact list -void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f); - -// test for internal flex collisions, add to contact list -void mj_collideFlexInternal(const mjModel* m, mjData* d, int f); - // test active element self-collisions with SAP void mj_collideFlexSAP(const mjModel* m, mjData* d, int f); diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 54e29eb1..de5a189a 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -577,29 +577,10 @@ static int addPreContact(mjtNum* points, mjPreContact* con, const mjtNum x[3], mju_addTo3(con->pos, pos2); mju_addTo3(con->pos, vec); - return cnt+1; + return cnt + 1; } -// adds candidate point to result -// flipNormal: 0 = normal points INTO SDF (for mesh-SDF where SDF is g2) -// 1 = normal points OUT of SDF (for flex-SDF where SDF is g1) -static int addContact(mjtNum* points, mjContact* con, const mjtNum x[3], - const mjtNum pos2[3], const mjtNum quat2[4], mjtNum dist, - int cnt, const mjModel* m, const mjSDF* s, const mjData* d, - int flipNormal) { - mjPreContact precon; - int ncon = addPreContact(points, &precon, x, pos2, quat2, dist, cnt, m, s, d, - flipNormal); - if (ncon > cnt) { - con[cnt].dist = precon.dist; - mju_copy3(con[cnt].pos, precon.pos); - mju_copy3(con[cnt].frame, precon.normal); - mju_zero3(con[cnt].frame + 3); - } - return ncon; -} - // finds minimum of Frank-Wolfe objective static mjtNum stepFrankWolfe(mjtNum x[3], const mjtNum* corners, int ncorners, const mjModel* m, const mjSDF* sdf, const mjData* d) { @@ -1264,7 +1245,7 @@ static int flexElemCallback(int elem_idx, int node, void* ctx) { } -int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con, +int mjc_FlexSDF(const mjModel* m, const mjData* d, mjPreContact* con, int* elem, int g, int f, mjtNum margin) { // g = SDF geom, f = flex int dim = m->flex_dim[f]; @@ -1336,11 +1317,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con, int elem_cnt = 0; for (int i = 0; i < elem_ncandidate && cnt < mjMAXCONPAIR; i++) { int old_elem_cnt = elem_cnt; - elem_cnt = addContact(elem_points, con + cnt, elem_candidate + 3*i, sdf_pos, sdf_quat, + elem_cnt = addPreContact(elem_points, con + cnt, elem_candidate + 3*i, sdf_pos, sdf_quat, elem_dist[i], elem_cnt, m, &sdf, d, 1); // set element ID for successfully added contact if (elem_cnt > old_elem_cnt) { - con[cnt].elem[1] = e; + elem[cnt] = e; cnt++; } } @@ -1380,11 +1361,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con, if (ncandidate <= mjMAXCONPAIR) { for (int i = 0; i < ncandidate; i++) { int old_cnt = cnt; - cnt = addContact(points, con, candidate + 3*i, sdf_pos, sdf_quat, - dist[i], cnt, m, &sdf, d, 1); + cnt = addPreContact(points, con + cnt, candidate + 3*i, sdf_pos, sdf_quat, dist[i], cnt, m, + &sdf, d, 1); // set element ID for successfully added contact if (cnt > old_cnt) { - con[old_cnt].elem[1] = elem_id[i]; + elem[old_cnt] = elem_id[i]; } } return cnt; @@ -1398,11 +1379,11 @@ int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con, for (int i = 0; i < nselected; i++) { int idx = selected_indices[i]; int old_cnt = cnt; - cnt = addContact(points, con, candidate + 3*idx, sdf_pos, sdf_quat, - dist[idx], cnt, m, &sdf, d, 1); + cnt = addPreContact(points, con + cnt, candidate + 3*idx, sdf_pos, sdf_quat, dist[idx], cnt, m, + &sdf, d, 1); // set element ID for successfully added contact if (cnt > old_cnt) { - con[old_cnt].elem[1] = elem_id[idx]; + elem[old_cnt] = elem_id[idx]; } } diff --git a/src/engine/engine_collision_sdf.h b/src/engine/engine_collision_sdf.h index 79e3d722..30a5f3d4 100644 --- a/src/engine/engine_collision_sdf.h +++ b/src/engine/engine_collision_sdf.h @@ -45,7 +45,7 @@ int mjc_MeshSDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, int mjc_SDF(const mjModel* m, mjData* d, mjPreContact* con, int g1, int g2, mjtNum margin); // collision between entire flex and SDF geom (batched processing) -int mjc_FlexSDF(const mjModel* m, const mjData* d, mjContact* con, +int mjc_FlexSDF(const mjModel* m, const mjData* d, mjPreContact* con, int* elem, int g, int f, mjtNum margin); #ifdef __cplusplus