diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 87580444..27b26576 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -27,55 +27,6 @@ //------------------------------ sparse operations ------------------------------------------------- -// dot-product, first vector is sparse -// flg_unc1: is vec1 memory layout uncompressed -mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, - int flg_unc1) { -#ifdef mjUSEAVX - return mju_dotSparse_avx(vec1, vec2, nnz1, ind1, flg_unc1); -#else - int i = 0; - mjtNum res = 0; - int n_4 = nnz1 - 4; - mjtNum res0 = 0; - mjtNum res1 = 0; - mjtNum res2 = 0; - mjtNum res3 = 0; - - - if (flg_unc1) { - for (; i <= n_4; i+=4) { - res0 += vec1[ind1[i+0]] * vec2[ind1[i+0]]; - res1 += vec1[ind1[i+1]] * vec2[ind1[i+1]]; - res2 += vec1[ind1[i+2]] * vec2[ind1[i+2]]; - res3 += vec1[ind1[i+3]] * vec2[ind1[i+3]]; - } - } else { - for (; i <= n_4; i+=4) { - res0 += vec1[i+0] * vec2[ind1[i+0]]; - res1 += vec1[i+1] * vec2[ind1[i+1]]; - res2 += vec1[i+2] * vec2[ind1[i+2]]; - res3 += vec1[i+3] * vec2[ind1[i+3]]; - } - } - res = (res0 + res2) + (res1 + res3); - - // scalar part - if (flg_unc1) { - for (; i < nnz1; i++) { - res += vec1[ind1[i]] * vec2[ind1[i]]; - } - } else { - for (; i < nnz1; i++) { - res += vec1[i] * vec2[ind1[i]]; - } - } - - return res; -#endif // mjUSEAVX -} - - // dot-productX3, first vector is sparse; supernode of size 3 void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2, diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index d1dc2d5d..319af57b 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -25,10 +25,6 @@ extern "C" { //------------------------------ sparse operations ------------------------------------------------- -// dot-product, vec1 is sparse, can be uncompressed -MJAPI mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, - int flg_unc1); - // dot-product, both vectors are sparse, vec2 can be uncompressed MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, int nnz2, const int* ind2, int flg_unc2); @@ -114,6 +110,58 @@ MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); MJAPI int mju_cholFactorNNZ(int* L_rownnz, const int* rownnz, const int* rowadr, const int* colind, int n, mjData* d); +// ------------------------------ inlined functions ------------------------------------------------ + +// dot-product, first vector is sparse +// flg_unc1: is vec1 memory layout uncompressed +static inline +mjtNum mju_dotSparse(const mjtNum* vec1, const mjtNum* vec2, int nnz1, const int* ind1, + int flg_unc1) { +#ifdef mjUSEAVX + return mju_dotSparse_avx(vec1, vec2, nnz1, ind1, flg_unc1); +#else + int i = 0; + mjtNum res = 0; + int n_4 = nnz1 - 4; + mjtNum res0 = 0; + mjtNum res1 = 0; + mjtNum res2 = 0; + mjtNum res3 = 0; + + + if (flg_unc1) { + for (; i <= n_4; i+=4) { + res0 += vec1[ind1[i+0]] * vec2[ind1[i+0]]; + res1 += vec1[ind1[i+1]] * vec2[ind1[i+1]]; + res2 += vec1[ind1[i+2]] * vec2[ind1[i+2]]; + res3 += vec1[ind1[i+3]] * vec2[ind1[i+3]]; + } + } else { + for (; i <= n_4; i+=4) { + res0 += vec1[i+0] * vec2[ind1[i+0]]; + res1 += vec1[i+1] * vec2[ind1[i+1]]; + res2 += vec1[i+2] * vec2[ind1[i+2]]; + res3 += vec1[i+3] * vec2[ind1[i+3]]; + } + } + res = (res0 + res2) + (res1 + res3); + + // scalar part + if (flg_unc1) { + for (; i < nnz1; i++) { + res += vec1[ind1[i]] * vec2[ind1[i]]; + } + } else { + for (; i < nnz1; i++) { + res += vec1[i] * vec2[ind1[i]]; + } + } + + return res; +#endif // mjUSEAVX +} + + #ifdef __cplusplus } #endif