9
3

quaternion.inl 5.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200
  1. #include "../trigonometric.hpp"
  2. #include "../geometric.hpp"
  3. #include "../exponential.hpp"
  4. #include "epsilon.hpp"
  5. #include <limits>
  6. namespace glm
  7. {
  8. template<typename T, qualifier Q>
  9. GLM_FUNC_QUALIFIER vec<3, T, Q> eulerAngles(qua<T, Q> const& x)
  10. {
  11. return vec<3, T, Q>(pitch(x), yaw(x), roll(x));
  12. }
  13. template<typename T, qualifier Q>
  14. GLM_FUNC_QUALIFIER T roll(qua<T, Q> const& q)
  15. {
  16. return static_cast<T>(atan(static_cast<T>(2) * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z));
  17. }
  18. template<typename T, qualifier Q>
  19. GLM_FUNC_QUALIFIER T pitch(qua<T, Q> const& q)
  20. {
  21. //return T(atan(T(2) * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z));
  22. T const y = static_cast<T>(2) * (q.y * q.z + q.w * q.x);
  23. T const x = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
  24. if(all(equal(vec<2, T, Q>(x, y), vec<2, T, Q>(0), epsilon<T>()))) //avoid atan2(0,0) - handle singularity - Matiis
  25. return static_cast<T>(static_cast<T>(2) * atan(q.x, q.w));
  26. return static_cast<T>(atan(y, x));
  27. }
  28. template<typename T, qualifier Q>
  29. GLM_FUNC_QUALIFIER T yaw(qua<T, Q> const& q)
  30. {
  31. return asin(clamp(static_cast<T>(-2) * (q.x * q.z - q.w * q.y), static_cast<T>(-1), static_cast<T>(1)));
  32. }
  33. template<typename T, qualifier Q>
  34. GLM_FUNC_QUALIFIER mat<3, 3, T, Q> mat3_cast(qua<T, Q> const& q)
  35. {
  36. mat<3, 3, T, Q> Result(T(1));
  37. T qxx(q.x * q.x);
  38. T qyy(q.y * q.y);
  39. T qzz(q.z * q.z);
  40. T qxz(q.x * q.z);
  41. T qxy(q.x * q.y);
  42. T qyz(q.y * q.z);
  43. T qwx(q.w * q.x);
  44. T qwy(q.w * q.y);
  45. T qwz(q.w * q.z);
  46. Result[0][0] = T(1) - T(2) * (qyy + qzz);
  47. Result[0][1] = T(2) * (qxy + qwz);
  48. Result[0][2] = T(2) * (qxz - qwy);
  49. Result[1][0] = T(2) * (qxy - qwz);
  50. Result[1][1] = T(1) - T(2) * (qxx + qzz);
  51. Result[1][2] = T(2) * (qyz + qwx);
  52. Result[2][0] = T(2) * (qxz + qwy);
  53. Result[2][1] = T(2) * (qyz - qwx);
  54. Result[2][2] = T(1) - T(2) * (qxx + qyy);
  55. return Result;
  56. }
  57. template<typename T, qualifier Q>
  58. GLM_FUNC_QUALIFIER mat<4, 4, T, Q> mat4_cast(qua<T, Q> const& q)
  59. {
  60. return mat<4, 4, T, Q>(mat3_cast(q));
  61. }
  62. template<typename T, qualifier Q>
  63. GLM_FUNC_QUALIFIER qua<T, Q> quat_cast(mat<3, 3, T, Q> const& m)
  64. {
  65. T fourXSquaredMinus1 = m[0][0] - m[1][1] - m[2][2];
  66. T fourYSquaredMinus1 = m[1][1] - m[0][0] - m[2][2];
  67. T fourZSquaredMinus1 = m[2][2] - m[0][0] - m[1][1];
  68. T fourWSquaredMinus1 = m[0][0] + m[1][1] + m[2][2];
  69. int biggestIndex = 0;
  70. T fourBiggestSquaredMinus1 = fourWSquaredMinus1;
  71. if(fourXSquaredMinus1 > fourBiggestSquaredMinus1)
  72. {
  73. fourBiggestSquaredMinus1 = fourXSquaredMinus1;
  74. biggestIndex = 1;
  75. }
  76. if(fourYSquaredMinus1 > fourBiggestSquaredMinus1)
  77. {
  78. fourBiggestSquaredMinus1 = fourYSquaredMinus1;
  79. biggestIndex = 2;
  80. }
  81. if(fourZSquaredMinus1 > fourBiggestSquaredMinus1)
  82. {
  83. fourBiggestSquaredMinus1 = fourZSquaredMinus1;
  84. biggestIndex = 3;
  85. }
  86. T biggestVal = sqrt(fourBiggestSquaredMinus1 + static_cast<T>(1)) * static_cast<T>(0.5);
  87. T mult = static_cast<T>(0.25) / biggestVal;
  88. switch(biggestIndex)
  89. {
  90. case 0:
  91. return qua<T, Q>(biggestVal, (m[1][2] - m[2][1]) * mult, (m[2][0] - m[0][2]) * mult, (m[0][1] - m[1][0]) * mult);
  92. case 1:
  93. return qua<T, Q>((m[1][2] - m[2][1]) * mult, biggestVal, (m[0][1] + m[1][0]) * mult, (m[2][0] + m[0][2]) * mult);
  94. case 2:
  95. return qua<T, Q>((m[2][0] - m[0][2]) * mult, (m[0][1] + m[1][0]) * mult, biggestVal, (m[1][2] + m[2][1]) * mult);
  96. case 3:
  97. return qua<T, Q>((m[0][1] - m[1][0]) * mult, (m[2][0] + m[0][2]) * mult, (m[1][2] + m[2][1]) * mult, biggestVal);
  98. default: // Silence a -Wswitch-default warning in GCC. Should never actually get here. Assert is just for sanity.
  99. assert(false);
  100. return qua<T, Q>(1, 0, 0, 0);
  101. }
  102. }
  103. template<typename T, qualifier Q>
  104. GLM_FUNC_QUALIFIER qua<T, Q> quat_cast(mat<4, 4, T, Q> const& m4)
  105. {
  106. return quat_cast(mat<3, 3, T, Q>(m4));
  107. }
  108. template<typename T, qualifier Q>
  109. GLM_FUNC_QUALIFIER vec<4, bool, Q> lessThan(qua<T, Q> const& x, qua<T, Q> const& y)
  110. {
  111. vec<4, bool, Q> Result;
  112. for(length_t i = 0; i < x.length(); ++i)
  113. Result[i] = x[i] < y[i];
  114. return Result;
  115. }
  116. template<typename T, qualifier Q>
  117. GLM_FUNC_QUALIFIER vec<4, bool, Q> lessThanEqual(qua<T, Q> const& x, qua<T, Q> const& y)
  118. {
  119. vec<4, bool, Q> Result;
  120. for(length_t i = 0; i < x.length(); ++i)
  121. Result[i] = x[i] <= y[i];
  122. return Result;
  123. }
  124. template<typename T, qualifier Q>
  125. GLM_FUNC_QUALIFIER vec<4, bool, Q> greaterThan(qua<T, Q> const& x, qua<T, Q> const& y)
  126. {
  127. vec<4, bool, Q> Result;
  128. for(length_t i = 0; i < x.length(); ++i)
  129. Result[i] = x[i] > y[i];
  130. return Result;
  131. }
  132. template<typename T, qualifier Q>
  133. GLM_FUNC_QUALIFIER vec<4, bool, Q> greaterThanEqual(qua<T, Q> const& x, qua<T, Q> const& y)
  134. {
  135. vec<4, bool, Q> Result;
  136. for(length_t i = 0; i < x.length(); ++i)
  137. Result[i] = x[i] >= y[i];
  138. return Result;
  139. }
  140. template<typename T, qualifier Q>
  141. GLM_FUNC_QUALIFIER qua<T, Q> quatLookAt(vec<3, T, Q> const& direction, vec<3, T, Q> const& up)
  142. {
  143. # if GLM_CONFIG_CLIP_CONTROL & GLM_CLIP_CONTROL_LH_BIT
  144. return quatLookAtLH(direction, up);
  145. # else
  146. return quatLookAtRH(direction, up);
  147. # endif
  148. }
  149. template<typename T, qualifier Q>
  150. GLM_FUNC_QUALIFIER qua<T, Q> quatLookAtRH(vec<3, T, Q> const& direction, vec<3, T, Q> const& up)
  151. {
  152. mat<3, 3, T, Q> Result;
  153. Result[2] = -direction;
  154. Result[0] = normalize(cross(up, Result[2]));
  155. Result[1] = cross(Result[2], Result[0]);
  156. return quat_cast(Result);
  157. }
  158. template<typename T, qualifier Q>
  159. GLM_FUNC_QUALIFIER qua<T, Q> quatLookAtLH(vec<3, T, Q> const& direction, vec<3, T, Q> const& up)
  160. {
  161. mat<3, 3, T, Q> Result;
  162. Result[2] = direction;
  163. Result[0] = normalize(cross(up, Result[2]));
  164. Result[1] = cross(Result[2], Result[0]);
  165. return quat_cast(Result);
  166. }
  167. }//namespace glm
  168. #if GLM_CONFIG_SIMD == GLM_ENABLE
  169. # include "quaternion_simd.inl"
  170. #endif