From 7d5b360dea2d5a45eef1ae3e71536b5d8b5860a1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 14 Jul 2025 04:02:06 -0700 Subject: [PATCH] Add clarifying comments to `mju_transformSpatial` PiperOrigin-RevId: 782842640 Change-Id: I1e6fc27a860b4afc3a00ab4be6d5cb2897b4426b --- src/engine/engine_util_spatial.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index dda47cf6..7bef17e3 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -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 -// 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, const mjtNum newpos[3], const mjtNum oldpos[3], 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); } - // apply rotation if provided + // if provided, apply old -> new rotation if (rotnew2old) { mju_mulMatTVec3(res, rotnew2old, tran); mju_mulMatTVec3(res+3, rotnew2old, tran+3);