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

Remove goto statement from engine collision code.

Browse Source 
PiperOrigin-RevId: 688539516
Change-Id: I29b5dd39acccde59df2dbcaf5e866eb6a0406c43
This commit is contained in:
Kyle Bayes
2024-10-22 07:21:19 -07:00
committed by Copybara-Service
parent bfa78614c8
commit 708fd83a49
1 changed files with 34 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+34 -39
View File
@@ -1243,48 +1243,43 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) {
}
}
// nothing collidable
if (ncollide < 2) {
goto endbroad;
}
// allocate and construct AAMMs for collidable only
mjtNum* aamm = mj_stackAllocNum(d, 6*ncollide);
for (int i=0; i < ncollide; i++) {
makeAAMM(m, d, aamm+6*i, bfid[i], frame);
}
// call SAP
int maxsappair = ncollide*(ncollide-1)/2;
int* sappair = mj_stackAllocInt(d, maxsappair);
int nsappair = mj_SAP(d, aamm, ncollide, 0, sappair, maxsappair);
if (nsappair < 0) {
mjERROR("SAP failed");
}
// filter SAP pairs, convert to bodyflex pairs
for (int i=0; i < nsappair; i++) {
int bf1 = bfid[sappair[i] >> 16];
int bf2 = bfid[sappair[i] & 0xFFFF];
// body pair: prune based on weld filter
if (bf1 < nbody && bf2 < nbody) {
int weld1 = m->body_weldid[bf1];
int weld2 = m->body_weldid[bf2];
int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
if (filterBodyPair(weld1, parent_weld1, weld2, parent_weld2,
dsbl_filterparent)) {
continue;
}
if (ncollide > 1) {
// allocate and construct AAMMs for collidable only
mjtNum* aamm = mj_stackAllocNum(d, 6*ncollide);
for (int i=0; i < ncollide; i++) {
makeAAMM(m, d, aamm+6*i, bfid[i], frame);
}
// add bodyflex pair if there is room in buffer
add_pair(m, bf1, bf2, &npair, bfpair, maxpair);
}
// call SAP
int maxsappair = ncollide*(ncollide-1)/2;
int* sappair = mj_stackAllocInt(d, maxsappair);
int nsappair = mj_SAP(d, aamm, ncollide, 0, sappair, maxsappair);
if (nsappair < 0) {
mjERROR("SAP failed");
}
endbroad:
// filter SAP pairs, convert to bodyflex pairs
for (int i=0; i < nsappair; i++) {
int bf1 = bfid[sappair[i] >> 16];
int bf2 = bfid[sappair[i] & 0xFFFF];
// body pair: prune based on weld filter
if (bf1 < nbody && bf2 < nbody) {
int weld1 = m->body_weldid[bf1];
int weld2 = m->body_weldid[bf2];
int parent_weld1 = m->body_weldid[m->body_parentid[weld1]];
int parent_weld2 = m->body_weldid[m->body_parentid[weld2]];
if (filterBodyPair(weld1, parent_weld1, weld2, parent_weld2,
dsbl_filterparent)) {
continue;
}
}
// add bodyflex pair if there is room in buffer
add_pair(m, bf1, bf2, &npair, bfpair, maxpair);
}
}
// sort bodyflex pairs by signature
if (npair > 1) {