Add tests for mju_normalize4.

PiperOrigin-RevId: 648422309
Change-Id: Ie363b396cd854588dc59994ae76059f42f2a54b0

test/engine/engine_util_blas_test.cc

@@ -16,6 +16,8 @@
 
 #include "src/engine/engine_util_blas.h"
 
+#include <random>
+
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <mujoco/mjtnum.h>
@@ -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<mjtNum> 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