Open Source Tomb Raider Engine
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

Quaternion.cpp 8.1KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300
  1. /*!
  2. * \file src/Quaternion.cpp
  3. * \brief Quaternion
  4. *
  5. * \author Mongoose
  6. */
  7. #include <math.h>
  8. #include "Quaternion.h"
  9. Quaternion::Quaternion() {
  10. mW = 0;
  11. mX = 0;
  12. mY = 0;
  13. mZ = 0;
  14. }
  15. Quaternion::Quaternion(vec_t w, vec_t x, vec_t y, vec_t z) {
  16. mW = w;
  17. mX = x;
  18. mY = y;
  19. mZ = z;
  20. }
  21. Quaternion::Quaternion(vec4_t v) {
  22. mW = v[0];
  23. mX = v[1];
  24. mY = v[2];
  25. mZ = v[3];
  26. }
  27. Quaternion::~Quaternion() {
  28. }
  29. void Quaternion::getMatrix(matrix_t m) {
  30. m[ 0] = 1.0f - 2.0f * (mY*mY + mZ*mZ);
  31. m[ 1] = 2.0f * (mX*mY - mW*mZ);
  32. m[ 2] = 2.0f * (mX*mZ + mW*mY);
  33. m[ 3] = 0.0f;
  34. m[ 4] = 2.0f * (mX*mY + mW*mZ);
  35. m[ 5] = 1.0f - 2.0f * (mX*mX + mZ*mZ);
  36. m[ 6] = 2.0f * (mY*mZ - mW*mX);
  37. m[ 7] = 0.0f;
  38. m[ 8] = 2.0f * (mX*mZ - mW*mY);
  39. m[ 9] = 2.0f * (mY*mZ + mW*mX);
  40. m[10] = 1.0f - 2.0f * (mX*mX + mY*mY);
  41. m[11] = 0.0f;
  42. m[12] = 0.0f;
  43. m[13] = 0.0f;
  44. m[14] = 0.0f;
  45. m[15] = 1.0f;
  46. }
  47. Quaternion &Quaternion::operator =(const Quaternion &q) {
  48. mW = q.mW;
  49. mX = q.mX;
  50. mY = q.mY;
  51. mZ = q.mZ;
  52. return *this;
  53. }
  54. Quaternion Quaternion::operator *(const Quaternion &q) {
  55. return multiply(*this, q);
  56. }
  57. Quaternion Quaternion::operator /(const Quaternion &q) {
  58. return divide(*this, q);
  59. }
  60. Quaternion Quaternion::operator +(const Quaternion &q) {
  61. return add(*this, q);
  62. }
  63. Quaternion Quaternion::operator -(const Quaternion &q) {
  64. return subtract(*this, q);
  65. }
  66. bool Quaternion::operator ==(const Quaternion &q) {
  67. //return (mX == q.mX && mY == q.mY && mZ == q.mZ && mW == q.mW);
  68. return (equalEpsilon(mX, q.mX) && equalEpsilon(mY, q.mY) &&
  69. equalEpsilon(mZ, q.mZ) && equalEpsilon(mW, q.mW));
  70. }
  71. Quaternion Quaternion::conjugate() {
  72. return Quaternion(mW, -mX, -mY, -mZ);
  73. }
  74. Quaternion Quaternion::scale(vec_t s) {
  75. return Quaternion(mW * s, mX * s, mY * s, mZ * s);
  76. }
  77. Quaternion Quaternion::inverse() {
  78. return conjugate().scale(1/magnitude());
  79. }
  80. vec_t Quaternion::dot(Quaternion a, Quaternion b) {
  81. return ((a.mW * b.mW) + (a.mX * b.mX) + (a.mY * b.mY) + (a.mZ * b.mZ));
  82. }
  83. vec_t Quaternion::magnitude() {
  84. return sqrtf(dot(*this, *this));
  85. }
  86. void Quaternion::setIdentity() {
  87. mW = 1.0;
  88. mX = 0.0;
  89. mY = 0.0;
  90. mZ = 0.0;
  91. }
  92. void Quaternion::set(vec_t angle, vec_t x, vec_t y, vec_t z) {
  93. vec_t temp, dist;
  94. // Normalize
  95. temp = x*x + y*y + z*z;
  96. dist = 1.0f / sqrtf(temp);
  97. x *= dist;
  98. y *= dist;
  99. z *= dist;
  100. mX = x;
  101. mY = y;
  102. mZ = z;
  103. mW = cosf(angle / 2.0f);
  104. }
  105. void Quaternion::normalize() {
  106. vec_t dist, square;
  107. square = mX * mX + mY * mY + mZ * mZ + mW * mW;
  108. if (square > 0.0) {
  109. dist = 1.0f / sqrtf(square);
  110. } else {
  111. dist = 1;
  112. }
  113. mX *= dist;
  114. mY *= dist;
  115. mZ *= dist;
  116. mW *= dist;
  117. }
  118. void Quaternion::copy(Quaternion q) {
  119. mW = q.mW;
  120. mX = q.mX;
  121. mY = q.mY;
  122. mZ = q.mZ;
  123. }
  124. Quaternion Quaternion::slerp(Quaternion a, Quaternion b, vec_t time) {
  125. /*******************************************************************
  126. * Spherical Linear Interpolation algorthim
  127. *-----------------------------------------------------------------
  128. *
  129. * Interpolate between A and B rotations ( Find qI )
  130. *
  131. * qI = (((qB . qA)^ -1)^ Time) qA
  132. *
  133. * http://www.magic-software.com/Documentation/quat.pdf
  134. *
  135. * Thanks to digiben for algorithms and basis of the notes in
  136. * this func
  137. *
  138. *******************************************************************/
  139. vec_t result, scaleA, scaleB;
  140. Quaternion i;
  141. // Don't bother if it's the same rotation, it's the same as the result
  142. if (a == b)
  143. return a;
  144. // A . B
  145. result = dot(a, b);
  146. // If the dot product is less than 0, the angle is greater than 90 degrees
  147. if (result < 0.0f) {
  148. // Negate quaternion B and the result of the dot product
  149. b = Quaternion(-b.mW, -b.mX, -b.mY, -b.mZ);
  150. result = -result;
  151. }
  152. // Set the first and second scale for the interpolation
  153. scaleA = 1 - time;
  154. scaleB = time;
  155. // Next, we want to actually calculate the spherical interpolation. Since this
  156. // calculation is quite computationally expensive, we want to only perform it
  157. // if the angle between the 2 quaternions is large enough to warrant it. If the
  158. // angle is fairly small, we can actually just do a simpler linear interpolation
  159. // of the 2 quaternions, and skip all the complex math. We create a "delta" value
  160. // of 0.1 to say that if the cosine of the angle (result of the dot product) between
  161. // the 2 quaternions is smaller than 0.1, then we do NOT want to perform the full on
  162. // interpolation using. This is because you won't really notice the difference.
  163. // Check if the angle between the 2 quaternions was big enough
  164. // to warrant such calculations
  165. if (1 - result > 0.1f) {
  166. // Get the angle between the 2 quaternions, and then
  167. // store the sin() of that angle
  168. vec_t theta = (float)acos(result);
  169. vec_t sinTheta = (float)sin(theta);
  170. // Calculate the scale for qA and qB, according to
  171. // the angle and it's sine value
  172. scaleA = (float)sin((1 - time) * theta) / sinTheta;
  173. scaleB = (float)sin((time * theta)) / sinTheta;
  174. }
  175. // Calculate the x, y, z and w values for the quaternion by using a special
  176. // form of linear interpolation for quaternions.
  177. return (a.scale(scaleA) + b.scale(scaleB));
  178. }
  179. void Quaternion::setByMatrix(matrix_t matrix) {
  180. float diagonal = matrix[0] + matrix[5] + matrix[10] + 1.0f;
  181. float scale = 0.0f;
  182. float w = 0.0f, x = 0.0f, y = 0.0f, z = 0.0f;
  183. if (diagonal > 0.00000001) {
  184. // Calculate the scale of the diagonal
  185. scale = (float)(sqrt(diagonal) * 2);
  186. w = 0.25f * scale;
  187. x = (matrix[9] - matrix[6]) / scale;
  188. y = (matrix[2] - matrix[8]) / scale;
  189. z = (matrix[4] - matrix[1]) / scale;
  190. } else {
  191. // If the first element of the diagonal is the greatest value
  192. if (matrix[0] > matrix[5] && matrix[0] > matrix[10]) {
  193. // Find the scale according to the first element, and double it
  194. scale = (float)sqrt(1.0f + matrix[0] - matrix[5] - matrix[10])*2.0f;
  195. // Calculate the quaternion
  196. w = (matrix[9] - matrix[6]) / scale;
  197. x = 0.25f * scale;
  198. y = (matrix[4] + matrix[1]) / scale;
  199. z = (matrix[2] + matrix[8]) / scale;
  200. } else if (matrix[5] > matrix[10]) {
  201. // The second element of the diagonal is the greatest value
  202. // Find the scale according to the second element, and double it
  203. scale = (float)sqrt(1.0f + matrix[5] - matrix[0] - matrix[10])*2.0f;
  204. // Calculate the quaternion
  205. w = (matrix[2] - matrix[8]) / scale;
  206. x = (matrix[4] + matrix[1]) / scale;
  207. y = 0.25f * scale;
  208. z = (matrix[9] + matrix[6]) / scale;
  209. } else { // The third element of the diagonal is the greatest value
  210. // Find the scale according to the third element, and double it
  211. scale = (float)sqrt(1.0f + matrix[10] - matrix[0] - matrix[5])*2.0f;
  212. // Calculate the quaternion
  213. w = (matrix[4] - matrix[1]) / scale;
  214. x = (matrix[2] + matrix[8]) / scale;
  215. y = (matrix[9] + matrix[6]) / scale;
  216. z = 0.25f * scale;
  217. }
  218. }
  219. mW = w;
  220. mX = x;
  221. mY = y;
  222. mZ = z;
  223. }
  224. Quaternion Quaternion::multiply(Quaternion a, Quaternion b) {
  225. return Quaternion(a.mW * b.mW - a.mX * b.mX - a.mY * b.mY - a.mZ * b.mZ,
  226. a.mW * b.mX + a.mX * b.mW + a.mY * b.mZ - a.mZ * b.mY,
  227. a.mW * b.mY + a.mY * b.mW + a.mZ * b.mX - a.mX * b.mZ,
  228. a.mW * b.mZ + a.mZ * b.mW + a.mX * b.mY - a.mY * b.mX);
  229. }
  230. Quaternion Quaternion::divide(Quaternion a, Quaternion b) {
  231. return (a * (b.inverse()));
  232. }
  233. Quaternion Quaternion::add(Quaternion a, Quaternion b) {
  234. return Quaternion(a.mW + b.mW,
  235. a.mX + b.mX,
  236. a.mY + b.mY,
  237. a.mZ + b.mZ);
  238. }
  239. Quaternion Quaternion::subtract(Quaternion a, Quaternion b) {
  240. return Quaternion(a.mW - b.mW,
  241. a.mX - b.mX,
  242. a.mY - b.mY,
  243. a.mZ - b.mZ);
  244. }