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

Refactor Mesh-SDF collision to use farthest point sampling and mjMAXCONPAIR.

Browse Source 
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
This commit is contained in:
Alessio Quaglino
2026-02-27 09:41:51 -08:00
committed by Copybara-Service
parent 67b6ab0589
commit c2e67ab507
1 changed files with 149 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_sdf.c
+149 -106
View File
@@ -31,8 +31,6 @@
#include "engine/engine_util_spatial.h" #include "engine/engine_util_spatial.h"
#define MAXSDFFACE 1300
#define MAXMESHPNT 500
//---------------------------- interpolated sdf ------------------------------------------- //---------------------------- interpolated sdf -------------------------------------------
@@ -516,23 +514,6 @@ static void mapPose(const mjtNum xpos1[3], const mjtNum xquat1[4],
//---------------------------- narrow phase ----------------------------------------------- //---------------------------- 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 // check if the collision point already exists
static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) { static int isknown(const mjtNum* points, const mjtNum x[3], int cnt) {
for (int i = 0; i < cnt; i++) { 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; 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 // process a single mesh face inline during BVH traversal
static void collideBVH(const mjModel* m, mjData* d, int g, static void processOneFace(int faceid, mjtByte* bvh_active, int node,
const mjtNum offset[3], const mjtNum rotation[9], MeshSDFContext* ctx) {
int* faces, int* npoints, int* n0, mjtNum corners[9], x[3], depth;
const mjSDF* sdf) { 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]]; const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]];
// mesh has no BVH tree // 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; mjtByte* bvh_active = m->vis.global.bvactive ? d->bvh_active + bvhadr : NULL;
mj_markStack(d); mj_markStack(d);
// TODO(quaglino): Store bvh max depths to make this bound tighter.
int max_stack = m->mesh_bvhnum[m->geom_dataid[g]]; int max_stack = m->mesh_bvhnum[m->geom_dataid[g]];
struct CollideTreeArgs_ { struct CollideTreeArgs_ {
int node; int node;
@@ -771,35 +820,26 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
nstack++; nstack++;
while (nstack) { while (nstack) {
(*n0)++;
// pop from stack
nstack--; nstack--;
int node = stack[nstack].node; int node = stack[nstack].node;
// node1 is a leaf // leaf node: process face inline
if (faceid[node] != -1) { if (faceid[node] != -1) {
if (boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { if (boxIntersect(bvh + 6*node, ctx->offset, ctx->rotation, m, ctx->sdf, d)) {
faces[*npoints] = faceid[node]; processOneFace(faceid[node], bvh_active, node, ctx);
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;
} }
continue; continue;
} }
// if no intersection at intermediate levels, stop // intermediate node: check bounding box
if (!boxIntersect(bvh+6*node, offset, rotation, m, sdf, d)) { if (!boxIntersect(bvh + 6*node, ctx->offset, ctx->rotation, m, ctx->sdf, d)) {
continue; continue;
} }
if (bvh_active) bvh_active[node] = 1; if (bvh_active) bvh_active[node] = 1;
// recursive call // push children
for (int i=0; i < 2; i++) { for (int i = 0; i < 2; i++) {
if (child[2*node+i] != -1) { if (child[2*node+i] != -1) {
if (nstack >= max_stack) mjERROR("BVH stack depth exceeded."); if (nstack >= max_stack) mjERROR("BVH stack depth exceeded.");
stack[nstack].node = child[2*node+i]; 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) { int mjc_MeshSDF(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO; mjGETINFO;
mjtNum offset[3], rotation[9], corners[9], x[3], depth; mjtNum offset[3], rotation[9];
mjtNum points[3*MAXSDFFACE], dist[MAXMESHPNT], candidate[3*MAXMESHPNT]; mjtNum points[3*mjMAXCONPAIR], dist[mjMAXCONPAIR], candidate[3*mjMAXCONPAIR];
int vertadr = m->mesh_vertadr[m->geom_dataid[g1]]; int cnt = 0, ncandidate = 0;
int faceadr = m->mesh_faceadr[m->geom_dataid[g1]];
int cnt=0, npoints=0, ncandidate=0, n0=0, faces[MAXSDFFACE]={-1}, index[MAXMESHPNT];
// get sdf plugin // get sdf plugin
int instance = m->geom_plugin[g2]; 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); mju_mat2Quat(sdf_quat, mat2);
mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation); mapPose(pos1, quat1, pos2, sdf_quat, offset, rotation);
// binary tree search // set up context for inline BVH processing
collideBVH(m, (mjData*)d, g1, offset, rotation, faces, &npoints, &n0, &sdf); 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 // binary tree search with inline face processing
for (int i=0; i < npoints; i++) { collideBVHInline(m, (mjData*)d, g1, &ctx);
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],
};
// transform local 1 (mesh) to local 2 (sdf) // if few candidates, add them all directly
mju_mulMatVec3(corners+3*v, rotation, vec); if (ncandidate <= mjMAXCONPAIR) {
mju_addTo3(corners+3*v, offset); 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 // farthest point sampling (FPS) for spatial coverage
if (!triangleIntersect(corners, m, &sdf, (mjData*)d)) { mjtByte selected[mjMAXCONPAIR] = {0};
continue; mjtNum min_dist2[mjMAXCONPAIR];
} for (int i = 0; i < ncandidate; i++) {
min_dist2[i] = mjMAXVAL;
}
// number of starting points per face // start with deepest penetrating contact
int nstartpts = mju_max(1, m->opt.sdf_initpoints); int best = 0;
mjtNum bestval = -dist[0];
for (int sp = 0; sp < nstartpts; sp++) { for (int i = 1; i < ncandidate; i++) {
// SHOULD NOT OCCUR if (-dist[i] > bestval) {
if (ncandidate == MAXMESHPNT) break; bestval = -dist[i];
best = i;
// 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;
}
} }
} }
// sort contacts using depth // iteratively select contacts using FPS
if (ncandidate > 1) { int nselected = 0;
int buf[MAXMESHPNT]; while (nselected < mjMAXCONPAIR && best >= 0) {
distSort(index, buf, ncandidate, dist); selected[best] = 1;
} mjtNum* bestpos = candidate + 3*best;
// add only the first mjMAXCONPAIR pairs // add the selected contact
for (int i=0; i < mju_min(ncandidate, mjMAXCONPAIR); i++) { cnt = addContact(points, con, bestpos, pos2, sdf_quat,
cnt = addContact(points, con, candidate + 3*index[i], pos2, sdf_quat, dist[best], cnt, m, &sdf, (mjData*)d);
dist[index[i]], 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; return cnt;