irr/include/quaternion.h

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