From e1a73f9c4c3a99151e274b5fa48c13cdd74d5fc1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 28 Oct 2025 04:58:15 -0700 Subject: [PATCH] Add early exit in `mj_xfrcAccumulate` using `mju_isZeroByte` `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 --- src/engine/engine_support.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index ce1b44ad..c2fbd2ff 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -425,9 +425,18 @@ void mj_applyFT(const mjModel* m, mjData* d, // accumulate xfrc_applied in qfrc void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) { - for (int i=1; i < m->nbody; i++) { - if (!mju_isZero(d->xfrc_applied+6*i, 6)) { - mj_applyFT(m, d, d->xfrc_applied+6*i, d->xfrc_applied+6*i+3, d->xipos+3*i, i, qfrc); + int nbody = m->nbody; + const mjtNum *xfrc = d->xfrc_applied; + + // 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); } } }