1 /////////////////////////////////////////////////////////////////////////////////////////////////// 2 // OpenGL Mathematics Copyright (c) 2005 - 2014 G-Truc Creation (www.g-truc.net) 3 /////////////////////////////////////////////////////////////////////////////////////////////////// 4 // Created : 2011-03-05 5 // Updated : 2011-03-05 6 // Licence : This source is under MIT License 7 // File : glm/gtx/matrix_interpolation.inl 8 /////////////////////////////////////////////////////////////////////////////////////////////////// 9 10 namespace glm 11 { 12 template <typename T, precision P> 13 GLM_FUNC_QUALIFIER void axisAngle 14 ( 15 detail::tmat4x4<T, P> const & mat, 16 detail::tvec3<T, P> & axis, 17 T & angle 18 ) 19 { 20 T epsilon = (T)0.01; 21 T epsilon2 = (T)0.1; 22 23 if((abs(mat[1][0] - mat[0][1]) < epsilon) && (abs(mat[2][0] - mat[0][2]) < epsilon) && (abs(mat[2][1] - mat[1][2]) < epsilon)) 24 { 25 if ((abs(mat[1][0] + mat[0][1]) < epsilon2) && (abs(mat[2][0] + mat[0][2]) < epsilon2) && (abs(mat[2][1] + mat[1][2]) < epsilon2) && (abs(mat[0][0] + mat[1][1] + mat[2][2] - (T)3.0) < epsilon2)) 26 { 27 angle = (T)0.0; 28 axis.x = (T)1.0; 29 axis.y = (T)0.0; 30 axis.z = (T)0.0; 31 return; 32 } 33 angle = static_cast<T>(3.1415926535897932384626433832795); 34 T xx = (mat[0][0] + (T)1.0) / (T)2.0; 35 T yy = (mat[1][1] + (T)1.0) / (T)2.0; 36 T zz = (mat[2][2] + (T)1.0) / (T)2.0; 37 T xy = (mat[1][0] + mat[0][1]) / (T)4.0; 38 T xz = (mat[2][0] + mat[0][2]) / (T)4.0; 39 T yz = (mat[2][1] + mat[1][2]) / (T)4.0; 40 if((xx > yy) && (xx > zz)) 41 { 42 if (xx < epsilon) { 43 axis.x = (T)0.0; 44 axis.y = (T)0.7071; 45 axis.z = (T)0.7071; 46 } else { 47 axis.x = sqrt(xx); 48 axis.y = xy / axis.x; 49 axis.z = xz / axis.x; 50 } 51 } 52 else if (yy > zz) 53 { 54 if (yy < epsilon) { 55 axis.x = (T)0.7071; 56 axis.y = (T)0.0; 57 axis.z = (T)0.7071; 58 } else { 59 axis.y = sqrt(yy); 60 axis.x = xy / axis.y; 61 axis.z = yz / axis.y; 62 } 63 } 64 else 65 { 66 if (zz < epsilon) { 67 axis.x = (T)0.7071; 68 axis.y = (T)0.7071; 69 axis.z = (T)0.0; 70 } else { 71 axis.z = sqrt(zz); 72 axis.x = xz / axis.z; 73 axis.y = yz / axis.z; 74 } 75 } 76 return; 77 } 78 T s = sqrt((mat[2][1] - mat[1][2]) * (mat[2][1] - mat[1][2]) + (mat[2][0] - mat[0][2]) * (mat[2][0] - mat[0][2]) + (mat[1][0] - mat[0][1]) * (mat[1][0] - mat[0][1])); 79 if (glm::abs(s) < T(0.001)) 80 s = (T)1.0; 81 angle = acos((mat[0][0] + mat[1][1] + mat[2][2] - (T)1.0) / (T)2.0); 82 axis.x = (mat[1][2] - mat[2][1]) / s; 83 axis.y = (mat[2][0] - mat[0][2]) / s; 84 axis.z = (mat[0][1] - mat[1][0]) / s; 85 } 86 87 template <typename T, precision P> 88 GLM_FUNC_QUALIFIER detail::tmat4x4<T, P> axisAngleMatrix 89 ( 90 detail::tvec3<T, P> const & axis, 91 T const angle 92 ) 93 { 94 T c = cos(angle); 95 T s = sin(angle); 96 T t = static_cast<T>(1) - c; 97 detail::tvec3<T, P> n = normalize(axis); 98 99 return detail::tmat4x4<T, P>( 100 t * n.x * n.x + c, t * n.x * n.y + n.z * s, t * n.x * n.z - n.y * s, T(0), 101 t * n.x * n.y - n.z * s, t * n.y * n.y + c, t * n.y * n.z + n.x * s, T(0), 102 t * n.x * n.z + n.y * s, t * n.y * n.z - n.x * s, t * n.z * n.z + c, T(0), 103 T(0), T(0), T(0), T(1) 104 ); 105 } 106 107 template <typename T, precision P> 108 GLM_FUNC_QUALIFIER detail::tmat4x4<T, P> extractMatrixRotation 109 ( 110 detail::tmat4x4<T, P> const & mat 111 ) 112 { 113 return detail::tmat4x4<T, P>( 114 mat[0][0], mat[0][1], mat[0][2], 0.0, 115 mat[1][0], mat[1][1], mat[1][2], 0.0, 116 mat[2][0], mat[2][1], mat[2][2], 0.0, 117 0.0, 0.0, 0.0, 1.0 118 ); 119 } 120 121 template <typename T, precision P> 122 GLM_FUNC_QUALIFIER detail::tmat4x4<T, P> interpolate 123 ( 124 detail::tmat4x4<T, P> const & m1, 125 detail::tmat4x4<T, P> const & m2, 126 T const delta 127 ) 128 { 129 detail::tmat4x4<T, P> m1rot = extractMatrixRotation(m1); 130 detail::tmat4x4<T, P> dltRotation = m2 * transpose(m1rot); 131 detail::tvec3<T, P> dltAxis; 132 T dltAngle; 133 axisAngle(dltRotation, dltAxis, dltAngle); 134 detail::tmat4x4<T, P> out = axisAngleMatrix(dltAxis, dltAngle * delta) * m1rot; 135 out[3][0] = m1[3][0] + delta * (m2[3][0] - m1[3][0]); 136 out[3][1] = m1[3][1] + delta * (m2[3][1] - m1[3][1]); 137 out[3][2] = m1[3][2] + delta * (m2[3][2] - m1[3][2]); 138 return out; 139 } 140 }//namespace glm 141
