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

Allow in-place mju_rotVecMat and mju_rotVecMatT (res=vec)

Browse Source 
PiperOrigin-RevId: 456021131
Change-Id: Id38ff5fabd15706d3087bc95d341b4f7a673449f
This commit is contained in:
Yuval Tassa
2022-06-20 01:57:05 -07:00
committed by Copybara-Service
parent b1faec1dc2
commit 08b2db2f62
1 changed files with 16 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_blas.c
+16 -6
View File
@@ -154,18 +154,28 @@ mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]) {
// multiply vector by 3D rotation matrix
void mju_rotVecMat(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
res[0] = mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2];
res[1] = mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2];
res[2] = mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2];
mjtNum tmp[3] = {
mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2],
mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2],
mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2]
};
res[0] = tmp[0];
res[1] = tmp[1];
res[2] = tmp[2];
}
// multiply vector by transposed 3D rotation matrix
void mju_rotVecMatT(mjtNum res[3], const mjtNum vec[3], const mjtNum mat[9]) {
res[0] = mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2];
res[1] = mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2];
res[2] = mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2];
mjtNum tmp[3] = {
mat[0]*vec[0] + mat[3]*vec[1] + mat[6]*vec[2],
mat[1]*vec[0] + mat[4]*vec[1] + mat[7]*vec[2],
mat[2]*vec[0] + mat[5]*vec[1] + mat[8]*vec[2]
};
res[0] = tmp[0];
res[1] = tmp[1];
res[2] = tmp[2];
}