diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 2a5e629e..ebd342a4 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -417,7 +417,7 @@ void mj_tendon(const mjModel* m, mjData* d) { // clear Jacobian: sparse or dense if (issparse) { - memset(rownnz, 0, nten*sizeof(int)); + mju_zeroInt(rownnz, nten); } else { mju_zero(J, nten*nv); } diff --git a/src/engine/engine_derivative_fd.c b/src/engine/engine_derivative_fd.c index 2cde539b..e095d9de 100644 --- a/src/engine/engine_derivative_fd.c +++ b/src/engine/engine_derivative_fd.c @@ -27,6 +27,7 @@ #include "engine/engine_support.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" +#include "engine/engine_util_misc.h" @@ -202,7 +203,7 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) { int* cnt = mj_stackAllocInt(d, nv); // clear row counters - memset(cnt, 0, nv*sizeof(int)); + mju_zeroInt(cnt, nv); // save qfrc_passive, assume mj_fwdVelocity was called mju_copy(qfrc_passive, d->qfrc_passive, nv); @@ -254,7 +255,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) { int* cnt = mj_stackAllocInt(d, nv); // clear row counters - memset(cnt, 0, nv*sizeof(int)); + mju_zeroInt(cnt, nv); // loop over dofs for (int i=0; i < nv; i++) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index af13d3e0..668891a3 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -836,7 +836,7 @@ static void makeDSparse(const mjModel* m, mjData* d) { int* remaining = mj_stackAllocInt(d, nv); // compute rownnz - memset(rownnz, 0, nv * sizeof(int)); + mju_zeroInt(rownnz, nv); for (int i = nv - 1; i >= 0; i--) { // init at diagonal int j = i; @@ -893,7 +893,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { int* colind = d->B_colind; // set rownnz to subtree dofs counts, including self - memset(rownnz, 0, sizeof(int) * nbody); + mju_zeroInt(rownnz, nbody); for (int i = nbody - 1; i > 0; i--) { rownnz[i] += m->body_dofnum[i]; rownnz[m->body_parentid[i]] += rownnz[i]; @@ -927,7 +927,7 @@ static void makeBSparse(const mjModel* m, mjData* d) { // allocate and clear incremental row counts mjMARKSTACK; int* cnt = mj_stackAllocInt(d, nbody); - memset(cnt, 0, sizeof(int) * nbody); + mju_zeroInt(cnt, nbody); // add subtree dofs to colind for (int i = nbody - 1; i > 0; i--) { diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 3dca27da..2c0d7a6d 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -27,6 +27,7 @@ #include "engine/engine_io.h" #include "engine/engine_support.h" #include "engine/engine_util_errmem.h" +#include "engine/engine_util_misc.h" #ifdef MEMORY_SANITIZER #include @@ -355,7 +356,7 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg int efc_id = -1; // clear treenedge - memset(treenedge, 0, m->ntree*sizeof(int)); + mju_zeroInt(treenedge, m->ntree); int nedge = 0; for (int i=0; i < nefc; i++) { @@ -456,7 +457,7 @@ void mj_island(const mjModel* m, mjData* d) { // compute dof_island, island_dofnum int num_dof_unc = 0; // number of unconstrained dofs - memset(d->island_dofnum, 0, nisland*sizeof(int)); + mju_zeroInt(d->island_dofnum, nisland); for (int i=0; i < nv; i++) { // dof_island int island = tree_island[m->dof_treeid[i]]; @@ -477,7 +478,7 @@ void mj_island(const mjModel* m, mjData* d) { } // reset island_dofnum - memset(d->island_dofnum, 0, nisland*sizeof(int)); + mju_zeroInt(d->island_dofnum, nisland); // compute dof_islandind, island_dofind int num_dof_island = 0; @@ -503,7 +504,7 @@ void mj_island(const mjModel* m, mjData* d) { } // compute efc_island, island_efcnum - memset(d->island_efcnum, 0, nisland*sizeof(int)); + mju_zeroInt(d->island_efcnum, nisland); for (int i=0; i < nefc; i++) { int tree[2]; treeFirst(m, d, tree, i); @@ -519,7 +520,7 @@ void mj_island(const mjModel* m, mjData* d) { } // reset island_efcnum - memset(d->island_efcnum, 0, nisland*sizeof(int)); + mju_zeroInt(d->island_efcnum, nisland); // compute efc_islandind for (int i=0; i < nefc; i++) { diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index dd9a9cbe..a11bbd00 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1207,6 +1207,15 @@ 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)); + } +} + + + // standard normal random number generator (optional second number) mjtNum mju_standardNormal(mjtNum* num2) { const mjtNum scale = 2.0/((mjtNum)RAND_MAX); diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 6260d0c5..c3150c88 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -119,6 +119,9 @@ MJAPI int mju_isBad(mjtNum x); // return 1 if all elements are 0 MJAPI int mju_isZero(mjtNum* vec, int n); +// set integer vector to 0 +MJAPI void mju_zeroInt(int* res, int n); + // standard normal random number generator (optional second number) MJAPI mjtNum mju_standardNormal(mjtNum* num2); diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 903d72c8..af1909ae 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -22,6 +22,7 @@ #include #include "engine/engine_io.h" #include "engine/engine_util_blas.h" +#include "engine/engine_util_misc.h" //------------------------------ sparse operations ------------------------------------------------- @@ -393,7 +394,7 @@ void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind, const int* rownnz, const int* rowadr, const int* colind) { // clear number of non-zeros for each row of transposed - memset(res_rownnz, 0, nc*sizeof(int)); + mju_zeroInt(res_rownnz, nc); // total number of non-zeros of mat int nnz = rowadr[nr-1] + rownnz[nr-1];