diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 899d935c..7568b0f6 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1357,7 +1357,7 @@ static mjtNum CGsearch(mjCGContext* ctx, mjtNum tolerance, mjtNum ls_iterations) // compute Mv = M * v mju_mulSymVecSparse(ctx->Mv, ctx->M, ctx->search, nv, - ctx->M_rownnz, ctx->M_rowadr, ctx->M_diagnum, ctx->M_colind); + ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind); // compute Jv = J * search (dense or sparse) if (!ctx->is_sparse) { @@ -1887,7 +1887,7 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int island, int maxiter, // compute Ma = M * qacc mju_mulSymVecSparse(ctx.Ma, ctx.M, ctx.qacc, nv, - ctx.M_rownnz, ctx.M_rowadr, ctx.M_diagnum, ctx.M_colind); + ctx.M_rownnz, ctx.M_rowadr, ctx.M_colind); // compute Jaref = J * qacc - aref (dense or sparse) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 8d364c5e..17e960a3 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -227,7 +227,7 @@ void mju_addToSymSparse(mjtNum* res, const mjtNum* mat, int n, void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat, const mjtNum* restrict vec, int n, const int* restrict rownnz, const int* restrict rowadr, - const int* restrict diagnum, const int* restrict colind) { + const int* restrict colind) { // clear res mju_zero(res, n); @@ -240,13 +240,6 @@ void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat, // diagonal res[i] = row[diag] * vec[i]; - // TODO: consider using SIMD if diagnum[i] >= 4 - - // shortcut for diagonal row/column - if (diagnum[i]) { - continue; - } - // off-diagonals const int* ind = colind + adr; for (int k=diag-1; k >= 0; k--) { diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 616b4d21..ab7ea0f3 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -64,8 +64,7 @@ MJAPI void mju_addToSymSparse(mjtNum* res, const mjtNum* mat, int n, // multiply symmetric matrix (only lower triangle represented) by vector: // res = (mat + strict_upper(mat')) * vec MJAPI void mju_mulSymVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n, - const int* rownnz, const int* rowadr, const int* diagnum, - const int* colind); + const int* rownnz, const int* rowadr, const int* colind); // compress sparse matrix, remove elements with abs(value) <= minval, return total non-zeros MJAPI int mju_compressSparse(mjtNum* mat, int nr, int nc, diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index 10875a67..7fee2127 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -1075,7 +1075,7 @@ TEST_F(EngineUtilSparseTest, MjuMulSymVecSparse) { constexpr int nnz = 9; mjtNum mat[n*n] = {1, 0, 0, 0, - -1, 2, 0, 0, // spurious (ignored) -1 at (1, 0) + 0, 2, 0, 0, 3, 0, 4, 0, 5, 6, 7, 8}; @@ -1090,12 +1090,11 @@ TEST_F(EngineUtilSparseTest, MjuMulSymVecSparse) { int rowadr[n]; int colind[nnz]; mju_dense2sparse(mat_sparse, mat, n, n, rownnz, rowadr, colind, nnz); - int diagnum[n] = {0, 1, 0, 0}; // multiply: res = (mat + strict_upper(mat')) * vec mjtNum vec[n] = {4, 3, 2, 1}; mjtNum res[n]; - mju_mulSymVecSparse(res, mat_sparse, vec, n, rownnz, rowadr, diagnum, colind); + mju_mulSymVecSparse(res, mat_sparse, vec, n, rownnz, rowadr, colind); // dense multiply mjtNum res2[n];