From 7eb8231fdaf55429eb801740d2a3c34e2cd57b85 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 6 Jan 2025 04:58:06 -0800 Subject: [PATCH] Represent only the lower triangle in Newton solver's reduced dof-dof matrix. PiperOrigin-RevId: 712488529 Change-Id: Iad91c72654376539791d7856765a0d0ac9088251 --- src/engine/engine_core_constraint.c | 4 +- src/engine/engine_io.c | 25 ++++++--- src/engine/engine_solver.c | 18 ++++-- src/engine/engine_util_sparse.c | 42 ++++++++------ src/engine/engine_util_sparse.h | 4 +- src/user/user_model.cc | 2 +- .../engine_util_sparse_benchmark_test.cc | 16 +++--- test/engine/engine_core_smooth_test.cc | 8 ++- test/engine/engine_solver_test.cc | 5 +- test/engine/engine_support_test.cc | 8 ++- test/engine/engine_util_sparse_test.cc | 56 +++++++++---------- 11 files changed, 112 insertions(+), 76 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index de206a59..10120294 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2176,12 +2176,12 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { // AR = JM2 * JM2' mju_sqrMatTDSparseInit(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, rownnzT, - rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d); + rowadrT, colindT, rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); mju_sqrMatTDSparse(d->efc_AR, JM2T, JM2, NULL, nv, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_colind, rownnzT, rowadrT, colindT, NULL, - rownnz, rowadr, colind, rowsuper, d); + rownnz, rowadr, colind, rowsuper, d, /*flg_upper=*/1); // add R to diagonal of AR for (int i=0; i < nefc; i++) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 34cc8dbb..1cbfa804 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -943,8 +943,11 @@ static void makeDofDofSparse(const mjModel* m, mjData* d, // process below diagonal unless reduced and dof is simple if (!reduced || !m->dof_simplenum[i]) { while ((j = m->dof_parentid[j]) >= 0) { + // both reduced and non-reduced have lower triangle rownnz[i]++; - rownnz[j]++; + + // only non-reduced has upper triangle + if (!reduced) rownnz[j]++; } } } @@ -969,8 +972,11 @@ static void makeDofDofSparse(const mjModel* m, mjData* d, remaining[i]--; colind[rowadr[i] + remaining[i]] = j; - remaining[j]--; - colind[rowadr[j] + remaining[j]] = i; + // only non-reduced has upper triangle + if (!reduced) { + remaining[j]--; + colind[rowadr[j] + remaining[j]] = i; + } } } } @@ -1152,8 +1158,11 @@ static void copyM2Sparse(const mjModel* m, mjData* d, int* dst, const int* src, remaining[i]--; dst[rowadr[i] + remaining[i]] = src[adr]; - remaining[j]--; - dst[rowadr[j] + remaining[j]] = src[adr]; + // only non-reduced has upper triangle + if (!reduced) { + remaining[j]--; + dst[rowadr[j] + remaining[j]] = src[adr]; + } adr++; } @@ -1172,7 +1181,7 @@ static void copyM2Sparse(const mjModel* m, mjData* d, int* dst, const int* src, -// integer valued dst[M] = src[D lower], handle different sparsity representations +// integer valued dst[M] = src[D lower] static void copyD2MSparse(const mjModel* m, const mjData* d, int* dst, const int* src) { int nv = m->nv; @@ -1197,7 +1206,7 @@ static void copyD2MSparse(const mjModel* m, const mjData* d, int* dst, const int // construct index mappings between M <-> D and M -> C -static void makeDmap(const mjModel* m, mjData* d) { +static void makeDofDofmap(const mjModel* m, mjData* d) { int nM = m->nM, nC = m->nC, nD = m->nD; mj_markStack(d); @@ -1955,7 +1964,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { // make C makeDofDofSparse(m, d, d->C_rownnz, d->C_rowadr, d->C_diag, d->C_colind, /*reduced=*/1); - makeDmap(m, d); + makeDofDofmap(m, d); } // restore pluginstate and plugindata diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index ac506fa4..39ca9b30 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1403,7 +1403,7 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { mju_sqrMatTDSparseInit(ctx->H_rownnz, ctx->H_rowadr, nv, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d); + d, /*flg_upper=*/0); // add nC to Hessian total nonzeros (unavoidable overcounting since H_colind is still unknown) ctx->nH = m->nC + ctx->H_rowadr[nv - 1] + ctx->H_rownnz[nv - 1]; @@ -1424,14 +1424,24 @@ static void MakeHessian(const mjModel* m, mjData* d, mjCGContext* ctx) { ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d); + d, /*flg_upper=*/0); // add mass matrix: H = J'*D*J + C mj_addMSparse(m, d, ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, ctx->C, d->C_rownnz, d->C_rowadr, d->C_colind); + // transiently compute H'; mju_cholFactorNNZ is memory-contiguous in upper triangle layout + mj_markStack(d); + int* HT_rownnz = mjSTACKALLOC(d, nv, int); + int* HT_rowadr = mjSTACKALLOC(d, nv, int); + int* HT_colind = mjSTACKALLOC(d, ctx->nH, int); + mju_transposeSparse(NULL, NULL, nv, nv, + HT_rownnz, HT_rowadr, HT_colind, + ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind); + // count total and row non-zeros of reverse-Cholesky factor L - ctx->nL = mju_cholFactorNNZ(ctx->L_rownnz, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, d); + ctx->nL = mju_cholFactorNNZ(ctx->L_rownnz, HT_rownnz, HT_rowadr, HT_colind, nv, d); + mj_freeStack(d); // compute L row adresses: rowadr = cumsum(rownnz) ctx->L_rowadr[0] = 0; @@ -1508,7 +1518,7 @@ static void FactorizeHessian(const mjModel* m, mjData* d, mjCGContext* ctx, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, d->efc_JT_rowsuper, - d); + d, /*flg_upper=*/0); // add mass matrix: H = J'*D*J + C mj_addMSparse(m, d, ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 80c3f256..444edbf2 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -628,7 +628,7 @@ void mju_superSparse(int nr, int* rowsuper, void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, const int* rownnz, const int* rowadr, const int* colind, const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d) { + const int* rowsuperT, mjData* d, int flg_upper) { mj_markStack(d); int* chain = mjSTACKALLOC(d, 2*nr, int); int nchain = 0; @@ -640,8 +640,10 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, res_rownnz[r] = res_rownnz[r - 1]; // fill in upper triangle - for (int j=0; j < nchain; j++) { - res_rownnz[res_colind[j]]++; + if (flg_upper) { + for (int j=0; j < nchain; j++) { + res_rownnz[res_colind[j]]++; + } } // update chain with diagonal @@ -691,15 +693,17 @@ void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, res_colind = chain + inew; // update upper triangle - int nchain_end = nchain; + if (flg_upper) { + int nchain_end = nchain; - // avoid double counting. - if (nchain > 0 && res_colind[nchain-1] == r) { - nchain_end = nchain - 1; - } + // avoid double counting + if (nchain > 0 && res_colind[nchain-1] == r) { + nchain_end = nchain - 1; + } - for (int j=0; j < nchain_end; j++) { - res_rownnz[res_colind[j]]++; + for (int j=0; j < nchain_end; j++) { + res_rownnz[res_colind[j]]++; + } } } } @@ -731,7 +735,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, - mjData* d) { + mjData* d, int flg_upper) { // allocate space for accumulation buffer and matT mj_markStack(d); @@ -846,13 +850,15 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, // fill upper triangle - for (int i=0; i < nc; i++) { - int start = res_rowadr[i]; - int end = start + res_rownnz[i] - 1; - for (int j=start; j < end; j++) { - int adr = res_rowadr[res_colind[j]] + res_rownnz[res_colind[j]]++; - res[adr] = res[j]; - res_colind[adr] = i; + if (flg_upper) { + for (int i=0; i < nc; i++) { + int start = res_rowadr[i]; + int end = start + res_rownnz[i] - 1; + for (int j=start; j < end; j++) { + int adr = res_rowadr[res_colind[j]] + res_rownnz[res_colind[j]]++; + res[adr] = res[j]; + res_colind[adr] = i; + } } } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index f1f46f4a..62cf6890 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -96,13 +96,13 @@ MJAPI void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, - mjData* d); + mjData* d, int flg_upper); // precount res_rownnz and precompute res_rowadr for mju_sqrMatTDSparse MJAPI void mju_sqrMatTDSparseInit(int* res_rownnz, int* res_rowadr, int nr, const int* rownnz, const int* rowadr, const int* colind, const int* rownnzT, const int* rowadrT, const int* colindT, - const int* rowsuperT, mjData* d); + const int* rowsuperT, mjData* d, int flg_upper); // precompute res_rowadr for mju_sqrMatTDSparse using uncompressed memory MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 3e6134bd..0cf6a4ba 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2495,7 +2495,7 @@ void mjCModel::CopyTree(mjModel* m) { } } } - m->nC = nC = 2 * nOD + nv; + m->nC = nC = nOD + nv; } // copy plugin data diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index d77dc067..838e71a1 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -43,7 +43,7 @@ void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind, const int* rownnz, const int* rowadr, const int* colind, const int* rowsuper, const int* rownnzT, const int* rowadrT, - const int* colindT, const int* rowsuperT, mjData* d) { + const int* colindT, const int* rowsuperT, mjData* d, int unused) { mj_markStack(d); int* chain = mj_stackAllocInt(d, 2 * nc); mjtNum* buffer = mj_stackAllocNum(d, nc); @@ -453,7 +453,8 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, d->efc_JT_rownnz, d->efc_JT_rowadr, - d->efc_JT_colind, d->efc_JT_rowsuper, d); + d->efc_JT_colind, d->efc_JT_rowsuper, d, + /*flg_upper=*/1); // compute H = M + J'*D*J mj_addM(m, d, H, rownnz, rowadr, colind); @@ -578,7 +579,7 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { func(H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, - d->efc_JT_rowsuper, d); + d->efc_JT_rowsuper, d, /*flg_upper=*/1); } } else { for (auto s : state) { @@ -587,10 +588,11 @@ static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); // compute H = J'*D*J, uncompressed layout - mju_sqrMatTDSparse_baseline(H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, - d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, - d->efc_JT_rowsuper, d); + mju_sqrMatTDSparse_baseline( + H, d->efc_J, d->efc_JT, D, d->nefc, m->nv, rownnz, rowadr, colind, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, + d->efc_JT_rownnz, d->efc_JT_rowadr, d->efc_JT_colind, + d->efc_JT_rowsuper, d, /*unused=*/0); } } diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index 1fa16ec2..0cf12290 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -481,8 +481,12 @@ TEST_F(CoreSmoothTest, SolveLDs) { vector LDdense2(nv*nv); mj_fullM(m, LDdense2.data(), d->qLD); - // expect dense matrices to match exactly - for (int i=0; i < nv*nv; i++) EXPECT_EQ(LDdense[i], LDdense2[i]); + // expect lower triangles to match exactly + for (int i=0; i < nv; i++) { + for (int j=0; j < i; j++) { + EXPECT_EQ(LDdense[i*nv+j], LDdense2[i*nv+j]); + } + } // compare LD and LDs vector solve vector vec(nv); diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index d20c3d38..6343ce24 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -61,8 +61,9 @@ static const char* const kIlslandEfcPath = // compare accelerations produced by CG solver with and without islands TEST_F(SolverTest, IslandsEquivalent) { const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); - ASSERT_THAT(model, NotNull()); + char error[1024]; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; model->opt.solver = mjSOL_CG; // use CG solver model->opt.tolerance = 0; // set tolerance to 0 model->opt.enableflags &= ~mjENBL_ISLAND; // disable islands diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 456143cd..8e915328 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -725,8 +725,12 @@ TEST_F(InertiaTest, DenseSameAsSparse) { // dense addM mj_addM(m, d, dst_dense.data(), nullptr, nullptr, nullptr); - // dense comparison, should be same matrix - EXPECT_THAT(dst_dense, ElementsAreArray(dst_sparse)); + // dense comparison, lower triangle should match + for (int i=0; i < nv; i++) { + for (int j=0; j < i; j++) { + EXPECT_EQ(dst_dense[i*nv+j], dst_sparse[i*nv+j]); + } + } // clean up mj_deleteData(d); diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index ade96c0b..37da5155 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -327,7 +327,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -336,7 +336,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -372,7 +372,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -382,7 +382,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -420,7 +420,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4)); @@ -429,7 +429,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 1, 0, 0, 0, 0)); @@ -468,7 +468,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 0, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 2)); @@ -477,7 +477,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(66, 4, 0, 0, 0, 0, 4, 35, 0)); EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 0, 0, 0, 0, 2, 0)); @@ -514,7 +514,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); @@ -523,7 +523,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(5, 6, 4, 6, 9, 0, 4, 16, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); @@ -559,7 +559,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 3)); @@ -568,7 +568,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(1, 2, 0, 4, 0, 0, 2, 13, 0)); EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 1, 0, 0, 0, 2, 0)); @@ -606,7 +606,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 2, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 2)); @@ -615,7 +615,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { mju_sqrMatTDUncompressedInit(rowadrH, 2); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 2, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(66, 4, 4, 35)); EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 1)); @@ -652,7 +652,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); @@ -661,7 +661,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 2, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(14, 18, 8, 18, 27, 0, 8, 32, 0)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); @@ -699,7 +699,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data); + rownnzT, rowadrT, colindT, nullptr, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -708,7 +708,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data); + nullptr, data, 1); EXPECT_THAT(matH, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -747,7 +747,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -756,7 +756,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(14, 14, 14, 14, 14, 14, 14, 14, 14)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -795,7 +795,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); @@ -804,7 +804,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { mju_sqrMatTDUncompressedInit(rowadrH, 3); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10)); EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); @@ -843,7 +843,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 4, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(4, 4, 4, 4)); EXPECT_THAT(rowadrH, ElementsAre(0, 4, 8, 12)); @@ -852,7 +852,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { mju_sqrMatTDUncompressedInit(rowadrH, 4); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 4, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 10, 10, 1, 1, 10, 10)); @@ -895,7 +895,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 5, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0, 0, 0)); EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4, 4, 4)); @@ -904,7 +904,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { mju_sqrMatTDUncompressedInit(rowadrH, 5); mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 5, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT(matH, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)); @@ -945,7 +945,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { // test precount mju_sqrMatTDSparseInit(rownnzH, rowadrH, 7, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data); + rownnzT, rowadrT, colindT, rowsuperT, data, 1); EXPECT_THAT(rownnzH, ElementsAre(7, 7, 7, 7, 7, 7, 7)); EXPECT_THAT(rowadrH, ElementsAre(0, 7, 14, 21, 28, 35, 42)); @@ -954,7 +954,7 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { mju_sqrMatTDUncompressedInit(rowadrH, 7); mju_sqrMatTDSparse(matH, mat, matT, nullptr, 1, 7, rownnzH, rowadrH, colindH, rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data); + rowsuperT, data, 1); EXPECT_THAT( matH, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2,