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

Compute diagonal indices in mj_sqrMatTDSparse

Browse Source 
PiperOrigin-RevId: 713714087
Change-Id: Icc8eae74e6e47ba1d12a6e9aa0d774ab006cfe38
This commit is contained in:
Yuval Tassa
2025-01-09 10:16:37 -08:00
committed by Copybara-Service
parent daba00ca9d
commit 00010f5848
6 changed files with 53 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_sparse.c
+9 -3
View File
@@ -724,7 +724,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, int flg_upper) {
mjData* d, int* diagind) {
// allocate space for accumulation buffer and matT
mj_markStack(d);
@@ -838,8 +838,14 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
}
// fill upper triangle
if (flg_upper) {
// diagonal indices requested: fill upper triangle
if (diagind) {
// save diagonal indices
for (int i=0; i < nc; i++) {
diagind[i] = res_rowadr[i] + res_rownnz[i] - 1;
}
// fill upper triangle
for (int i=0; i < nc; i++) {
int start = res_rowadr[i];
int end = start + res_rownnz[i] - 1;