Add zero vector shortcut to mju_rotVecQuat.

PiperOrigin-RevId: 597504456
Change-Id: I23580fc08571db97e984a87cba3f4d14e67d6b3a

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];
+    mjtNum tmp[3] = {
-    tmp[1] = quat[0]*vec[1] + quat[3]*vec[0] - quat[1]*vec[2];
+      quat[0]*vec[0] + quat[2]*vec[2] - quat[3]*vec[1],
-    tmp[2] = quat[0]*vec[2] + quat[1]*vec[1] - quat[2]*vec[0];
+      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]);