Add clarifying comments to mju_transformSpatial
PiperOrigin-RevId: 782842640 Change-Id: I1e6fc27a860b4afc3a00ab4be6d5cb2897b4426b
This commit is contained in:
committed by
Copybara-Service
parent
11e695b4c2
commit
7d5b360dea
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user