diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 576d0274..023b9459 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -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]);