diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 3cf3ab37..a52dffad 100644 --- a/src/engine/engine_util_spatial.c +++ b/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]; - mju_quat2Mat(mat, quat); - mju_rotVecMat(res, vec, mat); + 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]; + // 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]); } } diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index acf00619..04370298 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/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 #include #include #include +#include #include +#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