diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 30fdc651..83fdb634 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -238,7 +238,8 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int // sparse reverse-order Cholesky rank-one update: L'*L +/- x*x'; return rank // x is sparse, change in sparsity pattern of mat is not allowed int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, - const int* rownnz, const int* rowadr, int* colind, int x_nnz, int* x_ind, + const int* rownnz, const int* rowadr, const int* colind, + int x_nnz, int* x_ind, mjData* d) { mj_markStack(d); int* buf_ind = mjSTACKALLOC(d, n, int); @@ -264,14 +265,8 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, mat[adr+nnz-1] = r; // update row: mat(r,1:r-1) = (mat(r,1:r-1) + s*x(1:r-1)) / c - int new_nnz = mju_combineSparse(mat + adr, x, 1 / c, (flg_plus ? s / c : -s / c), - nnz-1, i, colind + adr, x_ind, - sparse_buf, buf_ind); - - // check for size change - if (new_nnz != nnz-1) { - mjERROR("varying sparsity pattern"); - } + mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c), + nnz-1, i, colind + adr, x_ind); // update x: x(1:r-1) = c*x(1:r-1) - s*mat(r,1:r-1) int new_x_nnz = mju_combineSparse(x, mat+adr, c, -s, i, nnz-1, x_ind, diff --git a/src/engine/engine_util_solve.h b/src/engine/engine_util_solve.h index 66e59842..6bbdc6bb 100644 --- a/src/engine/engine_util_solve.h +++ b/src/engine/engine_util_solve.h @@ -45,8 +45,8 @@ void mju_cholSolveSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int // sparse reverse-order Cholesky rank-one update: L'*L +/i x*x'; return rank // x is sparse, change in sparsity pattern of mat is not allowed int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus, - const int* rownnz, const int* rowadr, int* colind, int x_nnz, int* x_ind, - mjData* d); + const int* rownnz, const int* rowadr, const int* colind, + int x_nnz, int* x_ind, mjData* d); // band-dense Cholesky decomposition // returns minimum value in the factorized diagonal, or 0 if rank-deficient diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index e32115cf..0dfe4533 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -298,7 +298,7 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, // incomplete combine sparse: dst = a*dst + b*src at common indices void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b, - int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind) { + int dst_nnz, int src_nnz, const int* dst_ind, const int* src_ind) { // check for identical pattern if (dst_nnz == src_nnz) { if (mju_compare(dst_ind, src_ind, dst_nnz)) { diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 1473936b..745675cb 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -64,7 +64,7 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, // incomplete combine sparse: dst = a*dst + b*src at common indices void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b, - int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind); + int dst_nnz, int src_nnz, const int* dst_ind, const int* src_ind); // dst += scl * src, only at common non-zero indices void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src,