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

Fix axis angle implementation for near zero angles and near PI angles #1064
This commit is contained in:
Christophe 2021-04-05 13:25:56 +02:00 committed by GitHub
commit 820607ad2b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 88 additions and 16 deletions

View File

@ -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)
{
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);
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))
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(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>

View File

@ -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;