include/quaternion.h

   1 // Copyright (C) 2002-2012 Nikolaus Gebhardt\r
   2 // This file is part of the "Irrlicht Engine".\r
   3 // For conditions of distribution and use, see copyright notice in irrlicht.h\r
   4 \r
   5 #ifndef __IRR_QUATERNION_H_INCLUDED__\r
   6 #define __IRR_QUATERNION_H_INCLUDED__\r
   7 \r
   8 #include "irrTypes.h"\r
   9 #include "irrMath.h"\r
  10 #include "matrix4.h"\r
  11 #include "vector3d.h"\r
  12 \r
  13 // NOTE: You *only* need this when updating an application from Irrlicht before 1.8 to Irrlicht 1.8 or later.\r
  14 // Between Irrlicht 1.7 and Irrlicht 1.8 the quaternion-matrix conversions changed.\r
  15 // Before the fix they had mixed left- and right-handed rotations.\r
  16 // To test if your code was affected by the change enable IRR_TEST_BROKEN_QUATERNION_USE and try to compile your application.\r
  17 // This defines removes those functions so you get compile errors anywhere you use them in your code.\r
  18 // For every line with a compile-errors you have to change the corresponding lines like that:\r
  19 // - When you pass the matrix to the quaternion constructor then replace the matrix by the transposed matrix.\r
  20 // - For uses of getMatrix() you have to use quaternion::getMatrix_transposed instead.\r
  21 // #define IRR_TEST_BROKEN_QUATERNION_USE\r
  22 \r
  23 namespace irr\r
  24 {\r
  25 namespace core\r
  26 {\r
  27 \r
  28 //! Quaternion class for representing rotations.\r
  29 /** It provides cheap combinations and avoids gimbal locks.\r
  30 Also useful for interpolations. */\r
  31 class quaternion\r
  32 {\r
  33         public:\r
  34 \r
  35                 //! Default Constructor\r
  36                 quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}\r
  37 \r
  38                 //! Constructor\r
  39                 quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }\r
  40 \r
  41                 //! Constructor which converts Euler angles (radians) to a quaternion\r
  42                 quaternion(f32 x, f32 y, f32 z);\r
  43 \r
  44                 //! Constructor which converts Euler angles (radians) to a quaternion\r
  45                 quaternion(const vector3df& vec);\r
  46 \r
  47 #ifndef IRR_TEST_BROKEN_QUATERNION_USE\r
  48                 //! Constructor which converts a matrix to a quaternion\r
  49                 quaternion(const matrix4& mat);\r
  50 #endif\r
  51 \r
  52                 //! Equality operator\r
  53                 bool operator==(const quaternion& other) const;\r
  54 \r
  55                 //! inequality operator\r
  56                 bool operator!=(const quaternion& other) const;\r
  57 \r
  58 #ifndef IRR_TEST_BROKEN_QUATERNION_USE\r
  59                 //! Matrix assignment operator\r
  60                 inline quaternion& operator=(const matrix4& other);\r
  61 #endif\r
  62 \r
  63                 //! Add operator\r
  64                 quaternion operator+(const quaternion& other) const;\r
  65 \r
  66                 //! Multiplication operator\r
  67                 //! Be careful, unfortunately the operator order here is opposite of that in CMatrix4::operator*\r
  68                 quaternion operator*(const quaternion& other) const;\r
  69 \r
  70                 //! Multiplication operator with scalar\r
  71                 quaternion operator*(f32 s) const;\r
  72 \r
  73                 //! Multiplication operator with scalar\r
  74                 quaternion& operator*=(f32 s);\r
  75 \r
  76                 //! Multiplication operator\r
  77                 vector3df operator*(const vector3df& v) const;\r
  78 \r
  79                 //! Multiplication operator\r
  80                 quaternion& operator*=(const quaternion& other);\r
  81 \r
  82                 //! Calculates the dot product\r
  83                 inline f32 dotProduct(const quaternion& other) const;\r
  84 \r
  85                 //! Sets new quaternion\r
  86                 inline quaternion& set(f32 x, f32 y, f32 z, f32 w);\r
  87 \r
  88                 //! Sets new quaternion based on Euler angles (radians)\r
  89                 inline quaternion& set(f32 x, f32 y, f32 z);\r
  90 \r
  91                 //! Sets new quaternion based on Euler angles (radians)\r
  92                 inline quaternion& set(const core::vector3df& vec);\r
  93 \r
  94                 //! Sets new quaternion from other quaternion\r
  95                 inline quaternion& set(const core::quaternion& quat);\r
  96 \r
  97                 //! returns if this quaternion equals the other one, taking floating point rounding errors into account\r
  98                 inline bool equals(const quaternion& other,\r
  99                                 const f32 tolerance = ROUNDING_ERROR_f32 ) const;\r
 100 \r
 101                 //! Normalizes the quaternion\r
 102                 inline quaternion& normalize();\r
 103 \r
 104 #ifndef IRR_TEST_BROKEN_QUATERNION_USE\r
 105                 //! Creates a matrix from this quaternion\r
 106                 matrix4 getMatrix() const;\r
 107 #endif\r
 108                 //! Faster method to create a rotation matrix, you should normalize the quaternion before!\r
 109                 void getMatrixFast(matrix4 &dest) const;\r
 110 \r
 111                 //! Creates a matrix from this quaternion\r
 112                 void getMatrix( matrix4 &dest, const core::vector3df &translation=core::vector3df() ) const;\r
 113 \r
 114                 /*!\r
 115                         Creates a matrix from this quaternion\r
 116                         Rotate about a center point\r
 117                         shortcut for\r
 118                         core::quaternion q;\r
 119                         q.rotationFromTo ( vin[i].Normal, forward );\r
 120                         q.getMatrixCenter ( lookat, center, newPos );\r
 121 \r
 122                         core::matrix4 m2;\r
 123                         m2.setInverseTranslation ( center );\r
 124                         lookat *= m2;\r
 125 \r
 126                         core::matrix4 m3;\r
 127                         m2.setTranslation ( newPos );\r
 128                         lookat *= m3;\r
 129 \r
 130                 */\r
 131                 void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;\r
 132 \r
 133                 //! Creates a matrix from this quaternion\r
 134                 inline void getMatrix_transposed( matrix4 &dest ) const;\r
 135 \r
 136                 //! Inverts this quaternion\r
 137                 quaternion& makeInverse();\r
 138 \r
 139                 //! Set this quaternion to the linear interpolation between two quaternions\r
 140                 /** NOTE: lerp result is *not* a normalized quaternion. In most cases\r
 141                 you will want to use lerpN instead as most other quaternion functions expect\r
 142                 to work with a normalized quaternion.\r
 143                 \param q1 First quaternion to be interpolated.\r
 144                 \param q2 Second quaternion to be interpolated.\r
 145                 \param time Progress of interpolation. For time=0 the result is\r
 146                 q1, for time=1 the result is q2. Otherwise interpolation\r
 147                 between q1 and q2. Result is not normalized.\r
 148                 */\r
 149                 quaternion& lerp(quaternion q1, quaternion q2, f32 time);\r
 150 \r
 151                 //! Set this quaternion to the linear interpolation between two quaternions and normalize the result\r
 152                 /**\r
 153                 \param q1 First quaternion to be interpolated.\r
 154                 \param q2 Second quaternion to be interpolated.\r
 155                 \param time Progress of interpolation. For time=0 the result is\r
 156                 q1, for time=1 the result is q2. Otherwise interpolation\r
 157                 between q1 and q2. Result is normalized.\r
 158                 */\r
 159                 quaternion& lerpN(quaternion q1, quaternion q2, f32 time);\r
 160 \r
 161                 //! Set this quaternion to the result of the spherical interpolation between two quaternions\r
 162                 /** \param q1 First quaternion to be interpolated.\r
 163                 \param q2 Second quaternion to be interpolated.\r
 164                 \param time Progress of interpolation. For time=0 the result is\r
 165                 q1, for time=1 the result is q2. Otherwise interpolation\r
 166                 between q1 and q2.\r
 167                 \param threshold To avoid inaccuracies at the end (time=1) the\r
 168                 interpolation switches to linear interpolation at some point.\r
 169                 This value defines how much of the remaining interpolation will\r
 170                 be calculated with lerp. Everything from 1-threshold up will be\r
 171                 linear interpolation.\r
 172                 */\r
 173                 quaternion& slerp(quaternion q1, quaternion q2,\r
 174                                 f32 time, f32 threshold=.05f);\r
 175 \r
 176                 //! Set this quaternion to represent a rotation from angle and axis.\r
 177                 /** Axis must be unit length.\r
 178                 The quaternion representing the rotation is\r
 179                 q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k).\r
 180                 \param angle Rotation Angle in radians.\r
 181                 \param axis Rotation axis. */\r
 182                 quaternion& fromAngleAxis (f32 angle, const vector3df& axis);\r
 183 \r
 184                 //! Fills an angle (radians) around an axis (unit vector)\r
 185                 void toAngleAxis (f32 &angle, core::vector3df& axis) const;\r
 186 \r
 187                 //! Output this quaternion to an Euler angle (radians)\r
 188                 void toEuler(vector3df& euler) const;\r
 189 \r
 190                 //! Set quaternion to identity\r
 191                 quaternion& makeIdentity();\r
 192 \r
 193                 //! Set quaternion to represent a rotation from one vector to another.\r
 194                 quaternion& rotationFromTo(const vector3df& from, const vector3df& to);\r
 195 \r
 196                 //! Quaternion elements.\r
 197                 f32 X; // vectorial (imaginary) part\r
 198                 f32 Y;\r
 199                 f32 Z;\r
 200                 f32 W; // real part\r
 201 };\r
 202 \r
 203 \r
 204 // Constructor which converts Euler angles to a quaternion\r
 205 inline quaternion::quaternion(f32 x, f32 y, f32 z)\r
 206 {\r
 207         set(x,y,z);\r
 208 }\r
 209 \r
 210 \r
 211 // Constructor which converts Euler angles to a quaternion\r
 212 inline quaternion::quaternion(const vector3df& vec)\r
 213 {\r
 214         set(vec.X,vec.Y,vec.Z);\r
 215 }\r
 216 \r
 217 #ifndef IRR_TEST_BROKEN_QUATERNION_USE\r
 218 // Constructor which converts a matrix to a quaternion\r
 219 inline quaternion::quaternion(const matrix4& mat)\r
 220 {\r
 221         (*this) = mat;\r
 222 }\r
 223 #endif\r
 224 \r
 225 // equal operator\r
 226 inline bool quaternion::operator==(const quaternion& other) const\r
 227 {\r
 228         return ((X == other.X) &&\r
 229                 (Y == other.Y) &&\r
 230                 (Z == other.Z) &&\r
 231                 (W == other.W));\r
 232 }\r
 233 \r
 234 // inequality operator\r
 235 inline bool quaternion::operator!=(const quaternion& other) const\r
 236 {\r
 237         return !(*this == other);\r
 238 }\r
 239 \r
 240 #ifndef IRR_TEST_BROKEN_QUATERNION_USE\r
 241 // matrix assignment operator\r
 242 inline quaternion& quaternion::operator=(const matrix4& m)\r
 243 {\r
 244         const f32 diag = m[0] + m[5] + m[10] + 1;\r
 245 \r
 246         if( diag > 0.0f )\r
 247         {\r
 248                 const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal\r
 249 \r
 250                 // TODO: speed this up\r
 251                 X = (m[6] - m[9]) / scale;\r
 252                 Y = (m[8] - m[2]) / scale;\r
 253                 Z = (m[1] - m[4]) / scale;\r
 254                 W = 0.25f * scale;\r
 255         }\r
 256         else\r
 257         {\r
 258                 if (m[0]>m[5] && m[0]>m[10])\r
 259                 {\r
 260                         // 1st element of diag is greatest value\r
 261                         // find scale according to 1st element, and double it\r
 262                         const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f;\r
 263 \r
 264                         // TODO: speed this up\r
 265                         X = 0.25f * scale;\r
 266                         Y = (m[4] + m[1]) / scale;\r
 267                         Z = (m[2] + m[8]) / scale;\r
 268                         W = (m[6] - m[9]) / scale;\r
 269                 }\r
 270                 else if (m[5]>m[10])\r
 271                 {\r
 272                         // 2nd element of diag is greatest value\r
 273                         // find scale according to 2nd element, and double it\r
 274                         const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f;\r
 275 \r
 276                         // TODO: speed this up\r
 277                         X = (m[4] + m[1]) / scale;\r
 278                         Y = 0.25f * scale;\r
 279                         Z = (m[9] + m[6]) / scale;\r
 280                         W = (m[8] - m[2]) / scale;\r
 281                 }\r
 282                 else\r
 283                 {\r
 284                         // 3rd element of diag is greatest value\r
 285                         // find scale according to 3rd element, and double it\r
 286                         const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f;\r
 287 \r
 288                         // TODO: speed this up\r
 289                         X = (m[8] + m[2]) / scale;\r
 290                         Y = (m[9] + m[6]) / scale;\r
 291                         Z = 0.25f * scale;\r
 292                         W = (m[1] - m[4]) / scale;\r
 293                 }\r
 294         }\r
 295 \r
 296         return normalize();\r
 297 }\r
 298 #endif\r
 299 \r
 300 \r
 301 // multiplication operator\r
 302 inline quaternion quaternion::operator*(const quaternion& other) const\r
 303 {\r
 304         quaternion tmp;\r
 305 \r
 306         tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);\r
 307         tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);\r
 308         tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);\r
 309         tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);\r
 310 \r
 311         return tmp;\r
 312 }\r
 313 \r
 314 \r
 315 // multiplication operator\r
 316 inline quaternion quaternion::operator*(f32 s) const\r
 317 {\r
 318         return quaternion(s*X, s*Y, s*Z, s*W);\r
 319 }\r
 320 \r
 321 \r
 322 // multiplication operator\r
 323 inline quaternion& quaternion::operator*=(f32 s)\r
 324 {\r
 325         X*=s;\r
 326         Y*=s;\r
 327         Z*=s;\r
 328         W*=s;\r
 329         return *this;\r
 330 }\r
 331 \r
 332 // multiplication operator\r
 333 inline quaternion& quaternion::operator*=(const quaternion& other)\r
 334 {\r
 335         return (*this = other * (*this));\r
 336 }\r
 337 \r
 338 // add operator\r
 339 inline quaternion quaternion::operator+(const quaternion& b) const\r
 340 {\r
 341         return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);\r
 342 }\r
 343 \r
 344 #ifndef IRR_TEST_BROKEN_QUATERNION_USE\r
 345 // Creates a matrix from this quaternion\r
 346 inline matrix4 quaternion::getMatrix() const\r
 347 {\r
 348         core::matrix4 m;\r
 349         getMatrix(m);\r
 350         return m;\r
 351 }\r
 352 #endif\r
 353 \r
 354 //! Faster method to create a rotation matrix, you should normalize the quaternion before!\r
 355 inline void quaternion::getMatrixFast( matrix4 &dest) const\r
 356 {\r
 357         // TODO:\r
 358         // gpu quaternion skinning => fast Bones transform chain O_O YEAH!\r
 359         // http://www.mrelusive.com/publications/papers/SIMD-From-Quaternion-to-Matrix-and-Back.pdf\r
 360         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;\r
 361         dest[1] = 2.0f*X*Y + 2.0f*Z*W;\r
 362         dest[2] = 2.0f*X*Z - 2.0f*Y*W;\r
 363         dest[3] = 0.0f;\r
 364 \r
 365         dest[4] = 2.0f*X*Y - 2.0f*Z*W;\r
 366         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;\r
 367         dest[6] = 2.0f*Z*Y + 2.0f*X*W;\r
 368         dest[7] = 0.0f;\r
 369 \r
 370         dest[8] = 2.0f*X*Z + 2.0f*Y*W;\r
 371         dest[9] = 2.0f*Z*Y - 2.0f*X*W;\r
 372         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;\r
 373         dest[11] = 0.0f;\r
 374 \r
 375         dest[12] = 0.f;\r
 376         dest[13] = 0.f;\r
 377         dest[14] = 0.f;\r
 378         dest[15] = 1.f;\r
 379 \r
 380         dest.setDefinitelyIdentityMatrix(false);\r
 381 }\r
 382 \r
 383 /*!\r
 384         Creates a matrix from this quaternion\r
 385 */\r
 386 inline void quaternion::getMatrix(matrix4 &dest,\r
 387                 const core::vector3df &center) const\r
 388 {\r
 389         // ok creating a copy may be slower, but at least avoid internal\r
 390         // state chance (also because otherwise we cannot keep this method "const").\r
 391 \r
 392         quaternion q( *this);\r
 393         q.normalize();\r
 394         f32 X = q.X;\r
 395         f32 Y = q.Y;\r
 396         f32 Z = q.Z;\r
 397         f32 W = q.W;\r
 398 \r
 399         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;\r
 400         dest[1] = 2.0f*X*Y + 2.0f*Z*W;\r
 401         dest[2] = 2.0f*X*Z - 2.0f*Y*W;\r
 402         dest[3] = 0.0f;\r
 403 \r
 404         dest[4] = 2.0f*X*Y - 2.0f*Z*W;\r
 405         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;\r
 406         dest[6] = 2.0f*Z*Y + 2.0f*X*W;\r
 407         dest[7] = 0.0f;\r
 408 \r
 409         dest[8] = 2.0f*X*Z + 2.0f*Y*W;\r
 410         dest[9] = 2.0f*Z*Y - 2.0f*X*W;\r
 411         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;\r
 412         dest[11] = 0.0f;\r
 413 \r
 414         dest[12] = center.X;\r
 415         dest[13] = center.Y;\r
 416         dest[14] = center.Z;\r
 417         dest[15] = 1.f;\r
 418 \r
 419         dest.setDefinitelyIdentityMatrix ( false );\r
 420 }\r
 421 \r
 422 \r
 423 /*!\r
 424         Creates a matrix from this quaternion\r
 425         Rotate about a center point\r
 426         shortcut for\r
 427         core::quaternion q;\r
 428         q.rotationFromTo(vin[i].Normal, forward);\r
 429         q.getMatrix(lookat, center);\r
 430 \r
 431         core::matrix4 m2;\r
 432         m2.setInverseTranslation(center);\r
 433         lookat *= m2;\r
 434 */\r
 435 inline void quaternion::getMatrixCenter(matrix4 &dest,\r
 436                                         const core::vector3df &center,\r
 437                                         const core::vector3df &translation) const\r
 438 {\r
 439         quaternion q(*this);\r
 440         q.normalize();\r
 441         f32 X = q.X;\r
 442         f32 Y = q.Y;\r
 443         f32 Z = q.Z;\r
 444         f32 W = q.W;\r
 445 \r
 446         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;\r
 447         dest[1] = 2.0f*X*Y + 2.0f*Z*W;\r
 448         dest[2] = 2.0f*X*Z - 2.0f*Y*W;\r
 449         dest[3] = 0.0f;\r
 450 \r
 451         dest[4] = 2.0f*X*Y - 2.0f*Z*W;\r
 452         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;\r
 453         dest[6] = 2.0f*Z*Y + 2.0f*X*W;\r
 454         dest[7] = 0.0f;\r
 455 \r
 456         dest[8] = 2.0f*X*Z + 2.0f*Y*W;\r
 457         dest[9] = 2.0f*Z*Y - 2.0f*X*W;\r
 458         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;\r
 459         dest[11] = 0.0f;\r
 460 \r
 461         dest.setRotationCenter ( center, translation );\r
 462 }\r
 463 \r
 464 // Creates a matrix from this quaternion\r
 465 inline void quaternion::getMatrix_transposed(matrix4 &dest) const\r
 466 {\r
 467         quaternion q(*this);\r
 468         q.normalize();\r
 469         f32 X = q.X;\r
 470         f32 Y = q.Y;\r
 471         f32 Z = q.Z;\r
 472         f32 W = q.W;\r
 473 \r
 474         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;\r
 475         dest[4] = 2.0f*X*Y + 2.0f*Z*W;\r
 476         dest[8] = 2.0f*X*Z - 2.0f*Y*W;\r
 477         dest[12] = 0.0f;\r
 478 \r
 479         dest[1] = 2.0f*X*Y - 2.0f*Z*W;\r
 480         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;\r
 481         dest[9] = 2.0f*Z*Y + 2.0f*X*W;\r
 482         dest[13] = 0.0f;\r
 483 \r
 484         dest[2] = 2.0f*X*Z + 2.0f*Y*W;\r
 485         dest[6] = 2.0f*Z*Y - 2.0f*X*W;\r
 486         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;\r
 487         dest[14] = 0.0f;\r
 488 \r
 489         dest[3] = 0.f;\r
 490         dest[7] = 0.f;\r
 491         dest[11] = 0.f;\r
 492         dest[15] = 1.f;\r
 493 \r
 494         dest.setDefinitelyIdentityMatrix(false);\r
 495 }\r
 496 \r
 497 \r
 498 // Inverts this quaternion\r
 499 inline quaternion& quaternion::makeInverse()\r
 500 {\r
 501         X = -X; Y = -Y; Z = -Z;\r
 502         return *this;\r
 503 }\r
 504 \r
 505 \r
 506 // sets new quaternion\r
 507 inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)\r
 508 {\r
 509         X = x;\r
 510         Y = y;\r
 511         Z = z;\r
 512         W = w;\r
 513         return *this;\r
 514 }\r
 515 \r
 516 \r
 517 // sets new quaternion based on Euler angles\r
 518 inline quaternion& quaternion::set(f32 x, f32 y, f32 z)\r
 519 {\r
 520         f64 angle;\r
 521 \r
 522         angle = x * 0.5;\r
 523         const f64 sr = sin(angle);\r
 524         const f64 cr = cos(angle);\r
 525 \r
 526         angle = y * 0.5;\r
 527         const f64 sp = sin(angle);\r
 528         const f64 cp = cos(angle);\r
 529 \r
 530         angle = z * 0.5;\r
 531         const f64 sy = sin(angle);\r
 532         const f64 cy = cos(angle);\r
 533 \r
 534         const f64 cpcy = cp * cy;\r
 535         const f64 spcy = sp * cy;\r
 536         const f64 cpsy = cp * sy;\r
 537         const f64 spsy = sp * sy;\r
 538 \r
 539         X = (f32)(sr * cpcy - cr * spsy);\r
 540         Y = (f32)(cr * spcy + sr * cpsy);\r
 541         Z = (f32)(cr * cpsy - sr * spcy);\r
 542         W = (f32)(cr * cpcy + sr * spsy);\r
 543 \r
 544         return normalize();\r
 545 }\r
 546 \r
 547 // sets new quaternion based on Euler angles\r
 548 inline quaternion& quaternion::set(const core::vector3df& vec)\r
 549 {\r
 550         return set( vec.X, vec.Y, vec.Z);\r
 551 }\r
 552 \r
 553 // sets new quaternion based on other quaternion\r
 554 inline quaternion& quaternion::set(const core::quaternion& quat)\r
 555 {\r
 556         return (*this=quat);\r
 557 }\r
 558 \r
 559 \r
 560 //! returns if this quaternion equals the other one, taking floating point rounding errors into account\r
 561 inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const\r
 562 {\r
 563         return core::equals( X, other.X, tolerance) &&\r
 564                 core::equals( Y, other.Y, tolerance) &&\r
 565                 core::equals( Z, other.Z, tolerance) &&\r
 566                 core::equals( W, other.W, tolerance);\r
 567 }\r
 568 \r
 569 \r
 570 // normalizes the quaternion\r
 571 inline quaternion& quaternion::normalize()\r
 572 {\r
 573         // removed conditional branch since it may slow down and anyway the condition was\r
 574         // false even after normalization in some cases.\r
 575         return (*this *= (f32)reciprocal_squareroot ( (f64)(X*X + Y*Y + Z*Z + W*W) ));\r
 576 }\r
 577 \r
 578 // Set this quaternion to the result of the linear interpolation between two quaternions\r
 579 inline quaternion& quaternion::lerp( quaternion q1, quaternion q2, f32 time)\r
 580 {\r
 581         const f32 scale = 1.0f - time;\r
 582         return (*this = (q1*scale) + (q2*time));\r
 583 }\r
 584 \r
 585 // Set this quaternion to the result of the linear interpolation between two quaternions and normalize the result\r
 586 inline quaternion& quaternion::lerpN( quaternion q1, quaternion q2, f32 time)\r
 587 {\r
 588         const f32 scale = 1.0f - time;\r
 589         return (*this = ((q1*scale) + (q2*time)).normalize() );\r
 590 }\r
 591 \r
 592 // set this quaternion to the result of the interpolation between two quaternions\r
 593 inline quaternion& quaternion::slerp( quaternion q1, quaternion q2, f32 time, f32 threshold)\r
 594 {\r
 595         f32 angle = q1.dotProduct(q2);\r
 596 \r
 597         // make sure we use the short rotation\r
 598         if (angle < 0.0f)\r
 599         {\r
 600                 q1 *= -1.0f;\r
 601                 angle *= -1.0f;\r
 602         }\r
 603 \r
 604         if (angle <= (1-threshold)) // spherical interpolation\r
 605         {\r
 606                 const f32 theta = acosf(angle);\r
 607                 const f32 invsintheta = reciprocal(sinf(theta));\r
 608                 const f32 scale = sinf(theta * (1.0f-time)) * invsintheta;\r
 609                 const f32 invscale = sinf(theta * time) * invsintheta;\r
 610                 return (*this = (q1*scale) + (q2*invscale));\r
 611         }\r
 612         else // linear interpolation\r
 613                 return lerpN(q1,q2,time);\r
 614 }\r
 615 \r
 616 \r
 617 // calculates the dot product\r
 618 inline f32 quaternion::dotProduct(const quaternion& q2) const\r
 619 {\r
 620         return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);\r
 621 }\r
 622 \r
 623 \r
 624 //! axis must be unit length, angle in radians\r
 625 inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)\r
 626 {\r
 627         const f32 fHalfAngle = 0.5f*angle;\r
 628         const f32 fSin = sinf(fHalfAngle);\r
 629         W = cosf(fHalfAngle);\r
 630         X = fSin*axis.X;\r
 631         Y = fSin*axis.Y;\r
 632         Z = fSin*axis.Z;\r
 633         return *this;\r
 634 }\r
 635 \r
 636 \r
 637 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const\r
 638 {\r
 639         const f32 scale = sqrtf(X*X + Y*Y + Z*Z);\r
 640 \r
 641         if (core::iszero(scale) || W > 1.0f || W < -1.0f)\r
 642         {\r
 643                 angle = 0.0f;\r
 644                 axis.X = 0.0f;\r
 645                 axis.Y = 1.0f;\r
 646                 axis.Z = 0.0f;\r
 647         }\r
 648         else\r
 649         {\r
 650                 const f32 invscale = reciprocal(scale);\r
 651                 angle = 2.0f * acosf(W);\r
 652                 axis.X = X * invscale;\r
 653                 axis.Y = Y * invscale;\r
 654                 axis.Z = Z * invscale;\r
 655         }\r
 656 }\r
 657 \r
 658 inline void quaternion::toEuler(vector3df& euler) const\r
 659 {\r
 660         const f64 sqw = W*W;\r
 661         const f64 sqx = X*X;\r
 662         const f64 sqy = Y*Y;\r
 663         const f64 sqz = Z*Z;\r
 664         const f64 test = 2.0 * (Y*W - X*Z);\r
 665 \r
 666         if (core::equals(test, 1.0, 0.000001))\r
 667         {\r
 668                 // heading = rotation about z-axis\r
 669                 euler.Z = (f32) (-2.0*atan2(X, W));\r
 670                 // bank = rotation about x-axis\r
 671                 euler.X = 0;\r
 672                 // attitude = rotation about y-axis\r
 673                 euler.Y = (f32) (core::PI64/2.0);\r
 674         }\r
 675         else if (core::equals(test, -1.0, 0.000001))\r
 676         {\r
 677                 // heading = rotation about z-axis\r
 678                 euler.Z = (f32) (2.0*atan2(X, W));\r
 679                 // bank = rotation about x-axis\r
 680                 euler.X = 0;\r
 681                 // attitude = rotation about y-axis\r
 682                 euler.Y = (f32) (core::PI64/-2.0);\r
 683         }\r
 684         else\r
 685         {\r
 686                 // heading = rotation about z-axis\r
 687                 euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw));\r
 688                 // bank = rotation about x-axis\r
 689                 euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw));\r
 690                 // attitude = rotation about y-axis\r
 691                 euler.Y = (f32) asin( clamp(test, -1.0, 1.0) );\r
 692         }\r
 693 }\r
 694 \r
 695 \r
 696 inline vector3df quaternion::operator* (const vector3df& v) const\r
 697 {\r
 698         // nVidia SDK implementation\r
 699 \r
 700         vector3df uv, uuv;\r
 701         const vector3df qvec(X, Y, Z);\r
 702         uv = qvec.crossProduct(v);\r
 703         uuv = qvec.crossProduct(uv);\r
 704         uv *= (2.0f * W);\r
 705         uuv *= 2.0f;\r
 706 \r
 707         return v + uv + uuv;\r
 708 }\r
 709 \r
 710 // set quaternion to identity\r
 711 inline core::quaternion& quaternion::makeIdentity()\r
 712 {\r
 713         W = 1.f;\r
 714         X = 0.f;\r
 715         Y = 0.f;\r
 716         Z = 0.f;\r
 717         return *this;\r
 718 }\r
 719 \r
 720 inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to)\r
 721 {\r
 722         // Based on Stan Melax's article in Game Programming Gems\r
 723         // Copy, since cannot modify local\r
 724         vector3df v0 = from;\r
 725         vector3df v1 = to;\r
 726         v0.normalize();\r
 727         v1.normalize();\r
 728 \r
 729         const f32 d = v0.dotProduct(v1);\r
 730         if (d >= 1.0f) // If dot == 1, vectors are the same\r
 731         {\r
 732                 return makeIdentity();\r
 733         }\r
 734         else if (d <= -1.0f) // exactly opposite\r
 735         {\r
 736                 core::vector3df axis(1.0f, 0.f, 0.f);\r
 737                 axis = axis.crossProduct(v0);\r
 738                 if (axis.getLength()==0)\r
 739                 {\r
 740                         axis.set(0.f,1.f,0.f);\r
 741                         axis = axis.crossProduct(v0);\r
 742                 }\r
 743                 // same as fromAngleAxis(core::PI, axis).normalize();\r
 744                 return set(axis.X, axis.Y, axis.Z, 0).normalize();\r
 745         }\r
 746 \r
 747         const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt\r
 748         const f32 invs = 1.f / s;\r
 749         const vector3df c = v0.crossProduct(v1)*invs;\r
 750         return set(c.X, c.Y, c.Z, s * 0.5f).normalize();\r
 751 }\r
 752 \r
 753 \r
 754 } // end namespace core\r
 755 } // end namespace irr\r
 756 \r
 757 #endif\r
 758 \r