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