chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add tests for mju_normalize4.

Browse Source 
PiperOrigin-RevId: 648422309
Change-Id: Ie363b396cd854588dc59994ae76059f42f2a54b0
This commit is contained in:
Yuval Tassa
2024-07-01 10:57:26 -07:00
committed by Copybara-Service
parent 0c3698f278
commit c1ade85490
1 changed files with 85 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_util_blas_test.cc
+85
View File
@@ -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