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

Add clarifying comments to mju_transformSpatial

Browse Source 
PiperOrigin-RevId: 782842640
Change-Id: I1e6fc27a860b4afc3a00ab4be6d5cb2897b4426b
This commit is contained in:
Yuval Tassa
2025-07-14 04:02:06 -07:00
committed by Copybara-Service
parent 11e695b4c2
commit 7d5b360dea
1 changed files with 5 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_spatial.c
+5 -2
View File
@@ -491,7 +491,10 @@ void mju_mulDofVec(mjtNum* res, const mjtNum* dof, const mjtNum* vec, int n) {
// transform 6D motion or force vector between frames // transform 6D motion or force vector between frames
// rot is 3-by-3 matrix; flg_force determines vector type (motion or force) // flg_force: determines vector type (motion or force)
// rotnew2old: rotation that maps vectors from new to old frame,
// its columns are the new frame's axes, expressed in the old frame
// oldpos and newpos are expressed in old frame
void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force, void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force,
const mjtNum newpos[3], const mjtNum oldpos[3], const mjtNum newpos[3], const mjtNum oldpos[3],
const mjtNum rotnew2old[9]) { const mjtNum rotnew2old[9]) {
@@ -508,7 +511,7 @@ void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force,
mju_sub3(tran+3, vec+3, cros); mju_sub3(tran+3, vec+3, cros);
} }
// apply rotation if provided // if provided, apply old -> new rotation
if (rotnew2old) { if (rotnew2old) {
mju_mulMatTVec3(res, rotnew2old, tran); mju_mulMatTVec3(res, rotnew2old, tran);
mju_mulMatTVec3(res+3, rotnew2old, tran+3); mju_mulMatTVec3(res+3, rotnew2old, tran+3);