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