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

Remove unnecessary diagnum argument in mj_solveLD and mj_factorI

Browse Source 
PiperOrigin-RevId: 758669364
Change-Id: Icfefc4a7a1e7d28da373725358ccf9d5f589f689
This commit is contained in:
Yuval Tassa
2025-05-14 07:12:19 -07:00
committed by Copybara-Service
parent b0c36c6b33
commit d8bebdc675
9 changed files with 33 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_smooth.c
+16 -21
View File
@@ -1653,7 +1653,7 @@ void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD
void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
mju_copy(d->qLD, d->M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind);
TM_ADD(mjTIMER_POS_INERTIA);
}
@@ -1661,32 +1661,28 @@ void mj_factorM(const mjModel* m, mjData* d) {
// sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd
void mj_factorI(mjtNum* mat, mjtNum* diaginv, int nv,
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) {
const int* rownnz, const int* rowadr, const int* colind) {
// backward loop over rows
for (int k=nv-1; k >= 0; k--) {
// get row k's address, diagonal index, inverse diagonal value
int rowadr_k = rowadr[k];
int diag_k = rowadr_k + rownnz[k] - 1;
mjtNum invD = 1 / mat[diag_k];
int start = rowadr[k];
int diag = rownnz[k] - 1;
int end = start + diag;
mjtNum invD = 1 / mat[end];
if (diaginv) diaginv[k] = invD;
// skip if simple
if (diagnum[k]) {
continue;
}
// update triangle above row k, inclusive
for (int adr=diag_k - 1; adr >= rowadr_k; adr--) {
// update triangle above row k
for (int adr=end - 1; adr >= start; adr--) {
// tmp = L(k, i) / L(k, k)
mjtNum tmp = mat[adr] * invD;
// update row i < k: L(i, 0..i) -= L(i, 0..i) * L(k, i) / L(k, k)
int i = colind[adr];
mju_addToScl(mat + rowadr[i], mat + rowadr_k, -tmp, rownnz[i]);
// update ith element of row k: L(k, i) /= L(k, k)
mat[adr] = tmp;
mju_addToScl(mat + rowadr[i], mat + start, -tmp, rownnz[i]);
}
// update row k: L(k, :) /= L(k, k)
mju_scl(mat + start, mat + start, invD, diag);
}
}
@@ -1807,11 +1803,11 @@ void mj_solveLD_legacy(const mjModel* m, mjtNum* restrict x, int n,
// in-place sparse backsubstitution: x = inv(L'*D*L)*x
void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv, int nv, int n,
const int* rownnz, const int* rowadr, const int* diagnum, const int* colind) {
const int* rownnz, const int* rowadr, const int* colind) {
// x <- L^-T x
for (int i=nv-1; i > 0; i--) {
// skip diagonal rows
if (diagnum[i]) {
if (rownnz[i] == 1) {
continue;
}
@@ -1862,8 +1858,7 @@ void mj_solveLD(mjtNum* restrict x, const mjtNum* qLD, const mjtNum* qLDiagInv,
// x <- L^-1 x
for (int i=1; i < nv; i++) {
// skip diagonal rows
if (diagnum[i]) {
i += diagnum[i] - 1; // iterating forward: skip ahead, adjust i
if (rownnz[i] == 1) {
continue;
}
@@ -1895,7 +1890,7 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
mju_copy(x, y, n*m->nv);
}
mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n,
d->M_rownnz, d->M_rowadr, m->dof_simplenum, d->M_colind);
d->M_rownnz, d->M_rowadr, d->M_colind);
}