Speed up mju_cholUpdateSparse by not checking for varying sparsity pattern where it is guaranteed to not vary.

PiperOrigin-RevId: 720126576
Change-Id: I3a43bdd0259656fab462b1cd155cb422982e970d

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),
+    mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c),
-                                    nnz-1, i, colind + adr, x_ind,
+                         nnz-1, i, colind + adr, x_ind);
-                                    sparse_buf, buf_ind);
-
-    // check for size change
-    if (new_nnz != nnz-1) {
-      mjERROR("varying sparsity pattern");
-    }
 
     // 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,

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,
+                         const int* rownnz, const int* rowadr, const int* colind,
-                         mjData* d);
+                         int x_nnz, int* x_ind, mjData* d);
 
 // band-dense Cholesky decomposition
 //  returns minimum value in the factorized diagonal, or 0 if rank-deficient

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)) {

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,