| 1 | /// @ref gtx_quaternion |
| 2 | |
| 3 | #include <limits> |
| 4 | #include "../gtc/constants.hpp" |
| 5 | |
| 6 | namespace glm |
| 7 | { |
| 8 | template<typename T, qualifier Q> |
| 9 | GLM_FUNC_QUALIFIER qua<T, Q> quat_identity() |
| 10 | { |
| 11 | return qua<T, Q>(static_cast<T>(1), static_cast<T>(0), static_cast<T>(0), static_cast<T>(0)); |
| 12 | } |
| 13 | |
| 14 | template<typename T, qualifier Q> |
| 15 | GLM_FUNC_QUALIFIER vec<3, T, Q> cross(vec<3, T, Q> const& v, qua<T, Q> const& q) |
| 16 | { |
| 17 | return inverse(q) * v; |
| 18 | } |
| 19 | |
| 20 | template<typename T, qualifier Q> |
| 21 | GLM_FUNC_QUALIFIER vec<3, T, Q> cross(qua<T, Q> const& q, vec<3, T, Q> const& v) |
| 22 | { |
| 23 | return q * v; |
| 24 | } |
| 25 | |
| 26 | template<typename T, qualifier Q> |
| 27 | GLM_FUNC_QUALIFIER qua<T, Q> squad |
| 28 | ( |
| 29 | qua<T, Q> const& q1, |
| 30 | qua<T, Q> const& q2, |
| 31 | qua<T, Q> const& s1, |
| 32 | qua<T, Q> const& s2, |
| 33 | T const& h) |
| 34 | { |
| 35 | return mix(mix(q1, q2, h), mix(s1, s2, h), static_cast<T>(2) * (static_cast<T>(1) - h) * h); |
| 36 | } |
| 37 | |
| 38 | template<typename T, qualifier Q> |
| 39 | GLM_FUNC_QUALIFIER qua<T, Q> intermediate |
| 40 | ( |
| 41 | qua<T, Q> const& prev, |
| 42 | qua<T, Q> const& curr, |
| 43 | qua<T, Q> const& next |
| 44 | ) |
| 45 | { |
| 46 | qua<T, Q> invQuat = inverse(curr); |
| 47 | return exp((log(next * invQuat) + log(prev * invQuat)) / static_cast<T>(-4)) * curr; |
| 48 | } |
| 49 | |
| 50 | template<typename T, qualifier Q> |
| 51 | GLM_FUNC_QUALIFIER vec<3, T, Q> rotate(qua<T, Q> const& q, vec<3, T, Q> const& v) |
| 52 | { |
| 53 | return q * v; |
| 54 | } |
| 55 | |
| 56 | template<typename T, qualifier Q> |
| 57 | GLM_FUNC_QUALIFIER vec<4, T, Q> rotate(qua<T, Q> const& q, vec<4, T, Q> const& v) |
| 58 | { |
| 59 | return q * v; |
| 60 | } |
| 61 | |
| 62 | template<typename T, qualifier Q> |
| 63 | GLM_FUNC_QUALIFIER T extractRealComponent(qua<T, Q> const& q) |
| 64 | { |
| 65 | T w = static_cast<T>(1) - q.x * q.x - q.y * q.y - q.z * q.z; |
| 66 | if(w < T(0)) |
| 67 | return T(0); |
| 68 | else |
| 69 | return -sqrt(w); |
| 70 | } |
| 71 | |
| 72 | template<typename T, qualifier Q> |
| 73 | GLM_FUNC_QUALIFIER T length2(qua<T, Q> const& q) |
| 74 | { |
| 75 | return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; |
| 76 | } |
| 77 | |
| 78 | template<typename T, qualifier Q> |
| 79 | GLM_FUNC_QUALIFIER qua<T, Q> shortMix(qua<T, Q> const& x, qua<T, Q> const& y, T const& a) |
| 80 | { |
| 81 | if(a <= static_cast<T>(0)) return x; |
| 82 | if(a >= static_cast<T>(1)) return y; |
| 83 | |
| 84 | T fCos = dot(x, y); |
| 85 | qua<T, Q> y2(y); //BUG!!! qua<T> y2; |
| 86 | if(fCos < static_cast<T>(0)) |
| 87 | { |
| 88 | y2 = -y; |
| 89 | fCos = -fCos; |
| 90 | } |
| 91 | |
| 92 | //if(fCos > 1.0f) // problem |
| 93 | T k0, k1; |
| 94 | if(fCos > (static_cast<T>(1) - epsilon<T>())) |
| 95 | { |
| 96 | k0 = static_cast<T>(1) - a; |
| 97 | k1 = static_cast<T>(0) + a; //BUG!!! 1.0f + a; |
| 98 | } |
| 99 | else |
| 100 | { |
| 101 | T fSin = sqrt(T(1) - fCos * fCos); |
| 102 | T fAngle = atan(fSin, fCos); |
| 103 | T fOneOverSin = static_cast<T>(1) / fSin; |
| 104 | k0 = sin((static_cast<T>(1) - a) * fAngle) * fOneOverSin; |
| 105 | k1 = sin((static_cast<T>(0) + a) * fAngle) * fOneOverSin; |
| 106 | } |
| 107 | |
| 108 | return qua<T, Q>( |
| 109 | k0 * x.w + k1 * y2.w, |
| 110 | k0 * x.x + k1 * y2.x, |
| 111 | k0 * x.y + k1 * y2.y, |
| 112 | k0 * x.z + k1 * y2.z); |
| 113 | } |
| 114 | |
| 115 | template<typename T, qualifier Q> |
| 116 | GLM_FUNC_QUALIFIER qua<T, Q> fastMix(qua<T, Q> const& x, qua<T, Q> const& y, T const& a) |
| 117 | { |
| 118 | return glm::normalize(x * (static_cast<T>(1) - a) + (y * a)); |
| 119 | } |
| 120 | |
| 121 | template<typename T, qualifier Q> |
| 122 | GLM_FUNC_QUALIFIER qua<T, Q> rotation(vec<3, T, Q> const& orig, vec<3, T, Q> const& dest) |
| 123 | { |
| 124 | T cosTheta = dot(orig, dest); |
| 125 | vec<3, T, Q> rotationAxis; |
| 126 | |
| 127 | if(cosTheta >= static_cast<T>(1) - epsilon<T>()) { |
| 128 | // orig and dest point in the same direction |
| 129 | return quat_identity<T,Q>(); |
| 130 | } |
| 131 | |
| 132 | if(cosTheta < static_cast<T>(-1) + epsilon<T>()) |
| 133 | { |
| 134 | // special case when vectors in opposite directions : |
| 135 | // there is no "ideal" rotation axis |
| 136 | // So guess one; any will do as long as it's perpendicular to start |
| 137 | // This implementation favors a rotation around the Up axis (Y), |
| 138 | // since it's often what you want to do. |
| 139 | rotationAxis = cross(vec<3, T, Q>(0, 0, 1), orig); |
| 140 | if(length2(rotationAxis) < epsilon<T>()) // bad luck, they were parallel, try again! |
| 141 | rotationAxis = cross(vec<3, T, Q>(1, 0, 0), orig); |
| 142 | |
| 143 | rotationAxis = normalize(rotationAxis); |
| 144 | return angleAxis(pi<T>(), rotationAxis); |
| 145 | } |
| 146 | |
| 147 | // Implementation from Stan Melax's Game Programming Gems 1 article |
| 148 | rotationAxis = cross(orig, dest); |
| 149 | |
| 150 | T s = sqrt((T(1) + cosTheta) * static_cast<T>(2)); |
| 151 | T invs = static_cast<T>(1) / s; |
| 152 | |
| 153 | return qua<T, Q>( |
| 154 | s * static_cast<T>(0.5f), |
| 155 | rotationAxis.x * invs, |
| 156 | rotationAxis.y * invs, |
| 157 | rotationAxis.z * invs); |
| 158 | } |
| 159 | }//namespace glm |