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

Add zero vector shortcut to mju_rotVecQuat.

Browse Source 
PiperOrigin-RevId: 597504456
Change-Id: I23580fc08571db97e984a87cba3f4d14e67d6b3a
This commit is contained in:
Yuval Tassa
2024-01-11 03:18:35 -08:00
committed by Copybara-Service
parent d4e8070a3f
commit 60cd7248c2
1 changed files with 12 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_spatial.c
+12 -5
View File
@@ -25,18 +25,25 @@
// rotate vector by quaternion
void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
// zero vec: zero res
if (vec[0] == 0 && vec[1] == 0 && vec[2] == 0) {
mju_zero3(res);
}
// null quat: copy vec
if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
else if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) {
mju_copy3(res, vec);
}
// regular processing
else {
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];
mjtNum tmp[3] = {
quat[0]*vec[0] + quat[2]*vec[2] - quat[3]*vec[1],
quat[0]*vec[1] + quat[3]*vec[0] - quat[1]*vec[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]);