chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Represent only the lower triangle in Newton solver's reduced dof-dof matrix.

Browse Source 
PiperOrigin-RevId: 712488529
Change-Id: Iad91c72654376539791d7856765a0d0ac9088251
This commit is contained in:
Yuval Tassa
2025-01-06 04:58:06 -08:00
committed by Copybara-Service
parent ee6f4837f3
commit 7eb8231fda
11 changed files with 112 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+2 -2
View File
@@ -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++) {
src/engine/engine_io.c
+17 -8
View File
@@ -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
src/engine/engine_solver.c
+14 -4
View File
@@ -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,
src/engine/engine_util_sparse.c
+24 -18
View File
@@ -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;
}
}
}
src/engine/engine_util_sparse.h
+2 -2
View File
@@ -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);
src/user/user_model.cc
+1 -1
View File
@@ -2495,7 +2495,7 @@ void mjCModel::CopyTree(mjModel* m) {
}
}
}
m->nC = nC = 2 * nOD + nv;
m->nC = nC = nOD + nv;
}
// copy plugin data
test/benchmark/engine_util_sparse_benchmark_test.cc
+9 -7
View File
@@ -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);
}
}
test/engine/engine_core_smooth_test.cc
+6 -2
View File
@@ -481,8 +481,12 @@ TEST_F(CoreSmoothTest, SolveLDs) {
vector<mjtNum> 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<mjtNum> vec(nv);
test/engine/engine_solver_test.cc
+3 -2
View File
@@ -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
test/engine/engine_support_test.cc
+6 -2
View File
@@ -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);
test/engine/engine_util_sparse_test.cc
+28 -28
View File
@@ -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,