More efficient implementation of mju_rotVecQuat.

```
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

src/engine/engine_util_spatial.c

@@ -32,9 +32,15 @@ void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
 
   // regular processing
   else {
-    mjtNum mat[9];
+    mjtNum tmp[3];
-    mju_quat2Mat(mat, quat);
+    // tmp = q_w * v + cross(q_xyz, v)
-    mju_rotVecMat(res, vec, mat);
+    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]);
   }
 }
 

test/engine/engine_util_spatial_test.cc

@@ -14,14 +14,15 @@
 
 // Tests for engine/engine_util_spatial.c
 
-#include "src/engine/engine_util_spatial.h"
-
 #include <cmath>
 #include <vector>
 
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
+#include <mujoco/mjmodel.h>
 #include <mujoco/mjtnum.h>
+#include "src/engine/engine_util_blas.h"
+#include "src/engine/engine_util_spatial.h"
 #include "test/fixture.h"
 
 namespace mujoco {
@@ -102,5 +103,42 @@ TEST_F(RotVecQuatTest, TinyRotation) {
   );
 }
 
+// Alternative way of rotating a vector by explicitly converting the quaternion to a 3x3 matrix
+void RotVecQuatWithMatrix(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]) {
+  if (quat[0]==1 && quat[1]==0 && quat[2]==0 && quat[3]==0) {
+    mju_copy3(res, vec);
+  } else {
+    mjtNum mat[9];
+    mju_quat2Mat(mat, quat);
+    mju_rotVecMat(res, vec, mat);
+  }
+}
+
+TEST_F(RotVecQuatTest, TestEquivalence) {
+  mjtNum resultActual[3], resultExpected[3], quat[4];
+  // List of rotation axes
+  mjtNum vecs[5][3] = {
+      {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {-0.5, 1, -0.5}, {1.22, -2.33, 3.44}};
+  // List of angles to rotate by, in degrees
+  mjtNum angles[6] = {0.0, 1e-8, 31, 47, 181, 271};
+  static const mjtNum eps = 1e-15;
+  for (auto vec: vecs) {
+    // Unit-normalize the vector
+    mju_normalize3(vec);
+    for (auto angleDegree: angles) {
+      // Convert the axis-angle to a quaternion
+      auto angleRad = angleDegree * mjPI / 180;
+      mju_axisAngle2Quat(quat, vec, angleRad);
+      // Rotate
+      mju_rotVecQuat(resultActual, vec, quat);
+      RotVecQuatWithMatrix(resultExpected, vec, quat);
+      // Compare
+      EXPECT_NEAR(resultExpected[0], resultActual[0], eps);
+      EXPECT_NEAR(resultExpected[1], resultActual[1], eps);
+      EXPECT_NEAR(resultExpected[2], resultActual[2], eps);
+    }
+  }
+}
+
 }  // namespace
 }  // namespace mujoco