From 63c7175ed5f7f2e63ef0267aaeb6358b247ecef5 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 4 Aug 2026 09:37:44 -0700 Subject: [PATCH] Adjust rotEPS for single precision in mju_mat2Rot. 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 --- src/engine/engine_util_spatial.c | 12 ++++++++---- test/engine/engine_util_spatial_test.cc | 7 +++---- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index fb7ec3fb..012cb370 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -278,12 +278,16 @@ void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]) { // extract 3D rotation from an arbitrary 3x3 matrix +#ifdef mjUSESINGLE +static const mjtNum rotEPS = 1e-6f; +#else static const mjtNum rotEPS = 1e-9; +#endif int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]) { - // Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin. "A - // robust method to extract the rotational part of deformations." In - // Proceedings of the 9th International Conference on Motion in Games, pp. - // 55-60. 2016. + // Müller, Matthias, Jan Bender, Nuttapong Chentanez, and Miles Macklin. + // "A robust method to extract the rotational part of deformations." + // In Proceedings of the 9th International Conference on Motion in Games, + // pp. 55-60. 2016. int iter; mjtNum col1_mat[3] = {mat[0], mat[3], mat[6]}; diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 2377e19c..e532adc0 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/test/engine/engine_util_spatial_test.cc @@ -218,9 +218,8 @@ TEST_F(Mat2RotTest, RotationFromArbitraryMatrix) { // calculate rotational part of the matrix mjtNum quat[4] = {1, 0, 0, 0}; int niter = mju_mat2Rot(quat, mat); - EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-6), target)); - int max_iter = static_cast(MjTol(150, 500)); - EXPECT_LE(niter, max_iter); + EXPECT_THAT(quat, Pointwise(MjNear(1e-8, 1e-5), target)); + EXPECT_LE(niter, 150); } TEST_F(Mat2RotTest, IdentityFromRandomRotation) { @@ -244,7 +243,7 @@ TEST_F(Mat2RotTest, IdentityFromRandomRotation) { mju_normalize4(quat); EXPECT_LE(mju_mat2Rot(quat, mat), 40); mju_quat2Mat(res, quat); - EXPECT_THAT(res, Pointwise(MjNear(1e-6, 1e-6), mat)); + EXPECT_THAT(res, Pointwise(MjNear(1e-6, 1e-5), mat)); } }