9
3

matrix_interpolation.inl 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129
  1. /// @ref gtx_matrix_interpolation
  2. #include "../gtc/constants.hpp"
  3. namespace glm
  4. {
  5. template<typename T, qualifier Q>
  6. GLM_FUNC_QUALIFIER void axisAngle(mat<4, 4, T, Q> const& m, vec<3, T, Q> & axis, T& angle)
  7. {
  8. T epsilon = static_cast<T>(0.01);
  9. T epsilon2 = static_cast<T>(0.1);
  10. 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))
  11. {
  12. 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))
  13. {
  14. angle = static_cast<T>(0.0);
  15. axis.x = static_cast<T>(1.0);
  16. axis.y = static_cast<T>(0.0);
  17. axis.z = static_cast<T>(0.0);
  18. return;
  19. }
  20. angle = static_cast<T>(3.1415926535897932384626433832795);
  21. T xx = (m[0][0] + static_cast<T>(1.0)) * static_cast<T>(0.5);
  22. T yy = (m[1][1] + static_cast<T>(1.0)) * static_cast<T>(0.5);
  23. T zz = (m[2][2] + static_cast<T>(1.0)) * static_cast<T>(0.5);
  24. T xy = (m[1][0] + m[0][1]) * static_cast<T>(0.25);
  25. T xz = (m[2][0] + m[0][2]) * static_cast<T>(0.25);
  26. T yz = (m[2][1] + m[1][2]) * static_cast<T>(0.25);
  27. if((xx > yy) && (xx > zz))
  28. {
  29. if(xx < epsilon)
  30. {
  31. axis.x = static_cast<T>(0.0);
  32. axis.y = static_cast<T>(0.7071);
  33. axis.z = static_cast<T>(0.7071);
  34. }
  35. else
  36. {
  37. axis.x = sqrt(xx);
  38. axis.y = xy / axis.x;
  39. axis.z = xz / axis.x;
  40. }
  41. }
  42. else if (yy > zz)
  43. {
  44. if(yy < epsilon)
  45. {
  46. axis.x = static_cast<T>(0.7071);
  47. axis.y = static_cast<T>(0.0);
  48. axis.z = static_cast<T>(0.7071);
  49. }
  50. else
  51. {
  52. axis.y = sqrt(yy);
  53. axis.x = xy / axis.y;
  54. axis.z = yz / axis.y;
  55. }
  56. }
  57. else
  58. {
  59. if (zz < epsilon)
  60. {
  61. axis.x = static_cast<T>(0.7071);
  62. axis.y = static_cast<T>(0.7071);
  63. axis.z = static_cast<T>(0.0);
  64. }
  65. else
  66. {
  67. axis.z = sqrt(zz);
  68. axis.x = xz / axis.z;
  69. axis.y = yz / axis.z;
  70. }
  71. }
  72. return;
  73. }
  74. 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]));
  75. if (glm::abs(s) < T(0.001))
  76. s = static_cast<T>(1);
  77. T const angleCos = (m[0][0] + m[1][1] + m[2][2] - static_cast<T>(1)) * static_cast<T>(0.5);
  78. if(angleCos - static_cast<T>(1) < epsilon)
  79. angle = pi<T>() * static_cast<T>(0.25);
  80. else
  81. angle = acos(angleCos);
  82. axis.x = (m[1][2] - m[2][1]) / s;
  83. axis.y = (m[2][0] - m[0][2]) / s;
  84. axis.z = (m[0][1] - m[1][0]) / s;
  85. }
  86. template<typename T, qualifier Q>
  87. GLM_FUNC_QUALIFIER mat<4, 4, T, Q> axisAngleMatrix(vec<3, T, Q> const& axis, T const angle)
  88. {
  89. T c = cos(angle);
  90. T s = sin(angle);
  91. T t = static_cast<T>(1) - c;
  92. vec<3, T, Q> n = normalize(axis);
  93. return mat<4, 4, T, Q>(
  94. t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, static_cast<T>(0.0),
  95. t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, static_cast<T>(0.0),
  96. t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, static_cast<T>(0.0),
  97. static_cast<T>(0.0), static_cast<T>(0.0), static_cast<T>(0.0), static_cast<T>(1.0));
  98. }
  99. template<typename T, qualifier Q>
  100. GLM_FUNC_QUALIFIER mat<4, 4, T, Q> extractMatrixRotation(mat<4, 4, T, Q> const& m)
  101. {
  102. return mat<4, 4, T, Q>(
  103. m[0][0], m[0][1], m[0][2], static_cast<T>(0.0),
  104. m[1][0], m[1][1], m[1][2], static_cast<T>(0.0),
  105. m[2][0], m[2][1], m[2][2], static_cast<T>(0.0),
  106. static_cast<T>(0.0), static_cast<T>(0.0), static_cast<T>(0.0), static_cast<T>(1.0));
  107. }
  108. template<typename T, qualifier Q>
  109. GLM_FUNC_QUALIFIER mat<4, 4, T, Q> interpolate(mat<4, 4, T, Q> const& m1, mat<4, 4, T, Q> const& m2, T const delta)
  110. {
  111. mat<4, 4, T, Q> m1rot = extractMatrixRotation(m1);
  112. mat<4, 4, T, Q> dltRotation = m2 * transpose(m1rot);
  113. vec<3, T, Q> dltAxis;
  114. T dltAngle;
  115. axisAngle(dltRotation, dltAxis, dltAngle);
  116. mat<4, 4, T, Q> out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot;
  117. out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]);
  118. out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]);
  119. out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]);
  120. return out;
  121. }
  122. }//namespace glm