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

Add mju_addToSymSparse

Browse Source 
- Internal engine function to add a symmetric sparse matrix to a dense matrix.
- Also minor refactors to related functions.

PiperOrigin-RevId: 757954336
Change-Id: I8d8a48bcbbd5d6c618ae097a87df2bc1e6f5ef1d
This commit is contained in:
Yuval Tassa
2025-05-12 16:18:51 -07:00
committed by Copybara-Service
parent 755564a348
commit 436b5a8e1f
7 changed files with 109 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_blas.c
+15 -7
View File
@@ -843,9 +843,9 @@ void mju_mulMatMatT(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2,
}
// compute M'*diag*M (diag=NULL: compute M'*M)
void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, int nc) {
// compute M'*diag*M (diag=NULL: compute M'*M), upper triangle optional
void mju_sqrMatTD_impl(mjtNum* res, const mjtNum* mat, const mjtNum* diag,
int nr, int nc, int flg_upper) {
mjtNum tmp;
// half of MatMat routine: only lower triangle
@@ -870,15 +870,23 @@ void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, in
}
}
// make symmetric
for (int i=0; i < nc; i++) {
for (int j=i+1; j < nc; j++) {
res[i*nc+j] = res[j*nc+i];
// flg_upper is set: make symmetric
if (flg_upper) {
for (int i=0; i < nc; i++) {
for (int j=i+1; j < nc; j++) {
res[i*nc+j] = res[j*nc+i];
}
}
}
}
// compute M'*diag*M (diag=NULL: compute M'*M)
void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, int nc) {
mju_sqrMatTD_impl(res, mat, diag, nr, nc, /*flg_upper=*/ 1);
}
// multiply matrices, first argument transposed
void mju_mulMatTMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2,