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

Add fields for flex collisions in threaded narrowphase.

Browse Source 
PiperOrigin-RevId: 949651632
Change-Id: I09dfa85eb57b9c3a97850c35f51d1d2872852704
This commit is contained in:
Kyle Bayes
2026-07-17 10:48:01 -07:00
committed by Copybara-Service
parent 135b482c9d
commit e3293a5f29
1 changed files with 85 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+85 -46
View File
@@ -352,11 +352,36 @@ int mj_isElemActive(const mjModel* m, int f, int e) {
// internal structure for storing pairs meant for narrowphase collision detection // internal structure for storing pairs meant for narrowphase collision detection
typedef struct { typedef struct {
int g1; // type of collision pair
int g2; enum {
int ipair; mjCPAIR_GEOM_GEOM = 0,
mjCPAIR_GEOM_FLEX,
mjCPAIR_GEOM_ELEM,
mjCPAIR_ELEM_VERT,
mjCPAIR_ELEM_ELEM,
} type;
int conpos;
union {
struct { int g1, g2, ipair; } geom_geom;
struct { int g, f; } geom_flex;
struct { int g, f, e; } geom_elem;
struct { int f, e, v; } elem_vert;
struct { int f1, e1, f2, e2; } elem_elem;
};
} mjcPair; } mjcPair;
// initialize a default collision pair
static inline void defaultPair(mjcPair* pair, int type) {
pair->type = type;
// clear all fields in the union
pair->elem_elem.f1 = -1;
pair->elem_elem.e1 = -1;
pair->elem_elem.f2 = -1;
pair->elem_elem.e2 = -1;
}
// binary search between two bodyflex trees // binary search between two bodyflex trees
static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr); int merged, int startadr, int pairadr);
@@ -486,21 +511,30 @@ static void filterFlexContacts(mjData* d, int ncon_before) {
// push a candidate collision pair onto the arena // push a candidate collision pair onto the arena
static void pushPairArena(const mjModel* m, mjData* d, int g1, int g2, int ipair) { static void pushPairArena(mjData* d, mjcPair* pair) {
// allocate geom pair on the arena // allocate geom pair on the arena
mjcPair* pair = (mjcPair*) mj_arenaAllocByte(d, sizeof(mjcPair), _Alignof(mjcPair)); mjcPair* new_pair = (mjcPair*) mj_arenaAllocByte(d, sizeof(mjcPair), _Alignof(mjcPair));
if (!pair) { if (!pair) {
mjERROR("arena too small to allocate geom pair"); mjERROR("arena too small to allocate geom pair");
} }
*new_pair = *pair;
}
if (g1 >= 0 && g2 >= 0 && m->geom_type[g1] > m->geom_type[g2]) {
pair->g1 = g2; // push a geom-geom collision pair onto the arena
pair->g2 = g1; static void pushGeomGeom(const mjModel* m, mjData* d, int g1, int g2, int ipair) {
} else { if (m->geom_type[g1] > m->geom_type[g2]) {
pair->g1 = g1; int tmp = g1;
pair->g2 = g2; g1 = g2;
g2 = tmp;
} }
pair->ipair = ipair;
mjcPair pair;
defaultPair(&pair, mjCPAIR_GEOM_GEOM);
pair.geom_geom.g1 = g1;
pair.geom_geom.g2 = g2;
pair.geom_geom.ipair = ipair;
pushPairArena(d, &pair);
} }
@@ -616,7 +650,7 @@ void mj_collision(const mjModel* m, mjData* d) {
merged = (m->pair_signature[pairadr] == signature); merged = (m->pair_signature[pairadr] == signature);
g1 = m->pair_geom1[pairadr], g2 = m->pair_geom2[pairadr]; g1 = m->pair_geom1[pairadr], g2 = m->pair_geom2[pairadr];
if (filterCollisionPair(m, d, g1, g2, pairadr, 0, 0, 0)) { if (filterCollisionPair(m, d, g1, g2, pairadr, 0, 0, 0)) {
pushPairArena(m, d, g1, g2, pairadr); pushGeomGeom(m, d, g1, g2, pairadr);
ngeompair++; ngeompair++;
} }
} }
@@ -651,7 +685,7 @@ void mj_collision(const mjModel* m, mjData* d) {
// process bodyflex pair: two single-geom bodies // process bodyflex pair: two single-geom bodies
if (isbody1 && isbody2 && m->body_geomnum[bf1] == 1 && m->body_geomnum[bf2] == 1) { if (isbody1 && isbody2 && m->body_geomnum[bf1] == 1 && m->body_geomnum[bf2] == 1) {
if (filterCollisionPair(m, d, geomadr1, geomadr2, -1, merged, startadr, pairadr)) { if (filterCollisionPair(m, d, geomadr1, geomadr2, -1, merged, startadr, pairadr)) {
pushPairArena(m, d, geomadr1, geomadr2, -1); pushGeomGeom(m, d, geomadr1, geomadr2, -1);
ngeompair++; ngeompair++;
} }
} }
@@ -697,7 +731,7 @@ void mj_collision(const mjModel* m, mjData* d) {
for (g1=geomadr1; g1 < geomadr_end1; g1++) { for (g1=geomadr1; g1 < geomadr_end1; g1++) {
for (g2=geomadr2; g2 < geomadr_end2; g2++) { for (g2=geomadr2; g2 < geomadr_end2; g2++) {
if (filterCollisionPair(m, d, g1, g2, -1, merged, startadr, pairadr)) { if (filterCollisionPair(m, d, g1, g2, -1, merged, startadr, pairadr)) {
pushPairArena(m, d, g1, g2, -1); pushGeomGeom(m, d, g1, g2, -1);
ngeompair++; ngeompair++;
} }
} }
@@ -780,7 +814,7 @@ void mj_collision(const mjModel* m, mjData* d) {
for (; pairadr < npair; pairadr++) { for (; pairadr < npair; pairadr++) {
g1 = m->pair_geom1[pairadr], g2 = m->pair_geom2[pairadr]; g1 = m->pair_geom1[pairadr], g2 = m->pair_geom2[pairadr];
if (filterCollisionPair(m, d, g1, g2, pairadr, 0, 0, 0)) { if (filterCollisionPair(m, d, g1, g2, pairadr, 0, 0, 0)) {
pushPairArena(m, d, g1, g2, pairadr); pushGeomGeom(m, d, g1, g2, pairadr);
ngeompair++; ngeompair++;
} }
} }
@@ -1048,9 +1082,9 @@ static void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
n2 = nodeid1; n2 = nodeid1;
} }
mjcPair pair; mjcPair pair;
pair.g1 = n1; defaultPair(&pair, mjCPAIR_GEOM_GEOM);
pair.g2 = n2; pair.geom_geom.g1 = n1;
pair.ipair = -1; pair.geom_geom.g2 = n2;
mj_narrowphase(m, d, &pair, 1, 0); mj_narrowphase(m, d, &pair, 1, 0);
} }
if (mark_active) { if (mark_active) {
@@ -1833,16 +1867,23 @@ static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2],
} }
// struct holding extra data for a flex precontact
typedef struct {
int elem[2];
int vert[2];
} mjPreFlex;
// struct for collision task // struct for collision task
typedef struct { typedef struct {
mjPreContact* conbuffer; // pre-contact buffer returned by collision functions mjPreContact* conbuffer; // pre-contact buffer returned by collision functions
int* nconbuffer; // contact count for each collision pair mjPreFlex* flexbuffer; // pre-flex buffer returned by collision functions
char* epabuffer; // buffer for nativeccd int* nconbuffer; // contact count for each collision pair
int ccd_size; // size of nativeccd buffer char* epabuffer; // buffer for nativeccd
const int* pairbuffer; // collision pairs (g1, g2, ipair, index into conbuffer) int ccd_size; // size of nativeccd buffer
int npair; // number of collision pairs const mjcPair* pairbuffer; // collision pairs
int chunksize; // number of pairs to process per task int npair; // number of collision pairs
int maxcon; // maximum number of contacts (size of conbuffer) int chunksize; // number of pairs to process per task
int maxcon; // maximum number of contacts (size of conbuffer)
} mjContactArg; } mjContactArg;
@@ -1852,7 +1893,7 @@ static void collisionTask(const mjModel* m, mjData* d, void* arg, int thread_id,
char* epabuffer = conargs->epabuffer; char* epabuffer = conargs->epabuffer;
int chunksize = conargs->chunksize; int chunksize = conargs->chunksize;
int globalidx = chunksize * idx; int globalidx = chunksize * idx;
const int* pair = conargs->pairbuffer + 4 * globalidx; const mjcPair* pair = conargs->pairbuffer + globalidx;
int* ncon = conargs->nconbuffer + chunksize * idx; int* ncon = conargs->nconbuffer + chunksize * idx;
int npair = conargs->npair; int npair = conargs->npair;
@@ -1860,10 +1901,10 @@ static void collisionTask(const mjModel* m, mjData* d, void* arg, int thread_id,
mjc_setCCDBuffer(epabuffer + thread_id * conargs->ccd_size); mjc_setCCDBuffer(epabuffer + thread_id * conargs->ccd_size);
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) {
int g1 = pair[4*i + 0]; int g1 = pair[i].geom_geom.g1;
int g2 = pair[4*i + 1]; int g2 = pair[i].geom_geom.g2;
int ipair = pair[4*i + 2]; int ipair = pair[i].geom_geom.ipair;
int conpos = pair[4*i + 3]; int conpos = pair[i].conpos;
mjfCollision collision_func = mjCOLLISIONFUNC[m->geom_type[g1]][m->geom_type[g2]]; mjfCollision collision_func = mjCOLLISIONFUNC[m->geom_type[g1]][m->geom_type[g2]];
mjtNum margin = getMargin(m, g1, g2, ipair); mjtNum margin = getMargin(m, g1, g2, ipair);
@@ -1871,7 +1912,7 @@ static void collisionTask(const mjModel* m, mjData* d, void* arg, int thread_id,
ncon[i] = collision_func(m, d, conbuffer + conpos, g1, g2, margin + gap); ncon[i] = collision_func(m, d, conbuffer + conpos, g1, g2, margin + gap);
// SHOULD NOT OCCUR // SHOULD NOT OCCUR
int expected_max = (globalidx + i + 1 < npair ? pair[4*(i+1) + 3] : conargs->maxcon) - conpos; int expected_max = (globalidx + i + 1 < npair ? pair[i+1].conpos : conargs->maxcon) - conpos;
if (ncon[i] > expected_max) { if (ncon[i] > expected_max) {
mjERROR("collision function returned %d contacts for geom pair (%d, %d), " mjERROR("collision function returned %d contacts for geom pair (%d, %d), "
"expected at most %d from mj_maxContact", ncon[i], g1, g2, expected_max); "expected at most %d from mj_maxContact", ncon[i], g1, g2, expected_max);
@@ -1906,18 +1947,16 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i
mj_markStack(d); mj_markStack(d);
// buffer for pair data (g1, g2, ipair, index into conbuffer) // buffer for pair data
int* pairbuffer = mj_stackAllocInt(d, 4 * npair); mjcPair* pairbuffer = mjSTACKALLOC(d, npair, mjcPair);
int maxcon = 0; int maxcon = 0;
for (int i = 0; i < npair; i++) { for (int i = 0; i < npair; i++) {
int g1 = buffer[i].g1; int g1 = buffer[i].geom_geom.g1;
int g2 = buffer[i].g2; int g2 = buffer[i].geom_geom.g2;
int ipair = buffer[i].ipair; int ipair = buffer[i].geom_geom.ipair;
pairbuffer[4*i + 0] = g1; pairbuffer[i] = buffer[i];
pairbuffer[4*i + 1] = g2; pairbuffer[i].conpos = maxcon;
pairbuffer[4*i + 2] = ipair;
pairbuffer[4*i + 3] = maxcon;
margin = getMargin(m, g1, g2, ipair); margin = getMargin(m, g1, g2, ipair);
gap = getGap(m, g1, g2, ipair); gap = getGap(m, g1, g2, ipair);
maxcon += mj_maxContact(m, g1, g2, margin + gap > 0); maxcon += mj_maxContact(m, g1, g2, margin + gap > 0);
@@ -1973,9 +2012,9 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i
int condim; int condim;
mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum friction[5], solref[mjNREF], solimp[mjNIMP];
mjtNum solreffriction[mjNREF] = {0}; mjtNum solreffriction[mjNREF] = {0};
int g1 = pairbuffer[4*i + 0]; int g1 = pairbuffer[i].geom_geom.g1;
int g2 = pairbuffer[4*i + 1]; int g2 = pairbuffer[i].geom_geom.g2;
int ipair = pairbuffer[4*i + 2]; int ipair = pairbuffer[i].geom_geom.ipair;
if (ipair >= 0) { if (ipair >= 0) {
condim = m->pair_dim[ipair]; condim = m->pair_dim[ipair];
@@ -1989,7 +2028,7 @@ static void mj_narrowphase(const mjModel* m, mjData* d, const mjcPair* buffer, i
mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1); mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1);
} }
mjPreContact* bc = arg.conbuffer + pairbuffer[4*i+3]; mjPreContact* bc = arg.conbuffer + pairbuffer[i].conpos;
margin = getMargin(m, g1, g2, ipair); margin = getMargin(m, g1, g2, ipair);
for (int j=0; j < ncon; j++) { for (int j=0; j < ncon; j++) {
mjContact* c = con + conpos + j; mjContact* c = con + conpos + j;