1 /////////////////////////////////////////////////////////////////////////////////////////////////// 2 // OpenGL Mathematics Copyright (c) 2005 - 2014 G-Truc Creation (www.g-truc.net) 3 /////////////////////////////////////////////////////////////////////////////////////////////////// 4 // Created : 2005-12-21 5 // Updated : 2008-11-27 6 // Licence : This source is under MIT License 7 // File : glm/gtx/quaternion.inl 8 /////////////////////////////////////////////////////////////////////////////////////////////////// 9 10 #include <limits> 11 12 namespace glm 13 { 14 template <typename T, precision P> 15 GLM_FUNC_QUALIFIER detail::tvec3<T, P> cross 16 ( 17 detail::tvec3<T, P> const & v, 18 detail::tquat<T, P> const & q 19 ) 20 { 21 return inverse(q) * v; 22 } 23 24 template <typename T, precision P> 25 GLM_FUNC_QUALIFIER detail::tvec3<T, P> cross 26 ( 27 detail::tquat<T, P> const & q, 28 detail::tvec3<T, P> const & v 29 ) 30 { 31 return q * v; 32 } 33 34 template <typename T, precision P> 35 GLM_FUNC_QUALIFIER detail::tquat<T, P> squad 36 ( 37 detail::tquat<T, P> const & q1, 38 detail::tquat<T, P> const & q2, 39 detail::tquat<T, P> const & s1, 40 detail::tquat<T, P> const & s2, 41 T const & h) 42 { 43 return mix(mix(q1, q2, h), mix(s1, s2, h), T(2) * (T(1) - h) * h); 44 } 45 46 template <typename T, precision P> 47 GLM_FUNC_QUALIFIER detail::tquat<T, P> intermediate 48 ( 49 detail::tquat<T, P> const & prev, 50 detail::tquat<T, P> const & curr, 51 detail::tquat<T, P> const & next 52 ) 53 { 54 detail::tquat<T, P> invQuat = inverse(curr); 55 return exp((log(next + invQuat) + log(prev + invQuat)) / T(-4)) * curr; 56 } 57 58 template <typename T, precision P> 59 GLM_FUNC_QUALIFIER detail::tquat<T, P> exp 60 ( 61 detail::tquat<T, P> const & q 62 ) 63 { 64 detail::tvec3<T, P> u(q.x, q.y, q.z); 65 float Angle = glm::length(u); 66 detail::tvec3<T, P> v(u / Angle); 67 return detail::tquat<T, P>(cos(Angle), sin(Angle) * v); 68 } 69 70 template <typename T, precision P> 71 GLM_FUNC_QUALIFIER detail::tquat<T, P> log 72 ( 73 detail::tquat<T, P> const & q 74 ) 75 { 76 if((q.x == static_cast<T>(0)) && (q.y == static_cast<T>(0)) && (q.z == static_cast<T>(0))) 77 { 78 if(q.w > T(0)) 79 return detail::tquat<T, P>(log(q.w), T(0), T(0), T(0)); 80 else if(q.w < T(0)) 81 return detail::tquat<T, P>(log(-q.w), T(3.1415926535897932384626433832795), T(0),T(0)); 82 else 83 return detail::tquat<T, P>(std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity()); 84 } 85 else 86 { 87 T Vec3Len = sqrt(q.x * q.x + q.y * q.y + q.z * q.z); 88 T QuatLen = sqrt(Vec3Len * Vec3Len + q.w * q.w); 89 T t = atan(Vec3Len, T(q.w)) / Vec3Len; 90 return detail::tquat<T, P>(t * q.x, t * q.y, t * q.z, log(QuatLen)); 91 } 92 } 93 94 template <typename T, precision P> 95 GLM_FUNC_QUALIFIER detail::tquat<T, P> pow 96 ( 97 detail::tquat<T, P> const & x, 98 T const & y 99 ) 100 { 101 if(abs(x.w) > T(0.9999)) 102 return x; 103 float Angle = acos(y); 104 float NewAngle = Angle * y; 105 float Div = sin(NewAngle) / sin(Angle); 106 return detail::tquat<T, P>( 107 cos(NewAngle), 108 x.x * Div, 109 x.y * Div, 110 x.z * Div); 111 } 112 113 //template <typename T, precision P> 114 //GLM_FUNC_QUALIFIER detail::tquat<T, P> sqrt 115 //( 116 // detail::tquat<T, P> const & q 117 //) 118 //{ 119 // T q0 = static_cast<T>(1) - dot(q, q); 120 // return T(2) * (T(1) + q0) * q; 121 //} 122 123 template <typename T, precision P> 124 GLM_FUNC_QUALIFIER detail::tvec3<T, P> rotate 125 ( 126 detail::tquat<T, P> const & q, 127 detail::tvec3<T, P> const & v 128 ) 129 { 130 return q * v; 131 } 132 133 template <typename T, precision P> 134 GLM_FUNC_QUALIFIER detail::tvec4<T, P> rotate 135 ( 136 detail::tquat<T, P> const & q, 137 detail::tvec4<T, P> const & v 138 ) 139 { 140 return q * v; 141 } 142 143 template <typename T, precision P> 144 GLM_FUNC_QUALIFIER T extractRealComponent 145 ( 146 detail::tquat<T, P> const & q 147 ) 148 { 149 T w = static_cast<T>(1.0) - q.x * q.x - q.y * q.y - q.z * q.z; 150 if(w < T(0)) 151 return T(0); 152 else 153 return -sqrt(w); 154 } 155 156 template <typename T, precision P> 157 GLM_FUNC_QUALIFIER T length2 158 ( 159 detail::tquat<T, P> const & q 160 ) 161 { 162 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; 163 } 164 165 template <typename T, precision P> 166 GLM_FUNC_QUALIFIER detail::tquat<T, P> shortMix 167 ( 168 detail::tquat<T, P> const & x, 169 detail::tquat<T, P> const & y, 170 T const & a 171 ) 172 { 173 if(a <= T(0)) return x; 174 if(a >= T(1)) return y; 175 176 T fCos = dot(x, y); 177 detail::tquat<T, P> y2(y); //BUG!!! tquat<T> y2; 178 if(fCos < T(0)) 179 { 180 y2 = -y; 181 fCos = -fCos; 182 } 183 184 //if(fCos > 1.0f) // problem 185 T k0, k1; 186 if(fCos > T(0.9999)) 187 { 188 k0 = static_cast<T>(1) - a; 189 k1 = static_cast<T>(0) + a; //BUG!!! 1.0f + a; 190 } 191 else 192 { 193 T fSin = sqrt(T(1) - fCos * fCos); 194 T fAngle = atan(fSin, fCos); 195 T fOneOverSin = static_cast<T>(1) / fSin; 196 k0 = sin((T(1) - a) * fAngle) * fOneOverSin; 197 k1 = sin((T(0) + a) * fAngle) * fOneOverSin; 198 } 199 200 return detail::tquat<T, P>( 201 k0 * x.w + k1 * y2.w, 202 k0 * x.x + k1 * y2.x, 203 k0 * x.y + k1 * y2.y, 204 k0 * x.z + k1 * y2.z); 205 } 206 207 template <typename T, precision P> 208 GLM_FUNC_QUALIFIER detail::tquat<T, P> fastMix 209 ( 210 detail::tquat<T, P> const & x, 211 detail::tquat<T, P> const & y, 212 T const & a 213 ) 214 { 215 return glm::normalize(x * (T(1) - a) + (y * a)); 216 } 217 218 template <typename T, precision P> 219 GLM_FUNC_QUALIFIER detail::tquat<T, P> rotation 220 ( 221 detail::tvec3<T, P> const & orig, 222 detail::tvec3<T, P> const & dest 223 ) 224 { 225 T cosTheta = dot(orig, dest); 226 detail::tvec3<T, P> rotationAxis; 227 228 if(cosTheta < T(-1) + epsilon<T>()) 229 { 230 // special case when vectors in opposite directions : 231 // there is no "ideal" rotation axis 232 // So guess one; any will do as long as it's perpendicular to start 233 // This implementation favors a rotation around the Up axis (Y), 234 // since it's often what you want to do. 235 rotationAxis = cross(detail::tvec3<T, P>(0, 0, 1), orig); 236 if(length2(rotationAxis) < epsilon<T>()) // bad luck, they were parallel, try again! 237 rotationAxis = cross(detail::tvec3<T, P>(1, 0, 0), orig); 238 239 rotationAxis = normalize(rotationAxis); 240 return angleAxis(pi<T>(), rotationAxis); 241 } 242 243 // Implementation from Stan Melax's Game Programming Gems 1 article 244 rotationAxis = cross(orig, dest); 245 246 T s = sqrt((T(1) + cosTheta) * T(2)); 247 T invs = static_cast<T>(1) / s; 248 249 return detail::tquat<T, P>( 250 s * T(0.5f), 251 rotationAxis.x * invs, 252 rotationAxis.y * invs, 253 rotationAxis.z * invs); 254 } 255 256 }//namespace glm 257
