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

Add internal functions for 3x3 matmul.

Browse Source 
PiperOrigin-RevId: 616749614
Change-Id: I7f3ac2ffc826076b6c97424324f03ecd9ccd4655
This commit is contained in:
Yuval Tassa
2024-03-18 01:42:58 -07:00
committed by Copybara-Service
parent 398fd108b4
commit dd2580e546
8 changed files with 97 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+2 -2
View File
@@ -815,8 +815,8 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) {
// term1 = body angular momentum about self COM in world frame
mjtNum tmp1[9], tmp2[9];
mju_mulMatMat(tmp1, ximat, inertia, 3, 3, 3); // tmp1 = ximat * inertia
mju_mulMatMatT(tmp2, tmp1, ximat, 3, 3, 3); // tmp2 = ximat * inertia * ximat^T
mju_mulMatMat3(tmp1, ximat, inertia); // tmp1 = ximat * inertia
mju_mulMatMatT3(tmp2, tmp1, ximat); // tmp2 = ximat * inertia * ximat^T
mju_mulMatMat(term1, tmp2, jacr, 3, 3, nv); // term1 = ximat * inertia * ximat^T * jacr
// location of body COM w.r.t subtree COM