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

Refactor mju_combineSparse to eliminate temporary buffers.

Browse Source 
Combine sparse vectors in-place by first counting total `nnz` and then working backwards from the end. This removes the need for temporary buffers in `mju_combineSparse` and its callers and speeds up the function by ~10%.

PiperOrigin-RevId: 902530210
Change-Id: I4f48c327103552ab968d3915399c6067367bec9f
This commit is contained in:
Yuval Tassa
2026-04-20 03:09:31 -07:00
committed by Copybara-Service
parent 3230cf99f9
commit e6d77650f7
8 changed files with 57 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+3 -6
View File
@@ -496,12 +496,12 @@ static void mj_equalityAnchors(const mjModel* m, const mjData* d, int eq_id,
// equality constraints
void mj_instantiateEquality(const mjModel* m, mjData* d) {
int issparse = mj_isSparse(m), nv = m->nv;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL;
int flex_edgeadr, flex_edgenum;
int flex_vertadr, flex_vertnum;
mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
mjtNum *jac[2], *jacdif, *data;
// disabled or no equality constraints: return
if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
@@ -520,8 +520,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
if (issparse) {
chain = mjSTACKALLOC(d, nv, int);
chain2 = mjSTACKALLOC(d, nv, int);
buf_ind = mjSTACKALLOC(d, nv, int);
sparse_buf = mjSTACKALLOC(d, nv, mjtNum);
}
// find active equality constraints
@@ -689,8 +687,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
// compute Jacobian: sparse or dense
if (issparse) {
NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain,
chain2, sparse_buf, buf_ind);
NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, chain2);
} else {
mju_addToScl(jac[0], jac[1], -deriv, nv);
}