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

Add early exit in mj_xfrcAccumulate using mju_isZeroByte

Browse Source 
`xfrc_applied` mostly contains user perturbations which are rare. It is very often identically zero and for large arrays `mju_isZeroByte` is significantly faster than `mju_isZero`.

PiperOrigin-RevId: 824975356
Change-Id: Iadb1d300d68cf12de00c14a0665bd89924a2d7e0
This commit is contained in:
Yuval Tassa
2025-10-28 04:58:15 -07:00
committed by Copybara-Service
parent 017788b9d6
commit e1a73f9c4c
1 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+12 -3
View File
@@ -425,9 +425,18 @@ void mj_applyFT(const mjModel* m, mjData* d,
// accumulate xfrc_applied in qfrc // accumulate xfrc_applied in qfrc
void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) { void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) {
for (int i=1; i < m->nbody; i++) { int nbody = m->nbody;
if (!mju_isZero(d->xfrc_applied+6*i, 6)) { const mjtNum *xfrc = d->xfrc_applied;
mj_applyFT(m, d, d->xfrc_applied+6*i, d->xfrc_applied+6*i+3, d->xipos+3*i, i, qfrc);
// quick return if identically zero (efficient memcmp implementation)
if (mju_isZeroByte((const unsigned char*)(xfrc+6), 6*(nbody-1)*sizeof(mjtNum))) {
return;
}
// some non-zero wrenches, apply them
for (int i=1; i < nbody; i++) {
if (!mju_isZero(xfrc+6*i, 6)) {
mj_applyFT(m, d, xfrc+6*i, xfrc+6*i+3, d->xipos+3*i, i, qfrc);
} }
} }
} }