Merge branch 'fix_nan_get_angles' into 'master'

Avoid getting nan in conversion to euler angles (#7772)

Closes #7772

See merge request OpenMW/openmw!4258

apps/components_tests/CMakeLists.txt

@@ -21,11 +21,12 @@ file(GLOB UNITTEST_SRC_FILES
 
     lua/test_ui_content.cpp
 
-    misc/test_stringops.cpp
+    misc/compression.cpp
+    misc/progressreporter.cpp
     misc/test_endianness.cpp
     misc/test_resourcehelpers.cpp
-    misc/progressreporter.cpp
+    misc/test_stringops.cpp
-    misc/compression.cpp
+    misc/testmathutil.cpp
 
     nifloader/testbulletnifloader.cpp
 

apps/components_tests/misc/testmathutil.cpp

@@ -0,0 +1,194 @@
+#include <components/misc/mathutil.hpp>
+
+#include <osg/io_utils>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+
+#include <iomanip>
+#include <limits>
+
+MATCHER_P2(Vec3fEq, other, precision, "")
+{
+    return std::abs(arg.x() - other.x()) < precision && std::abs(arg.y() - other.y()) < precision
+        && std::abs(arg.z() - other.z()) < precision;
+}
+
+namespace testing
+{
+    template <>
+    inline testing::Message& Message::operator<<(const osg::Vec3f& value)
+    {
+        return (*this) << std::setprecision(std::numeric_limits<float>::max_exponent10) << "osg::Vec3f(" << value.x()
+                       << ", " << value.y() << ", " << value.z() << ')';
+    }
+
+    template <>
+    inline testing::Message& Message::operator<<(const osg::Quat& value)
+    {
+        return (*this) << std::setprecision(std::numeric_limits<float>::max_exponent10) << "osg::Quat(" << value.x()
+                       << ", " << value.y() << ", " << value.z() << ", " << value.w() << ')';
+    }
+}
+
+namespace Misc
+{
+    namespace
+    {
+        using namespace testing;
+
+        struct MiscToEulerAnglesXZQuatTest : TestWithParam<std::pair<osg::Quat, osg::Vec3f>>
+        {
+        };
+
+        TEST_P(MiscToEulerAnglesXZQuatTest, shouldReturnValueCloseTo)
+        {
+            const osg::Vec3f result = toEulerAnglesXZ(GetParam().first);
+            EXPECT_THAT(result, Vec3fEq(GetParam().second, std::numeric_limits<float>::epsilon()))
+                << "toEulerAnglesXZ(" << GetParam().first << ") = " << result;
+        }
+
+        const std::pair<osg::Quat, osg::Vec3f> eulerAnglesXZQuat[] = {
+            {
+                osg::Quat(1, 0, 0, 0),
+                osg::Vec3f(0, 0, osg::PI),
+            },
+            {
+                osg::Quat(0, 1, 0, 0),
+                osg::Vec3f(0, 0, 0),
+            },
+            {
+                osg::Quat(0, 0, 1, 0),
+                osg::Vec3f(0, 0, osg::PI),
+            },
+            {
+                osg::Quat(0, 0, 0, 1),
+                osg::Vec3f(0, 0, 0),
+            },
+            {
+                osg::Quat(-0.5, -0.5, -0.5, -0.5),
+                osg::Vec3f(-osg::PI_2f, 0, 0),
+            },
+            {
+                osg::Quat(0.5, -0.5, -0.5, -0.5),
+                osg::Vec3f(0, 0, -osg::PI_2f),
+            },
+            {
+                osg::Quat(0.5, 0.5, -0.5, -0.5),
+                osg::Vec3f(osg::PI_2f, 0, 0),
+            },
+            {
+                osg::Quat(0.5, 0.5, 0.5, -0.5),
+                osg::Vec3f(0, 0, osg::PI_2f),
+            },
+            {
+                osg::Quat(0.5, 0.5, 0.5, 0.5),
+                osg::Vec3f(-osg::PI_2f, 0, 0),
+            },
+            {
+                // normalized osg::Quat(0.1, 0.2, 0.3, 0.4)
+                osg::Quat(0.18257418583505536, 0.36514837167011072, 0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(-0.72972762584686279296875f, 0, -1.10714876651763916015625f),
+            },
+            {
+                osg::Quat(-0.18257418583505536, 0.36514837167011072, 0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(-0.13373161852359771728515625f, 0, -1.2277724742889404296875f),
+            },
+            {
+                osg::Quat(0.18257418583505536, -0.36514837167011072, 0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(0.13373161852359771728515625f, 0, -1.2277724742889404296875f),
+            },
+            {
+                osg::Quat(0.18257418583505536, 0.36514837167011072, -0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(0.13373161852359771728515625f, 0, 1.2277724742889404296875f),
+            },
+            {
+                osg::Quat(0.18257418583505536, 0.36514837167011072, 0.54772255750516607, -0.73029674334022143),
+                osg::Vec3f(-0.13373161852359771728515625, 0, 1.2277724742889404296875f),
+            },
+            {
+                osg::Quat(0.246736, -0.662657, -0.662667, 0.246739),
+                osg::Vec3f(-osg::PI_2f, 0, 2.5199801921844482421875f),
+            },
+        };
+
+        INSTANTIATE_TEST_SUITE_P(FromQuat, MiscToEulerAnglesXZQuatTest, ValuesIn(eulerAnglesXZQuat));
+
+        struct MiscToEulerAnglesZYXQuatTest : TestWithParam<std::pair<osg::Quat, osg::Vec3f>>
+        {
+        };
+
+        TEST_P(MiscToEulerAnglesZYXQuatTest, shouldReturnValueCloseTo)
+        {
+            const osg::Vec3f result = toEulerAnglesZYX(GetParam().first);
+            EXPECT_THAT(result, Vec3fEq(GetParam().second, std::numeric_limits<float>::epsilon()))
+                << "toEulerAnglesZYX(" << GetParam().first << ") = " << result;
+        }
+
+        const std::pair<osg::Quat, osg::Vec3f> eulerAnglesZYXQuat[] = {
+            {
+                osg::Quat(1, 0, 0, 0),
+                osg::Vec3f(osg::PI, 0, 0),
+            },
+            {
+                osg::Quat(0, 1, 0, 0),
+                osg::Vec3f(osg::PI, 0, osg::PI),
+            },
+            {
+                osg::Quat(0, 0, 1, 0),
+                osg::Vec3f(0, 0, osg::PI),
+            },
+            {
+                osg::Quat(0, 0, 0, 1),
+                osg::Vec3f(0, 0, 0),
+            },
+            {
+                osg::Quat(-0.5, -0.5, -0.5, -0.5),
+                osg::Vec3f(0, -osg::PI_2f, -osg::PI_2f),
+            },
+            {
+                osg::Quat(0.5, -0.5, -0.5, -0.5),
+                osg::Vec3f(osg::PI_2f, 0, -osg::PI_2f),
+            },
+            {
+                osg::Quat(0.5, 0.5, -0.5, -0.5),
+                osg::Vec3f(0, osg::PI_2f, -osg::PI_2f),
+            },
+            {
+                osg::Quat(0.5, 0.5, 0.5, -0.5),
+                osg::Vec3f(osg::PI_2f, 0, osg::PI_2f),
+            },
+            {
+                osg::Quat(0.5, 0.5, 0.5, 0.5),
+                osg::Vec3f(0, -osg::PI_2f, -osg::PI_2f),
+            },
+            {
+                // normalized osg::Quat(0.1, 0.2, 0.3, 0.4)
+                osg::Quat(0.18257418583505536, 0.36514837167011072, 0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(0.1973955929279327392578125f, -0.8232119083404541015625f, -1.37340080738067626953125f),
+            },
+            {
+                osg::Quat(-0.18257418583505536, 0.36514837167011072, 0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(0.78539812564849853515625f, -0.339836895465850830078125f, -1.428899288177490234375f),
+            },
+            {
+                osg::Quat(0.18257418583505536, -0.36514837167011072, 0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(-0.78539812564849853515625f, 0.339836895465850830078125f, -1.428899288177490234375f),
+            },
+            {
+                osg::Quat(0.18257418583505536, 0.36514837167011072, -0.54772255750516607, 0.73029674334022143),
+                osg::Vec3f(-0.78539812564849853515625f, -0.339836895465850830078125f, 1.428899288177490234375f),
+            },
+            {
+                osg::Quat(0.18257418583505536, 0.36514837167011072, 0.54772255750516607, -0.73029674334022143),
+                osg::Vec3f(0.78539812564849853515625f, 0.339836895465850830078125f, 1.428899288177490234375f),
+            },
+            {
+                osg::Quat(0.246736, -0.662657, 0.246739, -0.662667),
+                osg::Vec3f(0.06586204469203948974609375f, -osg::PI_2f, 0.64701664447784423828125f),
+            },
+        };
+
+        INSTANTIATE_TEST_SUITE_P(FromQuat, MiscToEulerAnglesZYXQuatTest, ValuesIn(eulerAnglesZYXQuat));
+    }
+}

components/misc/mathutil.hpp

@@ -7,11 +7,11 @@
 #include <osg/Vec2f>
 #include <osg/Vec3f>
 
+#include <cmath>
 #include <type_traits>
 
 namespace Misc
 {
-
     /// Normalizes given angle to the range [-PI, PI]. E.g. PI*3/2 -> -PI/2.
     inline double normalizeAngle(double angle)
     {
@@ -29,15 +29,19 @@ namespace Misc
 
     inline osg::Vec3f toEulerAnglesXZ(osg::Vec3f forward)
     {
-        float x = -asin(forward.z());
+        float x = -std::asin(forward.z());
-        float z = atan2(forward.x(), forward.y());
+        float z = std::atan2(forward.x(), forward.y());
         return osg::Vec3f(x, 0, z);
     }
-    inline osg::Vec3f toEulerAnglesXZ(osg::Quat quat)
+
+    inline osg::Vec3f toEulerAnglesXZ(const osg::Quat& quat)
     {
-        return toEulerAnglesXZ(quat * osg::Vec3f(0, 1, 0));
+        osg::Vec3f forward = quat * osg::Vec3f(0, 1, 0);
+        forward.normalize();
+        return toEulerAnglesXZ(forward);
     }
-    inline osg::Vec3f toEulerAnglesXZ(osg::Matrixf m)
+
+    inline osg::Vec3f toEulerAnglesXZ(const osg::Matrixf& m)
     {
         osg::Vec3f forward(m(1, 0), m(1, 1), m(1, 2));
         forward.normalize();
@@ -46,17 +50,23 @@ namespace Misc
 
     inline osg::Vec3f toEulerAnglesZYX(osg::Vec3f forward, osg::Vec3f up)
     {
-        float y = -asin(up.x());
+        float y = -std::asin(up.x());
-        float x = atan2(up.y(), up.z());
+        float x = std::atan2(up.y(), up.z());
         osg::Vec3f forwardZ = (osg::Quat(x, osg::Vec3f(1, 0, 0)) * osg::Quat(y, osg::Vec3f(0, 1, 0))) * forward;
-        float z = atan2(forwardZ.x(), forwardZ.y());
+        float z = std::atan2(forwardZ.x(), forwardZ.y());
         return osg::Vec3f(x, y, z);
     }
-    inline osg::Vec3f toEulerAnglesZYX(osg::Quat quat)
+
+    inline osg::Vec3f toEulerAnglesZYX(const osg::Quat& quat)
     {
-        return toEulerAnglesZYX(quat * osg::Vec3f(0, 1, 0), quat * osg::Vec3f(0, 0, 1));
+        osg::Vec3f forward = quat * osg::Vec3f(0, 1, 0);
+        forward.normalize();
+        osg::Vec3f up = quat * osg::Vec3f(0, 0, 1);
+        up.normalize();
+        return toEulerAnglesZYX(forward, up);
     }
-    inline osg::Vec3f toEulerAnglesZYX(osg::Matrixf m)
+
+    inline osg::Vec3f toEulerAnglesZYX(const osg::Matrixf& m)
     {
         osg::Vec3f forward(m(1, 0), m(1, 1), m(1, 2));
         osg::Vec3f up(m(2, 0), m(2, 1), m(2, 2));

scripts/data/integration_tests/test_lua_api/player.lua

@@ -97,6 +97,30 @@ testing.registerLocalTest('playerPitchAndYawRotation',
         testing.expectEqualWithDelta(gamma2, math.rad(-16), 0.05, 'Incorrect gamma rotation in ZYX convention')
     end)
 
+testing.registerLocalTest('playerRotation',
+    function()
+        local rotation = math.sqrt(2)
+        local endTime = core.getSimulationTime() + 3
+        while core.getSimulationTime() < endTime do
+            self.controls.jump = false
+            self.controls.run = true
+            self.controls.movement = 0
+            self.controls.sideMovement = 0
+            self.controls.pitchChange = rotation
+            self.controls.yawChange = rotation
+            coroutine.yield()
+
+            local alpha1, gamma1 = self.rotation:getAnglesXZ()
+            testing.expectThat(alpha1, testing.isNotNan(), 'Alpha rotation in XZ convention is nan')
+            testing.expectThat(gamma1, testing.isNotNan(), 'Gamma rotation in XZ convention is nan')
+
+            local alpha2, beta2, gamma2 = self.rotation:getAnglesZYX()
+            testing.expectThat(alpha2, testing.isNotNan(), 'Alpha rotation in ZYX convention is nan')
+            testing.expectThat(beta2, testing.isNotNan(), 'Beta rotation in ZYX convention is nan')
+            testing.expectThat(gamma2, testing.isNotNan(), 'Gamma rotation in ZYX convention is nan')
+        end
+    end)
+
 testing.registerLocalTest('playerForwardRunning',
     function()
         local startPos = self.position

scripts/data/integration_tests/test_lua_api/test.lua

@@ -225,6 +225,10 @@ tests = {
         initPlayer()
         testing.runLocalTest(player, 'playerPitchAndYawRotation')
     end},
+    {'rotating player should not lead to nan rotation', function()
+        initPlayer()
+        testing.runLocalTest(player, 'playerRotation')
+    end},
     {'playerForwardRunning', function()
         initPlayer()
         testing.runLocalTest(player, 'playerForwardRunning')

scripts/data/integration_tests/testing_util/testing_util.lua

@@ -133,6 +133,20 @@ function M.elementsAreArray(expected)
     end
 end
 
+---
+-- Matcher verifying that given number is not a nan.
+-- @function isNotNan
+-- @usage
+-- expectThat(value, isNotNan())
+function M.isNotNan(expected)
+    return function(actual)
+        if actual ~= actual then
+            return 'actual value is nan, expected to be not nan'
+        end
+        return ''
+    end
+end
+
 ---
 -- Verifies that given value matches provided matcher.
 -- @function expectThat