Merge pull request #1064 from SergeyKrivohatskiy/fix-axisAngle-for-small-angles

Fix axis angle implementation for near zero angles and near PI angles #1064

glm/gtx/matrix_interpolation.inl

@@ -1,26 +1,37 @@
 /// @ref gtx_matrix_interpolation
 
-#include "../gtc/constants.hpp"
+#include "../ext/scalar_constants.hpp"
+
+#include <limits>
 
 namespace glm
 {
 	template<typename T, qualifier Q>
-	GLM_FUNC_QUALIFIER void axisAngle(mat<4, 4, T, Q> const& m, vec<3, T, Q> & axis, T& angle)
+	GLM_FUNC_QUALIFIER void axisAngle(mat<4, 4, T, Q> const& m, vec<3, T, Q>& axis, T& angle)
 	{
-		T epsilon = static_cast<T>(0.01);
-		T epsilon2 = static_cast<T>(0.1);
+		T const epsilon =
+		    std::numeric_limits<T>::epsilon() * static_cast<T>(1e2);
+
+        bool const nearSymmetrical =
+            abs(m[1][0] - m[0][1]) < epsilon &&
+            abs(m[2][0] - m[0][2]) < epsilon &&
+            abs(m[2][1] - m[1][2]) < epsilon;
 
-		if((abs(m[1][0] - m[0][1]) < epsilon) && (abs(m[2][0] - m[0][2]) < epsilon) && (abs(m[2][1] - m[1][2]) < epsilon))
+		if(nearSymmetrical)
 		{
-			if ((abs(m[1][0] + m[0][1]) < epsilon2) && (abs(m[2][0] + m[0][2]) < epsilon2) && (abs(m[2][1] + m[1][2]) < epsilon2) && (abs(m[0][0] + m[1][1] + m[2][2] - static_cast<T>(3.0)) < epsilon2))
+            bool const nearIdentity =
+                abs(m[1][0] + m[0][1]) < epsilon &&
+                abs(m[2][0] + m[0][2]) < epsilon &&
+                abs(m[2][1] + m[1][2]) < epsilon &&
+                abs(m[0][0] + m[1][1] + m[2][2] - T(3.0)) < epsilon;
+			if (nearIdentity)
 			{
 				angle = static_cast<T>(0.0);
-				axis.x = static_cast<T>(1.0);
-				axis.y = static_cast<T>(0.0);
-				axis.z = static_cast<T>(0.0);
+				axis = vec<3, T, Q>(
+				    static_cast<T>(1.0), static_cast<T>(0.0), static_cast<T>(0.0));
 				return;
 			}
-			angle = static_cast<T>(3.1415926535897932384626433832795);
+			angle = pi<T>();
 			T xx = (m[0][0] + static_cast<T>(1.0)) * static_cast<T>(0.5);
 			T yy = (m[1][1] + static_cast<T>(1.0)) * static_cast<T>(0.5);
 			T zz = (m[2][2] + static_cast<T>(1.0)) * static_cast<T>(0.5);
@@ -74,9 +85,7 @@ namespace glm
 			}
 			return;
 		}
-		T s = sqrt((m[2][1] - m[1][2]) * (m[2][1] - m[1][2]) + (m[2][0] - m[0][2]) * (m[2][0] - m[0][2]) + (m[1][0] - m[0][1]) * (m[1][0] - m[0][1]));
-		if (glm::abs(s) < T(0.001))
-			s = static_cast<T>(1);
+
 		T const angleCos = (m[0][0] + m[1][1] + m[2][2] - static_cast<T>(1)) * static_cast<T>(0.5);
 		if(angleCos >= static_cast<T>(1.0))
 		{
@@ -90,9 +99,9 @@ namespace glm
 		{
 			angle = acos(angleCos);
 		}
-		axis.x = (m[1][2] - m[2][1]) / s;
-		axis.y = (m[2][0] - m[0][2]) / s;
-		axis.z = (m[0][1] - m[1][0]) / s;
+
+        axis = glm::normalize(glm::vec<3, T, Q>(
+            m[1][2] - m[2][1], m[2][0] - m[0][2], m[0][1] - m[1][0]));
 	}
 
 	template<typename T, qualifier Q>

test/gtx/gtx_matrix_interpolation.cpp

@@ -1,8 +1,12 @@
 #define GLM_ENABLE_EXPERIMENTAL
 #include <glm/gtc/quaternion.hpp>
+#include <glm/gtx/component_wise.hpp>
 #include <glm/gtx/matrix_interpolation.hpp>
 
 #include <iostream>
+#include <limits>
+#include <math.h>
+
 
 static int test_axisAngle()
 {
@@ -34,6 +38,64 @@ static int test_axisAngle()
 	return Error;
 }
 
+template <class T>
+int testForAxisAngle(glm::vec<3, T, glm::defaultp> const axisTrue, T const angleTrue)
+{
+    T const eps = std::sqrt(std::numeric_limits<T>::epsilon());
+
+    glm::mat<4, 4, T, glm::defaultp> const matTrue = glm::axisAngleMatrix(axisTrue, angleTrue);
+
+    glm::vec<3, T, glm::defaultp> axis;
+    T angle;
+    glm::axisAngle(matTrue, axis, angle);
+    glm::mat<4, 4, T, glm::defaultp> const matRebuilt = glm::axisAngleMatrix(axis, angle);
+
+    glm::mat<4, 4, T, glm::defaultp> const errMat = matTrue - matRebuilt;
+    T const maxErr = glm::compMax(glm::vec<4, T, glm::defaultp>(
+            glm::compMax(glm::abs(errMat[0])),
+            glm::compMax(glm::abs(errMat[1])),
+            glm::compMax(glm::abs(errMat[2])),
+            glm::compMax(glm::abs(errMat[3]))
+        ));
+    
+    return maxErr < eps ? 0 : 1;
+}
+
+static int test_axisAngle2()
+{
+	int Error = 0;
+    
+    Error += testForAxisAngle(glm::vec3(0.0f, 1.0f, 0.0f), 0.0f);
+    Error += testForAxisAngle(glm::vec3(0.358f, 0.0716f, 0.9309f), 0.00001f);
+    Error += testForAxisAngle(glm::vec3(1.0f, 0.0f, 0.0f), 0.0001f);
+    Error += testForAxisAngle(glm::vec3(0.0f, 0.0f, 1.0f), 0.001f);
+    Error += testForAxisAngle(glm::vec3(0.0f, 0.0f, 1.0f), 0.001f);
+    Error += testForAxisAngle(glm::vec3(0.0f, 1.0f, 0.0f), 0.005f);
+    Error += testForAxisAngle(glm::vec3(0.0f, 0.0f, 1.0f), 0.005f);
+    Error += testForAxisAngle(glm::vec3(0.358f, 0.0716f, 0.9309f), 0.03f);
+    Error += testForAxisAngle(glm::vec3(0.358f, 0.0716f, 0.9309f), 0.0003f);
+    Error += testForAxisAngle(glm::vec3(0.0f, 0.0f, 1.0f), 0.01f);
+    Error += testForAxisAngle(glm::dvec3(0.0f, 1.0f, 0.0f), 0.00005);
+    Error += testForAxisAngle(glm::dvec3(-1.0f, 0.0f, 0.0f), 0.000001);
+    Error += testForAxisAngle(glm::dvec3(0.7071f, 0.7071f, 0.0f), 0.5);
+    Error += testForAxisAngle(glm::dvec3(0.7071f, 0.0f, 0.7071f), 0.0002);
+    Error += testForAxisAngle(glm::dvec3(0.7071f, 0.0f, 0.7071f), 0.00002);
+    Error += testForAxisAngle(glm::dvec3(0.7071f, 0.0f, 0.7071f), 0.000002);
+    Error += testForAxisAngle(glm::dvec3(0.7071f, 0.0f, 0.7071f), 0.0000002);
+    Error += testForAxisAngle(glm::vec3(0.0f, 0.7071f, 0.7071f), 1.3f);
+    Error += testForAxisAngle(glm::vec3(0.0f, 0.7071f, 0.7071f), 6.3f);
+    Error += testForAxisAngle(glm::vec3(1.0f, 0.0f, 0.0f), -0.23456f);
+    Error += testForAxisAngle(glm::vec3(1.0f, 0.0f, 0.0f), glm::pi<float>());
+    Error += testForAxisAngle(glm::vec3(0.0f, 1.0f, 0.0f), -glm::pi<float>());
+    Error += testForAxisAngle(glm::vec3(0.358f, 0.0716f, 0.9309f), -glm::pi<float>());
+    Error += testForAxisAngle(glm::vec3(1.0f, 0.0f, 0.0f), glm::pi<float>() + 2e-6f);
+    Error += testForAxisAngle(glm::vec3(1.0f, 0.0f, 0.0f), glm::pi<float>() + 1e-4f);
+    Error += testForAxisAngle(glm::vec3(0.0f, 1.0f, 0.0f), -glm::pi<float>() + 1e-3f);
+    Error += testForAxisAngle(glm::vec3(0.358f, 0.0716f, 0.9309f), -glm::pi<float>() + 5e-3f);
+
+	return Error;
+}
+
 static int test_rotate()
 {
 	glm::mat4 m2(1.0);
@@ -51,6 +113,7 @@ int main()
 	int Error = 0;
 
 	Error += test_axisAngle();
+	Error += test_axisAngle2();
 	Error += test_rotate();
 
 	return Error;