From c2e67ab5079c91697b76f51059e720cb9d10059a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 27 Feb 2026 09:41:51 -0800 Subject: [PATCH] Refactor Mesh-SDF collision to use farthest point sampling and mjMAXCONPAIR. Deleted MAXMESHPNT farthest point sampling instead of sorting by penetration distance is now used to select the best contact points, up to mjMAXCONPAIR. Deleted MAXSDFFACE, the BVH traversal now directly calls a function to process each leaf face against the SDF, instead of collecting face indices and processing them in a separate loop. This reduces memory by removing the need to store all intersecting face indices. PiperOrigin-RevId: 876296914 Change-Id: I8068d634e2a01ffa9357d0c5381219d097e97abe --- src/engine/engine_collision_sdf.c | 255 +++++++++++++++++------------- 1 file changed, 149 insertions(+), 106 deletions(-) diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 39971712..a49acecc 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -31,8 +31,6 @@ #include "engine/engine_util_spatial.h" -#define MAXSDFFACE 1300 -#define MAXMESHPNT 500 //---------------------------- interpolated sdf ------------------------------------------- @@ -516,23 +514,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4], //---------------------------- narrow phase ----------------------------------------------- -// comparison function for contact sorting -static inline int distcmp(int* i, int* j, void* context) { - mjtNum d1 = ((mjtNum*)context)[*i]; - mjtNum d2 = ((mjtNum*)context)[*j]; - - if (d1 < d2) { - return -1; - } else if (d1 == d2) { - return 0; - } else { - return 1; - } -} - -// define distSort function for contact sorting -mjSORT(distSort, int, distcmp); - // check if the collision point already exists static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) { for (int i = 0; i < cnt; i++) { @@ -739,13 +720,82 @@ static int boxIntersect(const mjtNum bvh[6], const mjtNum offset[3], return mjc_distance(m, d, s, candidate) < r; } -//---------------------------- mesh vs sdf broad phase -------------------------------------------- +// Context for inline face processing during BVH traversal +typedef struct { + const mjModel* m; + mjData* d; + const mjSDF* sdf; + const mjtNum* offset; + const mjtNum* rotation; + int vertadr; + int faceadr; + int nstartpts; + mjtNum* candidate; + mjtNum* dist; + int* ncandidate; +} MeshSDFContext; -// tree vs sdf binary search -static void collideBVH(const mjModel* m, mjData* d, int g, - const mjtNum offset[3], const mjtNum rotation[9], - int* faces, int* npoints, int* n0, - const mjSDF* sdf) { +// process a single mesh face inline during BVH traversal +static void processOneFace(int faceid, mjtByte* bvh_active, int node, + MeshSDFContext* ctx) { + mjtNum corners[9], x[3], depth; + const mjModel* m = ctx->m; + mjData* d = ctx->d; + int face = ctx->faceadr + faceid; + + // transform triangle vertices to SDF local coordinates + for (int v = 0; v < 3; v++) { + mjtNum vec[3] = { + m->mesh_vert[3*(ctx->vertadr + m->mesh_face[3*face+v]) + 0], + m->mesh_vert[3*(ctx->vertadr + m->mesh_face[3*face+v]) + 1], + m->mesh_vert[3*(ctx->vertadr + m->mesh_face[3*face+v]) + 2], + }; + mju_mulMatVec3(corners + 3*v, ctx->rotation, vec); + mju_addTo3(corners + 3*v, ctx->offset); + } + + // stricter culling using triangle circumsphere + if (!triangleIntersect(corners, m, ctx->sdf, d)) { + return; + } + + if (bvh_active) bvh_active[node] = 1; + + // sample multiple starting points using Halton sequence + for (int sp = 0; sp < ctx->nstartpts; sp++) { + if (*(ctx->ncandidate) >= mjMAXCONPAIR) break; + + // barycentric coordinates from Halton sequence + mjtNum u = mju_Halton(sp + 1, 2); + mjtNum v = mju_Halton(sp + 1, 3); + if (u + v > 1) { + u = 1 - u; + v = 1 - v; + } + mjtNum b0 = 1 - u - v; + mjtNum b1 = u; + mjtNum b2 = v; + + // starting point + x[0] = b0*corners[0] + b1*corners[3] + b2*corners[6]; + x[1] = b0*corners[1] + b1*corners[4] + b2*corners[7]; + x[2] = b0*corners[2] + b1*corners[5] + b2*corners[8]; + + depth = stepFrankWolfe(x, corners, 3, m, ctx->sdf, d); + + // store candidate if penetration + if (depth < 0) { + int nc = *(ctx->ncandidate); + mju_copy3(ctx->candidate + 3*nc, x); + ctx->dist[nc] = depth; + (*(ctx->ncandidate))++; + } + } +} + +// tree vs sdf binary search with inline face processing +static void collideBVHInline(const mjModel* m, mjData* d, int g, + MeshSDFContext* ctx) { const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]]; // mesh has no BVH tree @@ -759,7 +809,6 @@ static void collideBVH(const mjModel* m, mjData* d, int g, mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL; mj_markStack(d); - // TODO(quaglino): Store bvh max depths to make this bound tighter. int max_stack = m->mesh_bvhnum[m->geom_dataid[g]]; struct CollideTreeArgs_ { int node; @@ -771,35 +820,26 @@ static void collideBVH(const mjModel* m, mjData* d, int g, nstack++; while (nstack) { - (*n0)++; - - // pop from stack nstack--; int node = stack[nstack].node; - // node1 is a leaf + // leaf node: process face inline if (faceid[node] != -1) { - if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { - faces[*npoints] = faceid[node]; - if (++(*npoints) == MAXSDFFACE) { - mju_warning("mjc_MeshSDF: too many bounding volumes, some contacts may be missed"); - mj_freeStack(d); - return; - } - if (bvh_active) bvh_active[node] = 1; + if (boxIntersect(bvh + 6*node, ctx->offset, ctx->rotation, m, ctx->sdf, d)) { + processOneFace(faceid[node], bvh_active, node, ctx); } continue; } - // if no intersection at intermediate levels, stop - if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { + // intermediate node: check bounding box + if (!boxIntersect(bvh + 6*node, ctx->offset, ctx->rotation, m, ctx->sdf, d)) { continue; } if (bvh_active) bvh_active[node] = 1; - // recursive call - for (int i=0; i < 2; i++) { + // push children + for (int i = 0; i < 2; i++) { if (child[2*node+i] != -1) { if (nstack >= max_stack) mjERROR("BVH stack depth exceeded."); stack[nstack].node = child[2*node+i]; @@ -824,11 +864,9 @@ int mjc_HFieldSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { mjGETINFO; - mjtNum offset[3], rotation[9], corners[9], x[3], depth; - mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT]; - int vertadr = m->mesh_vertadr[m->geom_dataid[g1]]; - int faceadr = m->mesh_faceadr[m->geom_dataid[g1]]; - int cnt=0, npoints=0, ncandidate=0, n0=0, faces[MAXSDFFACE]={-1}, index[MAXMESHPNT]; + mjtNum offset[3], rotation[9]; + mjtNum points[3*mjMAXCONPAIR], dist[mjMAXCONPAIR], candidate[3*mjMAXCONPAIR]; + int cnt = 0, ncandidate = 0; // get sdf plugin int instance = m->geom_plugin[g2]; @@ -849,74 +887,79 @@ int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g mju_mat2Quat(sdf_quat, mat2); mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation); - // binary tree search - collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf); + // set up context for inline BVH processing + MeshSDFContext ctx; + ctx.m = m; + ctx.d = (mjData*)d; + ctx.sdf = &sdf; + ctx.offset = offset; + ctx.rotation = rotation; + ctx.vertadr = m->mesh_vertadr[m->geom_dataid[g1]]; + ctx.faceadr = m->mesh_faceadr[m->geom_dataid[g1]]; + ctx.nstartpts = mju_max(1, m->opt.sdf_initpoints); + ctx.candidate = candidate; + ctx.dist = dist; + ctx.ncandidate = &ncandidate; - // Frank-Wolfe algorithm - for (int i=0; i < npoints; i++) { - int face = faceadr + faces[i]; - for (int v=0; v < 3; v++) { - mjtNum vec[3] = { - m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+0], - m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+1], - m->mesh_vert[3*(vertadr+m->mesh_face[3*face+v])+2], - }; + // binary tree search with inline face processing + collideBVHInline(m, (mjData*)d, g1, &ctx); - // transform local 1 (mesh) to local 2 (sdf) - mju_mulMatVec3(corners+3*v, rotation, vec); - mju_addTo3(corners+3*v, offset); + // if few candidates, add them all directly + if (ncandidate <= mjMAXCONPAIR) { + for (int i = 0; i < ncandidate; i++) { + cnt = addContact(points, con, candidate + 3*i, pos2, sdf_quat, + dist[i], cnt, m, &sdf, (mjData*)d); } + return cnt; + } - // stricter culling - if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) { - continue; - } + // farthest point sampling (FPS) for spatial coverage + mjtByte selected[mjMAXCONPAIR] = {0}; + mjtNum min_dist2[mjMAXCONPAIR]; + for (int i = 0; i < ncandidate; i++) { + min_dist2[i] = mjMAXVAL; + } - // number of starting points per face - int nstartpts = mju_max(1, m->opt.sdf_initpoints); - - for (int sp = 0; sp < nstartpts; sp++) { - // SHOULD NOT OCCUR - if (ncandidate == MAXMESHPNT) break; - - // generate barycentric coordinates using Halton sequence - // map unit square to barycentric simplex - mjtNum u = mju_Halton(sp + 1, 2); - mjtNum v = mju_Halton(sp + 1, 3); - if (u + v > 1) { - u = 1 - u; - v = 1 - v; - } - mjtNum b0 = 1 - u - v; - mjtNum b1 = u; - mjtNum b2 = v; - - // starting point using barycentric coordinates - x[0] = b0*corners[0] + b1*corners[3] + b2*corners[6]; - x[1] = b0*corners[1] + b1*corners[4] + b2*corners[7]; - x[2] = b0*corners[2] + b1*corners[5] + b2*corners[8]; - - depth = stepFrankWolfe(x, corners, 3, m, &sdf, (mjData*)d); - - // store candidate if there is penetration - if (depth < 0) { - mju_copy3(candidate + 3*ncandidate, x); - index[ncandidate] = ncandidate; - dist[ncandidate++] = depth; - } + // start with deepest penetrating contact + int best = 0; + mjtNum bestval = -dist[0]; + for (int i = 1; i < ncandidate; i++) { + if (-dist[i] > bestval) { + bestval = -dist[i]; + best = i; } } - // sort contacts using depth - if (ncandidate > 1) { - int buf[MAXMESHPNT]; - distSort(index, buf, ncandidate, dist); - } + // iteratively select contacts using FPS + int nselected = 0; + while (nselected < mjMAXCONPAIR && best >= 0) { + selected[best] = 1; + mjtNum* bestpos = candidate + 3*best; - // add only the first mjMAXCONPAIR pairs - for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) { - cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat, - dist[index[i]], cnt, m, &sdf, (mjData*)d); + // add the selected contact + cnt = addContact(points, con, bestpos, pos2, sdf_quat, + dist[best], cnt, m, &sdf, (mjData*)d); + nselected++; + + // find next farthest point + int nextbest = -1; + mjtNum nextbestdist = -1; + for (int i = 0; i < ncandidate; i++) { + if (selected[i]) continue; + + mjtNum dx = candidate[3*i+0] - bestpos[0]; + mjtNum dy = candidate[3*i+1] - bestpos[1]; + mjtNum dz = candidate[3*i+2] - bestpos[2]; + mjtNum d2 = dx*dx + dy*dy + dz*dz; + if (d2 < min_dist2[i]) { + min_dist2[i] = d2; + } + if (min_dist2[i] > nextbestdist) { + nextbestdist = min_dist2[i]; + nextbest = i; + } + } + best = nextbest; } return cnt;