1/// @ref gtx_quaternion 2/// @file glm/gtx/quaternion.inl 3 4#include <limits> 5#include "../gtc/constants.hpp" 6 7namespace glm 8{ 9 template <typename T, precision P> 10 GLM_FUNC_QUALIFIER tvec3<T, P> cross(tvec3<T, P> const& v, tquat<T, P> const& q) 11 { 12 return inverse(q) * v; 13 } 14 15 template <typename T, precision P> 16 GLM_FUNC_QUALIFIER tvec3<T, P> cross(tquat<T, P> const& q, tvec3<T, P> const& v) 17 { 18 return q * v; 19 } 20 21 template <typename T, precision P> 22 GLM_FUNC_QUALIFIER tquat<T, P> squad 23 ( 24 tquat<T, P> const & q1, 25 tquat<T, P> const & q2, 26 tquat<T, P> const & s1, 27 tquat<T, P> const & s2, 28 T const & h) 29 { 30 return mix(mix(q1, q2, h), mix(s1, s2, h), static_cast<T>(2) * (static_cast<T>(1) - h) * h); 31 } 32 33 template <typename T, precision P> 34 GLM_FUNC_QUALIFIER tquat<T, P> intermediate 35 ( 36 tquat<T, P> const & prev, 37 tquat<T, P> const & curr, 38 tquat<T, P> const & next 39 ) 40 { 41 tquat<T, P> invQuat = inverse(curr); 42 return exp((log(next + invQuat) + log(prev + invQuat)) / static_cast<T>(-4)) * curr; 43 } 44 45 template <typename T, precision P> 46 GLM_FUNC_QUALIFIER tquat<T, P> exp(tquat<T, P> const& q) 47 { 48 tvec3<T, P> u(q.x, q.y, q.z); 49 T const Angle = glm::length(u); 50 if (Angle < epsilon<T>()) 51 return tquat<T, P>(); 52 53 tvec3<T, P> const v(u / Angle); 54 return tquat<T, P>(cos(Angle), sin(Angle) * v); 55 } 56 57 template <typename T, precision P> 58 GLM_FUNC_QUALIFIER tquat<T, P> log(tquat<T, P> const& q) 59 { 60 tvec3<T, P> u(q.x, q.y, q.z); 61 T Vec3Len = length(u); 62 63 if (Vec3Len < epsilon<T>()) 64 { 65 if(q.w > static_cast<T>(0)) 66 return tquat<T, P>(log(q.w), static_cast<T>(0), static_cast<T>(0), static_cast<T>(0)); 67 else if(q.w < static_cast<T>(0)) 68 return tquat<T, P>(log(-q.w), pi<T>(), static_cast<T>(0), static_cast<T>(0)); 69 else 70 return tquat<T, P>(std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity(), std::numeric_limits<T>::infinity()); 71 } 72 else 73 { 74 T t = atan(Vec3Len, T(q.w)) / Vec3Len; 75 T QuatLen2 = Vec3Len * Vec3Len + q.w * q.w; 76 return tquat<T, P>(static_cast<T>(0.5) * log(QuatLen2), t * q.x, t * q.y, t * q.z); 77 } 78 } 79 80 template <typename T, precision P> 81 GLM_FUNC_QUALIFIER tquat<T, P> pow(tquat<T, P> const & x, T const & y) 82 { 83 //Raising to the power of 0 should yield 1 84 //Needed to prevent a division by 0 error later on 85 if(y > -epsilon<T>() && y < epsilon<T>()) 86 return tquat<T, P>(1,0,0,0); 87 88 //To deal with non-unit quaternions 89 T magnitude = sqrt(x.x * x.x + x.y * x.y + x.z * x.z + x.w *x.w); 90 91 //Equivalent to raising a real number to a power 92 //Needed to prevent a division by 0 error later on 93 if(abs(x.w / magnitude) > static_cast<T>(1) - epsilon<T>() && abs(x.w / magnitude) < static_cast<T>(1) + epsilon<T>()) 94 return tquat<T, P>(pow(x.w, y),0,0,0); 95 96 T Angle = acos(x.w / magnitude); 97 T NewAngle = Angle * y; 98 T Div = sin(NewAngle) / sin(Angle); 99 T Mag = pow(magnitude, y - static_cast<T>(1)); 100 101 return tquat<T, P>(cos(NewAngle) * magnitude * Mag, x.x * Div * Mag, x.y * Div * Mag, x.z * Div * Mag); 102 } 103 104 template <typename T, precision P> 105 GLM_FUNC_QUALIFIER tvec3<T, P> rotate(tquat<T, P> const& q, tvec3<T, P> const& v) 106 { 107 return q * v; 108 } 109 110 template <typename T, precision P> 111 GLM_FUNC_QUALIFIER tvec4<T, P> rotate(tquat<T, P> const& q, tvec4<T, P> const& v) 112 { 113 return q * v; 114 } 115 116 template <typename T, precision P> 117 GLM_FUNC_QUALIFIER T extractRealComponent(tquat<T, P> const& q) 118 { 119 T w = static_cast<T>(1) - q.x * q.x - q.y * q.y - q.z * q.z; 120 if(w < T(0)) 121 return T(0); 122 else 123 return -sqrt(w); 124 } 125 126 template <typename T, precision P> 127 GLM_FUNC_QUALIFIER T length2(tquat<T, P> const& q) 128 { 129 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; 130 } 131 132 template <typename T, precision P> 133 GLM_FUNC_QUALIFIER tquat<T, P> shortMix(tquat<T, P> const& x, tquat<T, P> const& y, T const& a) 134 { 135 if(a <= static_cast<T>(0)) return x; 136 if(a >= static_cast<T>(1)) return y; 137 138 T fCos = dot(x, y); 139 tquat<T, P> y2(y); //BUG!!! tquat<T> y2; 140 if(fCos < static_cast<T>(0)) 141 { 142 y2 = -y; 143 fCos = -fCos; 144 } 145 146 //if(fCos > 1.0f) // problem 147 T k0, k1; 148 if(fCos > (static_cast<T>(1) - epsilon<T>())) 149 { 150 k0 = static_cast<T>(1) - a; 151 k1 = static_cast<T>(0) + a; //BUG!!! 1.0f + a; 152 } 153 else 154 { 155 T fSin = sqrt(T(1) - fCos * fCos); 156 T fAngle = atan(fSin, fCos); 157 T fOneOverSin = static_cast<T>(1) / fSin; 158 k0 = sin((static_cast<T>(1) - a) * fAngle) * fOneOverSin; 159 k1 = sin((static_cast<T>(0) + a) * fAngle) * fOneOverSin; 160 } 161 162 return tquat<T, P>( 163 k0 * x.w + k1 * y2.w, 164 k0 * x.x + k1 * y2.x, 165 k0 * x.y + k1 * y2.y, 166 k0 * x.z + k1 * y2.z); 167 } 168 169 template <typename T, precision P> 170 GLM_FUNC_QUALIFIER tquat<T, P> fastMix(tquat<T, P> const& x, tquat<T, P> const& y, T const & a) 171 { 172 return glm::normalize(x * (static_cast<T>(1) - a) + (y * a)); 173 } 174 175 template <typename T, precision P> 176 GLM_FUNC_QUALIFIER tquat<T, P> rotation(tvec3<T, P> const& orig, tvec3<T, P> const& dest) 177 { 178 T cosTheta = dot(orig, dest); 179 tvec3<T, P> rotationAxis; 180 181 if(cosTheta >= static_cast<T>(1) - epsilon<T>()) 182 return quat(); 183 184 if(cosTheta < static_cast<T>(-1) + epsilon<T>()) 185 { 186 // special case when vectors in opposite directions : 187 // there is no "ideal" rotation axis 188 // So guess one; any will do as long as it's perpendicular to start 189 // This implementation favors a rotation around the Up axis (Y), 190 // since it's often what you want to do. 191 rotationAxis = cross(tvec3<T, P>(0, 0, 1), orig); 192 if(length2(rotationAxis) < epsilon<T>()) // bad luck, they were parallel, try again! 193 rotationAxis = cross(tvec3<T, P>(1, 0, 0), orig); 194 195 rotationAxis = normalize(rotationAxis); 196 return angleAxis(pi<T>(), rotationAxis); 197 } 198 199 // Implementation from Stan Melax's Game Programming Gems 1 article 200 rotationAxis = cross(orig, dest); 201 202 T s = sqrt((T(1) + cosTheta) * static_cast<T>(2)); 203 T invs = static_cast<T>(1) / s; 204 205 return tquat<T, P>( 206 s * static_cast<T>(0.5f), 207 rotationAxis.x * invs, 208 rotationAxis.y * invs, 209 rotationAxis.z * invs); 210 } 211 212}//namespace glm 213