diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index d17d06db..641b7bdc 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1578,16 +1578,19 @@ void mj_solveLDs(mjtNum* restrict x, const mjtNum* qLDs, const mjtNum* qLDiagInv const int* rownnz, const int* rowadr, const int* diagind, const int* diagnum, const int* colind) { // x <- L^-T x - for (int i=nv-2; i >= 0; i--) { - // skip diagonal (simple) rows - if (diagnum[i]) { + for (int i=nv-1; i > 0; i--) { + // skip diagonal (simple) rows, exploit sparsity of input vector + if (diagnum[i] || x[i] == 0) { continue; } - int d1 = diagind[i] + 1; - int nnz = rownnz[i] - d1; - int adr = rowadr[i] + d1; - x[i] -= mju_dotSparse(qLDs+adr, x, nnz, colind+adr, /*flg_unc1=*/0); + int d = diagind[i]; + int adr_i = rowadr[i]; + mjtNum x_i = x[i]; + for (int j=0; j < d; j++) { + int adr = adr_i + j; + x[colind[adr]] -= qLDs[adr] * x_i; + } } // x(i) /= D(i,i) diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index dd406caa..1fa16ec2 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -456,53 +456,6 @@ TEST_F(CoreSmoothTest, FactorI) { mj_deleteModel(model); } -// in-place sparse backsubstitution: x = inv(L'*D*L)*x -// like mj_solveLD, but using the CSR representation of L -// variant that only uses the lower triangle of qLDs -static void mj_solveLDsLower(mjtNum* x, const mjtNum* qLDs, - const mjtNum* qLDiagInv, int nv, const int* rownnz, - const int* rowadr, const int* diagind, - const int* diagnum, const int* colind, - int* scratch) { - int* marker = scratch; - for (int i=1; i < nv; i++) { - marker[i] = rowadr[i] + diagind[i] - 1; - } - - // x <- L^-T x - for (int i=nv-2; i >= 0; i--) { - // skip diagonal (simple) rows - if (diagnum[i]) { - continue; - } - - for (int j=i+1; j < nv; j++) { - if (colind[marker[j]] == i) { - x[i] -= qLDs[marker[j]--] * x[j]; - } - } - } - - // x(i) /= D(i,i) - for (int i=0; i < nv; i++) { - x[i] *= qLDiagInv[i]; - } - - // x <- L^-1 x - for (int i=1; i < nv; i++) { - // skip diagonal (simple) rows - if (diagnum[i]) { - i += diagnum[i] - 1; // when iterating forward we can skip ahead - continue; - } - - int d = diagind[i]; - int adr = rowadr[i]; - x[i] -= mju_dotSparse(qLDs+adr, x, d, colind+adr, /*flg_unc1=*/0); - } -} - - TEST_F(CoreSmoothTest, SolveLDs) { const std::string xml_path = GetTestDataFilePath(kInertiaPath); char error[1024]; @@ -537,7 +490,6 @@ TEST_F(CoreSmoothTest, SolveLDs) { for (int i=0; i < nv; i++) vec[i] = vec2[i] = 20 + 30*i; for (int i=0; i < nv; i+=2) vec[i] = vec2[i] = 0; - // use upper triangle mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); mj_solveLDs(vec2.data(), LDs.data(), d->qLDiagInv, nv, d->C_rownnz, d->C_rowadr, d->C_diag, m->dof_simplenum, @@ -548,18 +500,6 @@ TEST_F(CoreSmoothTest, SolveLDs) { EXPECT_FLOAT_EQ(vec[i], vec2[i]); } - // don't use use upper triangle - mj_solveLD(m, vec.data(), 1, d->qLD, d->qLDiagInv); - vector scratch(nv); - mj_solveLDsLower(vec2.data(), LDs.data(), d->qLDiagInv, nv, d->C_rownnz, - d->C_rowadr, d->C_diag, m->dof_simplenum, d->C_colind, - scratch.data()); - - // expect vectors to match up to floating point precision - for (int i=0; i < nv; i++) { - EXPECT_FLOAT_EQ(vec[i], vec2[i]); - } - mj_deleteData(d); mj_deleteModel(m); }