From 60cd7248c2f95f4b5820517eb2fc1fa8fb24557a Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Thu, 11 Jan 2024 03:18:35 -0800
Subject: [PATCH] Add zero vector shortcut to `mju_rotVecQuat`.

PiperOrigin-RevId: 597504456
Change-Id: I23580fc08571db97e984a87cba3f4d14e67d6b3a
---
 src/engine/engine_util_spatial.c | 17 ++++++++++++-----
 1 file changed, 12 insertions(+), 5 deletions(-)

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]);