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

Add mjSAMEFRAME_BODYROT and mjSAMEFRAME_INERTIAROT frame alignment shortcuts.

Browse Source 
PiperOrigin-RevId: 666318799
Change-Id: I0af1824d2bd9eaca7b48a9e33e09f873dd33b22d
This commit is contained in:
Yuval Tassa
2024-08-22 06:07:30 -07:00
committed by Copybara-Service
parent 8b03daa09b
commit 2efbd31762
7 changed files with 83 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+4
View File
@@ -1620,6 +1620,8 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
if (xpos && pos) {
switch (sf) {
case mjSAMEFRAME_NONE:
case mjSAMEFRAME_BODYROT:
case mjSAMEFRAME_INERTIAROT:
mju_mulMatVec3(xpos, d->xmat+9*body, pos);
mju_addTo3(xpos, d->xpos+3*body);
break;
@@ -1641,9 +1643,11 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
mju_quat2Mat(xmat, tmp);
break;
case mjSAMEFRAME_BODY:
case mjSAMEFRAME_BODYROT:
mju_copy(xmat, d->xmat+9*body, 9);
break;
case mjSAMEFRAME_INERTIA:
case mjSAMEFRAME_INERTIAROT:
mju_copy(xmat, d->ximat+9*body, 9);
break;
}