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

Allow RHS vector in mj_mulM_island to use uncompressed memory.

Browse Source 
PiperOrigin-RevId: 562605681
Change-Id: If34bceed9eade59957ac9fd9b6a41fb736b58024
This commit is contained in:
Yuval Tassa
2023-09-04 13:55:56 -07:00
committed by Copybara-Service
parent 600c12533d
commit 1aa375ef9a
3 changed files with 31 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+14 -4
View File
@@ -885,7 +885,8 @@ void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
// multiply vector by inertia matrix for one dof island
void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec, int island) {
void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec,
int island, int flg_vecunc) {
// if no island, call regular function
if (island < 0) {
mj_mulM(m, d, res, vec);
@@ -913,7 +914,11 @@ void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum
int adr = Madr[i];
// diagonal
res[k] = M[adr]*vec[k];
if (flg_vecunc) {
res[k] = M[adr]*vec[i];
} else {
res[k] = M[adr]*vec[k];
}
// simple dof: continue
if (simplenum[i]) {
@@ -925,8 +930,13 @@ void mj_mulM_island(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum
while (j >= 0) {
adr++;
int l = islandind[j];
res[k] += M[adr]*vec[l];
res[l] += M[adr]*vec[k];
if (flg_vecunc) {
res[k] += M[adr]*vec[j];
res[l] += M[adr]*vec[i];
} else {
res[k] += M[adr]*vec[l];
res[l] += M[adr]*vec[k];
}
// advance to parent
j = parentid[j];