Use only lower triangle in CSR back-substitution.

PiperOrigin-RevId: 711686157
Change-Id: I4fc98cdfb927e5608ce3a99ea34890ce556fcfd7

Author: yuvaltassa

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)

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<int> 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);
 }