From d5292976de7aed080672b0a23b5617271d040ff6 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 Sep 2023 07:13:37 -0700 Subject: [PATCH] Add internal utility for zeroing int vectors. Remove unnecessary branches in related functions. PiperOrigin-RevId: 562768873 Change-Id: I3964a7a953d95bcd4e29137a7b590be5ffa6e311 --- src/engine/engine_core_constraint.c | 11 +++++------ src/engine/engine_io.c | 2 +- src/engine/engine_island.c | 3 +-- src/engine/engine_solver.c | 10 +++++----- src/engine/engine_support.c | 4 +--- src/engine/engine_util_blas.c | 8 ++------ src/engine/engine_util_misc.c | 11 ++++++++--- src/engine/engine_util_misc.h | 3 +++ src/engine/engine_util_solve.c | 3 +-- src/engine/engine_util_sparse.c | 6 +++--- 10 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 98857258..b44ae2b3 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -252,7 +251,7 @@ int mj_addConstraint(const mjModel* m, mjData* d, // copy if not empty if (NV) { - memcpy(ind + adr[nefc+i], chain, sizeof(int)*NV); + mju_copyInt(ind + adr[nefc+i], chain, NV); mju_copy(J + adr[nefc+i], jac + i*NV, NV); } } @@ -644,11 +643,11 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // add first or second chain if (j == 0) { NV = d->ten_J_rownnz[id[j]]; - memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int)); + mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV); } else { NV2 = d->ten_J_rownnz[id[j]]; - memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int)); + mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); mju_copy(jac[j], d->ten_J+d->ten_J_rowadr[id[j]], NV2); } } else { @@ -1530,10 +1529,10 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) { } else { if (!j) { NV = d->ten_J_rownnz[id[j]]; - memcpy(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV*sizeof(int)); + mju_copyInt(chain, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV); } else { NV2 = d->ten_J_rownnz[id[j]]; - memcpy(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2*sizeof(int)); + mju_copyInt(chain2, d->ten_J_colind+d->ten_J_rowadr[id[j]], NV2); } } } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 5df0f59e..e78b4a37 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -874,7 +874,7 @@ static void makeDSparse(const mjModel* m, mjData* d) { } // populate colind - memcpy(remaining, rownnz, nv * sizeof(int)); + mju_copyInt(remaining, rownnz, nv); for (int i = nv - 1; i >= 0; i--) { // init at diagonal remaining[i]--; diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 2c0d7a6d..0c6771a8 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -76,7 +75,7 @@ int mj_floodFill(int* island, int nr, const int* rownnz, const int* rowadr, cons island[v] = nisland; // push adjacent vertices onto stack - memcpy(stack + nstack, colind + rowadr[v], rownnz[v]*sizeof(int)); + mju_copyInt(stack + nstack, colind + rowadr[v], rownnz[v]); nstack += rownnz[v]; } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index dd058a04..8e4ab123 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -464,7 +464,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { } // process state - memcpy(oldstate, d->efc_state, nefc*sizeof(int)); + mju_copyInt(oldstate, d->efc_state, nefc); int nactive = dualState(m, d); int nchange = 0; for (int i=0; i < nefc; i++) { @@ -680,7 +680,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { } // process state - memcpy(oldstate, d->efc_state, nefc*sizeof(int)); + mju_copyInt(oldstate, d->efc_state, nefc); int nactive = dualState(m, d); int nchange = 0; for (int i=0; i < nefc; i++) { @@ -1314,7 +1314,7 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) { for (int r=0; r < dim; r++) { // copy data for this row mju_copy(LTJ_row, LTJ+r*nnz, nnz); - memcpy(LTJ_ind, d->efc_J_colind+d->efc_J_rowadr[i+r], nnz*sizeof(int)); + mju_copyInt(LTJ_ind, d->efc_J_colind+d->efc_J_rowadr[i+r], nnz); // update mju_cholUpdateSparse(ctx->Hcone, LTJ_row, nv, 1, @@ -1476,7 +1476,7 @@ static void HessianIncremental(const mjModel* m, mjData* d, // scale vec, copy colind mju_scl(vec, d->efc_J+adr, mju_sqrt(d->efc_D[i]), nnz); - memcpy(vec_ind, d->efc_J_colind+adr, nnz*sizeof(int)); + mju_copyInt(vec_ind, d->efc_J_colind+adr, nnz); // sparse update rank = mju_cholUpdateSparse(ctx->H, vec, nv, flag_update, @@ -1563,7 +1563,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New mju_copy(gradold, ctx.grad, nv); mju_copy(Mgradold, ctx.Mgrad, nv); } - memcpy(oldstate, d->efc_state, nefc*sizeof(int)); + mju_copyInt(oldstate, d->efc_state, nefc); mjtNum oldcost = ctx.cost; // update diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 14cd04d5..44a1a20c 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -14,8 +14,6 @@ #include "engine/engine_support.h" -#include - #include #include #include @@ -1161,7 +1159,7 @@ void mj_copyM2DSparse(const mjModel* m, mjData* d, mjtNum* dst, const mjtNum* sr // init remaining int* remaining = mj_stackAllocInt(d, nv); - memcpy(remaining, d->D_rownnz, nv * sizeof(int)); + mju_copyInt(remaining, d->D_rownnz, nv); // copy data for (int i = nv - 1; i >= 0; i--) { diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index f0b19daa..31a77a58 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -238,9 +238,7 @@ mjtNum mju_normalize4(mjtNum vec[4]) { // res = 0 void mju_zero(mjtNum* res, int n) { - if (n > 0) { - memset(res, 0, n*sizeof(mjtNum)); - } + memset(res, 0, n*sizeof(mjtNum)); } @@ -256,9 +254,7 @@ void mju_fill(mjtNum* res, mjtNum val, int n) { // res = vec void mju_copy(mjtNum* res, const mjtNum* vec, int n) { - if (n > 0) { - memcpy(res, vec, n*sizeof(mjtNum)); - } + memcpy(res, vec, n*sizeof(mjtNum)); } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index a11bbd00..924d0bfc 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1209,9 +1209,14 @@ int mju_isZero(mjtNum* vec, int n) { // set integer vector to 0 void mju_zeroInt(int* res, int n) { - if (n > 0) { - memset(res, 0, n*sizeof(int)); - } + memset(res, 0, n*sizeof(int)); +} + + + +// copy int vector vec into res +void mju_copyInt(int* res, const int* vec, int n) { + memcpy(res, vec, n*sizeof(int)); } diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index c3150c88..08ca64fa 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -122,6 +122,9 @@ MJAPI int mju_isZero(mjtNum* vec, int n); // set integer vector to 0 MJAPI void mju_zeroInt(int* res, int n); +// copy int vector vec into res +MJAPI void mju_copyInt(int* res, const int* vec, int n); + // standard normal random number generator (optional second number) MJAPI mjtNum mju_standardNormal(mjtNum* num2); diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 933b2d90..d5dafbb5 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -592,7 +591,7 @@ void mju_factorLUSparse(mjtNum* LU, int n, int* scratch, int* remaining = scratch; // set remaining = rownnz - memcpy(remaining, rownnz, n*sizeof(int)); + mju_copyInt(remaining, rownnz, n); // diagonal elements (i,i) for (int i=n-1; i >= 0; i--) { diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index af1909ae..cde53635 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -273,8 +273,8 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, // copy dst into buf if (dst_nnz) { - memcpy(buf, dst, dst_nnz*sizeof(mjtNum)); - memcpy(buf_ind, dst_ind, dst_nnz*sizeof(int)); + mju_copy(buf, dst, dst_nnz); + mju_copyInt(buf_ind, dst_ind, dst_nnz); } // prepare to merge buf and src into dst @@ -599,7 +599,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, // if rowsuper, use the previous row sparsity structure if (rowsuperT && i > 0 && rowsuperT[i-1]) { res_rownnz[i] = res_rownnz[i-1]; - memcpy(cols, res_colind+res_rowadr[i-1], res_rownnz[i]*sizeof(int)); + mju_copyInt(cols, res_colind+res_rowadr[i-1], res_rownnz[i]); } // iterate through each row of M'