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

Adjust rotEPS for single precision in mju_mat2Rot.

Browse Source 
This change defines rotEPS as 1e-6f when mjUSESINGLE is defined, and 1e-9 otherwise. With the larger epsilon for single precision, the algorithm converges in fewer iterations, allowing the test assertion for maximum iterations to be simplified to a constant 150.

PiperOrigin-RevId: 959070496
Change-Id: I4f6fae056cd71955d3921c01a9929af2cdd81fac
This commit is contained in:
Alessio Quaglino
2026-08-04 09:37:44 -07:00
committed by Copybara-Service
parent b3ef7a8c2f
commit 63c7175ed5
2 changed files with 11 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_util_spatial.c
+8 -4
View File
@@ -278,12 +278,16 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) {
// extract 3D rotation from an arbitrary 3x3 matrix // extract 3D rotation from an arbitrary 3x3 matrix
#ifdef mjUSESINGLE
static const mjtNum rotEPS = 1e-6f;
#else
static const mjtNum rotEPS = 1e-9; static const mjtNum rotEPS = 1e-9;
#endif
int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) { int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) {
// Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin. "A // Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin.
// robust method to extract the rotational part of deformations." In // "A robust method to extract the rotational part of deformations."
// Proceedings of the 9th International Conference on Motion in Games, pp. // In Proceedings of the 9th International Conference on Motion in Games,
// 55-60. 2016. // pp. 55-60. 2016.
int iter; int iter;
mjtNum col1_mat[3] = {mat[0], mat[3], mat[6]}; mjtNum col1_mat[3] = {mat[0], mat[3], mat[6]};
test/engine/engine_util_spatial_test.cc
+3 -4
View File
@@ -218,9 +218,8 @@ TEST_F(Mat2RotTest, RotationFromArbitraryMatrix) {
// calculate rotational part of the matrix // calculate rotational part of the matrix
mjtNum quat[4] = {1, 0, 0, 0}; mjtNum quat[4] = {1, 0, 0, 0};
int niter = mju_mat2Rot(quat, mat); int niter = mju_mat2Rot(quat, mat);
EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-6), target)); EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-5), target));
int max_iter = static_cast<int>(MjTol(150, 500)); EXPECT_LE(niter, 150);
EXPECT_LE(niter, max_iter);
} }
TEST_F(Mat2RotTest, IdentityFromRandomRotation) { TEST_F(Mat2RotTest, IdentityFromRandomRotation) {
@@ -244,7 +243,7 @@ TEST_F(Mat2RotTest, IdentityFromRandomRotation) {
mju_normalize4(quat); mju_normalize4(quat);
EXPECT_LE(mju_mat2Rot(quat, mat), 40); EXPECT_LE(mju_mat2Rot(quat, mat), 40);
mju_quat2Mat(res, quat); mju_quat2Mat(res, quat);
EXPECT_THAT(res, Pointwise(MjNear(1e-6, 1e-6), mat)); EXPECT_THAT(res, Pointwise(MjNear(1e-6, 1e-5), mat));
} }
} }