9
3

quaternion.inl 4.1 KB

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