diff --git a/test/engine/engine_util_blas_test.cc b/test/engine/engine_util_blas_test.cc index e2d354d9e2..706c4d17a4 100644 --- a/test/engine/engine_util_blas_test.cc +++ b/test/engine/engine_util_blas_test.cc @@ -16,6 +16,8 @@ #include "src/engine/engine_util_blas.h" +#include + #include #include #include @@ -128,5 +130,88 @@ TEST_F(EngineUtilBlasTest, MjuMulMat3) { } } +// utility: random quaternion, normally distributed +void RandomQuat(mjtNum quat[4], int seed) { + // make distribution using seed + std::mt19937_64 rng; + rng.seed(seed); + std::normal_distribution dist(0, 1); + + // sample + for (int i = 0; i < 4; i++) { + quat[i] = dist(rng); + } +} + +TEST_F(EngineUtilBlasTest, Normalize4IsIdempotent) { + for (int i = 0; i < 10000; ++i) { + // make random quaternion and normalize it + mjtNum quat[4]; + RandomQuat(quat, i); + mju_normalize4(quat); + + // normalize again + mjtNum quat_renormalized[4] = {quat[0], quat[1], quat[2], quat[3]}; + mju_normalize4(quat_renormalized); + + // expect equality + EXPECT_EQ(quat[0], quat_renormalized[0]); + EXPECT_EQ(quat[1], quat_renormalized[1]); + EXPECT_EQ(quat[2], quat_renormalized[2]); + EXPECT_EQ(quat[3], quat_renormalized[3]); + } +} + +TEST_F(EngineUtilBlasTest, Normalize4EdgeCases) { + // zero quat normalizes to unit quat + mjtNum quat[4] = {0}; + mjtNum norm = mju_normalize4(quat); + EXPECT_EQ(norm, 0); + EXPECT_EQ(quat[0], 1); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // small quat normalizes regularly + quat[0] = 0; + quat[1] = mjMINVAL; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, mjMINVAL); + EXPECT_EQ(quat[0], 0); + EXPECT_EQ(quat[1], 1); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // tiny quat normalizes to unit quat + quat[0] = 0; + quat[1] = mjMINVAL/2; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, mjMINVAL/2); + EXPECT_EQ(quat[0], 1); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // near-unit quat is normalized + quat[0] = 1 + mjMINVAL; + quat[1] = 0; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, 1 + mjMINVAL); + EXPECT_EQ(quat[0], 1); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // very-near-unit quat is untouched + quat[0] = 1 + mjMINVAL/2; + quat[1] = 0; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, 1 + mjMINVAL/2); + EXPECT_EQ(quat[0], 1 + mjMINVAL/2); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); +} + } // namespace } // namespace mujoco