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

More efficient implementation of mju_rotVecQuat.

Browse Source 
```
Before:
BM_RotVecQuat_mean           10.1           10.1    833409628  99.055M items/s

After:
BM_RotVecQuat_mean            6.41           6.41  1200000000  156.095M items/s
```
PiperOrigin-RevId: 454601673
Change-Id: Ibb3ce8a6c838a88bdcebd7aaa979b649639edf0b
This commit is contained in:
Kevin Zakka
2022-06-13 07:16:24 -07:00
committed by Copybara-Service
parent 959ed08246
commit ec7133b0d3
2 changed files with 49 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_spatial.c
+9 -3
View File
@@ -32,9 +32,15 @@ void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
// regular processing
else {
mjtNum mat[9];
mju_quat2Mat(mat, quat);
mju_rotVecMat(res, vec, mat);
mjtNum tmp[3];
// tmp = q_w * v + cross(q_xyz, v)
tmp[0] = quat[0]*vec[0] + quat[2]*vec[2] - quat[3]*vec[1];
tmp[1] = quat[0]*vec[1] + quat[3]*vec[0] - quat[1]*vec[2];
tmp[2] = quat[0]*vec[2] + quat[1]*vec[1] - quat[2]*vec[0];
// res = v + 2 * cross(q_xyz, t)
res[0] = vec[0] + 2 * (quat[2]*tmp[2] - quat[3]*tmp[1]);
res[1] = vec[1] + 2 * (quat[3]*tmp[0] - quat[1]*tmp[2]);
res[2] = vec[2] + 2 * (quat[1]*tmp[1] - quat[2]*tmp[0]);
}
}