Irrlicht 3D Engine
quaternion.h
Go to the documentation of this file.
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 #ifndef __IRR_QUATERNION_H_INCLUDED__
6 #define __IRR_QUATERNION_H_INCLUDED__
7 
8 #include "irrTypes.h"
9 #include "irrMath.h"
10 #include "matrix4.h"
11 #include "vector3d.h"
12 
13 // Between Irrlicht 1.7 and Irrlicht 1.8 the quaternion-matrix conversions got fixed.
14 // This define disables all involved functions completely to allow finding all places
15 // where the wrong conversions had been in use.
16 #define IRR_TEST_BROKEN_QUATERNION_USE 0
17 
18 namespace irr
19 {
20 namespace core
21 {
22 
24 
27 {
28  public:
29 
31  quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
32 
34  quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
35 
37  quaternion(f32 x, f32 y, f32 z);
38 
40  quaternion(const vector3df& vec);
41 
42 #if !IRR_TEST_BROKEN_QUATERNION_USE
43  quaternion(const matrix4& mat);
45 #endif
46 
48  bool operator==(const quaternion& other) const;
49 
51  bool operator!=(const quaternion& other) const;
52 
54  inline quaternion& operator=(const quaternion& other);
55 
56 #if !IRR_TEST_BROKEN_QUATERNION_USE
57  inline quaternion& operator=(const matrix4& other);
59 #endif
60 
62  quaternion operator+(const quaternion& other) const;
63 
65  quaternion operator*(const quaternion& other) const;
66 
68  quaternion operator*(f32 s) const;
69 
72 
74  vector3df operator*(const vector3df& v) const;
75 
77  quaternion& operator*=(const quaternion& other);
78 
80  inline f32 dotProduct(const quaternion& other) const;
81 
83  inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
84 
86  inline quaternion& set(f32 x, f32 y, f32 z);
87 
89  inline quaternion& set(const core::vector3df& vec);
90 
92  inline quaternion& set(const core::quaternion& quat);
93 
95  inline bool equals(const quaternion& other,
96  const f32 tolerance = ROUNDING_ERROR_f32 ) const;
97 
99  inline quaternion& normalize();
100 
101 #if !IRR_TEST_BROKEN_QUATERNION_USE
102  matrix4 getMatrix() const;
104 #endif
105 
107  void getMatrix( matrix4 &dest, const core::vector3df &translation=core::vector3df() ) const;
108 
126  void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;
127 
129  inline void getMatrix_transposed( matrix4 &dest ) const;
130 
133 
135 
141  quaternion& lerp(quaternion q1, quaternion q2, f32 time);
142 
144 
156  f32 time, f32 threshold=.05f);
157 
159 
164  quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
165 
167  void toAngleAxis (f32 &angle, core::vector3df& axis) const;
168 
170  void toEuler(vector3df& euler) const;
171 
174 
176  quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
177 
179  f32 X; // vectorial (imaginary) part
182  f32 W; // real part
183 };
184 
185 
186 // Constructor which converts euler angles to a quaternion
188 {
189  set(x,y,z);
190 }
191 
192 
193 // Constructor which converts euler angles to a quaternion
195 {
196  set(vec.X,vec.Y,vec.Z);
197 }
198 
199 #if !IRR_TEST_BROKEN_QUATERNION_USE
200 // Constructor which converts a matrix to a quaternion
201 inline quaternion::quaternion(const matrix4& mat)
202 {
203  (*this) = mat;
204 }
205 #endif
206 
207 // equal operator
208 inline bool quaternion::operator==(const quaternion& other) const
209 {
210  return ((X == other.X) &&
211  (Y == other.Y) &&
212  (Z == other.Z) &&
213  (W == other.W));
214 }
215 
216 // inequality operator
217 inline bool quaternion::operator!=(const quaternion& other) const
218 {
219  return !(*this == other);
220 }
221 
222 // assignment operator
224 {
225  X = other.X;
226  Y = other.Y;
227  Z = other.Z;
228  W = other.W;
229  return *this;
230 }
231 
232 #if !IRR_TEST_BROKEN_QUATERNION_USE
233 // matrix assignment operator
235 {
236  const f32 diag = m[0] + m[5] + m[10] + 1;
237 
238  if( diag > 0.0f )
239  {
240  const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
241 
242  // TODO: speed this up
243  X = (m[6] - m[9]) / scale;
244  Y = (m[8] - m[2]) / scale;
245  Z = (m[1] - m[4]) / scale;
246  W = 0.25f * scale;
247  }
248  else
249  {
250  if (m[0]>m[5] && m[0]>m[10])
251  {
252  // 1st element of diag is greatest value
253  // find scale according to 1st element, and double it
254  const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f;
255 
256  // TODO: speed this up
257  X = 0.25f * scale;
258  Y = (m[4] + m[1]) / scale;
259  Z = (m[2] + m[8]) / scale;
260  W = (m[6] - m[9]) / scale;
261  }
262  else if (m[5]>m[10])
263  {
264  // 2nd element of diag is greatest value
265  // find scale according to 2nd element, and double it
266  const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f;
267 
268  // TODO: speed this up
269  X = (m[4] + m[1]) / scale;
270  Y = 0.25f * scale;
271  Z = (m[9] + m[6]) / scale;
272  W = (m[8] - m[2]) / scale;
273  }
274  else
275  {
276  // 3rd element of diag is greatest value
277  // find scale according to 3rd element, and double it
278  const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f;
279 
280  // TODO: speed this up
281  X = (m[8] + m[2]) / scale;
282  Y = (m[9] + m[6]) / scale;
283  Z = 0.25f * scale;
284  W = (m[1] - m[4]) / scale;
285  }
286  }
287 
288  return normalize();
289 }
290 #endif
291 
292 
293 // multiplication operator
294 inline quaternion quaternion::operator*(const quaternion& other) const
295 {
296  quaternion tmp;
297 
298  tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
299  tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
300  tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
301  tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
302 
303  return tmp;
304 }
305 
306 
307 // multiplication operator
309 {
310  return quaternion(s*X, s*Y, s*Z, s*W);
311 }
312 
313 
314 // multiplication operator
316 {
317  X*=s;
318  Y*=s;
319  Z*=s;
320  W*=s;
321  return *this;
322 }
323 
324 // multiplication operator
326 {
327  return (*this = other * (*this));
328 }
329 
330 // add operator
332 {
333  return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
334 }
335 
336 #if !IRR_TEST_BROKEN_QUATERNION_USE
337 // Creates a matrix from this quaternion
339 {
340  core::matrix4 m;
341  getMatrix(m);
342  return m;
343 }
344 #endif
345 
349 inline void quaternion::getMatrix(matrix4 &dest,
350  const core::vector3df &center) const
351 {
352  dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
353  dest[1] = 2.0f*X*Y + 2.0f*Z*W;
354  dest[2] = 2.0f*X*Z - 2.0f*Y*W;
355  dest[3] = 0.0f;
356 
357  dest[4] = 2.0f*X*Y - 2.0f*Z*W;
358  dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
359  dest[6] = 2.0f*Z*Y + 2.0f*X*W;
360  dest[7] = 0.0f;
361 
362  dest[8] = 2.0f*X*Z + 2.0f*Y*W;
363  dest[9] = 2.0f*Z*Y - 2.0f*X*W;
364  dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
365  dest[11] = 0.0f;
366 
367  dest[12] = center.X;
368  dest[13] = center.Y;
369  dest[14] = center.Z;
370  dest[15] = 1.f;
371 
372  dest.setDefinitelyIdentityMatrix ( false );
373 }
374 
375 
389  const core::vector3df &center,
390  const core::vector3df &translation) const
391 {
392  dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
393  dest[1] = 2.0f*X*Y + 2.0f*Z*W;
394  dest[2] = 2.0f*X*Z - 2.0f*Y*W;
395  dest[3] = 0.0f;
396 
397  dest[4] = 2.0f*X*Y - 2.0f*Z*W;
398  dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
399  dest[6] = 2.0f*Z*Y + 2.0f*X*W;
400  dest[7] = 0.0f;
401 
402  dest[8] = 2.0f*X*Z + 2.0f*Y*W;
403  dest[9] = 2.0f*Z*Y - 2.0f*X*W;
404  dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
405  dest[11] = 0.0f;
406 
407  dest.setRotationCenter ( center, translation );
408 }
409 
410 // Creates a matrix from this quaternion
412 {
413  dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
414  dest[4] = 2.0f*X*Y + 2.0f*Z*W;
415  dest[8] = 2.0f*X*Z - 2.0f*Y*W;
416  dest[12] = 0.0f;
417 
418  dest[1] = 2.0f*X*Y - 2.0f*Z*W;
419  dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
420  dest[9] = 2.0f*Z*Y + 2.0f*X*W;
421  dest[13] = 0.0f;
422 
423  dest[2] = 2.0f*X*Z + 2.0f*Y*W;
424  dest[6] = 2.0f*Z*Y - 2.0f*X*W;
425  dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
426  dest[14] = 0.0f;
427 
428  dest[3] = 0.f;
429  dest[7] = 0.f;
430  dest[11] = 0.f;
431  dest[15] = 1.f;
432 
433  dest.setDefinitelyIdentityMatrix(false);
434 }
435 
436 
437 // Inverts this quaternion
439 {
440  X = -X; Y = -Y; Z = -Z;
441  return *this;
442 }
443 
444 
445 // sets new quaternion
447 {
448  X = x;
449  Y = y;
450  Z = z;
451  W = w;
452  return *this;
453 }
454 
455 
456 // sets new quaternion based on euler angles
458 {
459  f64 angle;
460 
461  angle = x * 0.5;
462  const f64 sr = sin(angle);
463  const f64 cr = cos(angle);
464 
465  angle = y * 0.5;
466  const f64 sp = sin(angle);
467  const f64 cp = cos(angle);
468 
469  angle = z * 0.5;
470  const f64 sy = sin(angle);
471  const f64 cy = cos(angle);
472 
473  const f64 cpcy = cp * cy;
474  const f64 spcy = sp * cy;
475  const f64 cpsy = cp * sy;
476  const f64 spsy = sp * sy;
477 
478  X = (f32)(sr * cpcy - cr * spsy);
479  Y = (f32)(cr * spcy + sr * cpsy);
480  Z = (f32)(cr * cpsy - sr * spcy);
481  W = (f32)(cr * cpcy + sr * spsy);
482 
483  return normalize();
484 }
485 
486 // sets new quaternion based on euler angles
488 {
489  return set(vec.X, vec.Y, vec.Z);
490 }
491 
492 // sets new quaternion based on other quaternion
494 {
495  return (*this=quat);
496 }
497 
498 
500 inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const
501 {
502  return core::equals(X, other.X, tolerance) &&
503  core::equals(Y, other.Y, tolerance) &&
504  core::equals(Z, other.Z, tolerance) &&
505  core::equals(W, other.W, tolerance);
506 }
507 
508 
509 // normalizes the quaternion
511 {
512  const f32 n = X*X + Y*Y + Z*Z + W*W;
513 
514  if (n == 1)
515  return *this;
516 
517  //n = 1.0f / sqrtf(n);
518  return (*this *= reciprocal_squareroot ( n ));
519 }
520 
521 
522 // set this quaternion to the result of the linear interpolation between two quaternions
524 {
525  const f32 scale = 1.0f - time;
526  return (*this = (q1*scale) + (q2*time));
527 }
528 
529 
530 // set this quaternion to the result of the interpolation between two quaternions
531 inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time, f32 threshold)
532 {
533  f32 angle = q1.dotProduct(q2);
534 
535  // make sure we use the short rotation
536  if (angle < 0.0f)
537  {
538  q1 *= -1.0f;
539  angle *= -1.0f;
540  }
541 
542  if (angle <= (1-threshold)) // spherical interpolation
543  {
544  const f32 theta = acosf(angle);
545  const f32 invsintheta = reciprocal(sinf(theta));
546  const f32 scale = sinf(theta * (1.0f-time)) * invsintheta;
547  const f32 invscale = sinf(theta * time) * invsintheta;
548  return (*this = (q1*scale) + (q2*invscale));
549  }
550  else // linear interploation
551  return lerp(q1,q2,time);
552 }
553 
554 
555 // calculates the dot product
556 inline f32 quaternion::dotProduct(const quaternion& q2) const
557 {
558  return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
559 }
560 
561 
564 {
565  const f32 fHalfAngle = 0.5f*angle;
566  const f32 fSin = sinf(fHalfAngle);
567  W = cosf(fHalfAngle);
568  X = fSin*axis.X;
569  Y = fSin*axis.Y;
570  Z = fSin*axis.Z;
571  return *this;
572 }
573 
574 
575 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
576 {
577  const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
578 
579  if (core::iszero(scale) || W > 1.0f || W < -1.0f)
580  {
581  angle = 0.0f;
582  axis.X = 0.0f;
583  axis.Y = 1.0f;
584  axis.Z = 0.0f;
585  }
586  else
587  {
588  const f32 invscale = reciprocal(scale);
589  angle = 2.0f * acosf(W);
590  axis.X = X * invscale;
591  axis.Y = Y * invscale;
592  axis.Z = Z * invscale;
593  }
594 }
595 
596 inline void quaternion::toEuler(vector3df& euler) const
597 {
598  const f64 sqw = W*W;
599  const f64 sqx = X*X;
600  const f64 sqy = Y*Y;
601  const f64 sqz = Z*Z;
602  const f64 test = 2.0 * (Y*W - X*Z);
603 
604  if (core::equals(test, 1.0, 0.000001))
605  {
606  // heading = rotation about z-axis
607  euler.Z = (f32) (-2.0*atan2(X, W));
608  // bank = rotation about x-axis
609  euler.X = 0;
610  // attitude = rotation about y-axis
611  euler.Y = (f32) (core::PI64/2.0);
612  }
613  else if (core::equals(test, -1.0, 0.000001))
614  {
615  // heading = rotation about z-axis
616  euler.Z = (f32) (2.0*atan2(X, W));
617  // bank = rotation about x-axis
618  euler.X = 0;
619  // attitude = rotation about y-axis
620  euler.Y = (f32) (core::PI64/-2.0);
621  }
622  else
623  {
624  // heading = rotation about z-axis
625  euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw));
626  // bank = rotation about x-axis
627  euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw));
628  // attitude = rotation about y-axis
629  euler.Y = (f32) asin( clamp(test, -1.0, 1.0) );
630  }
631 }
632 
633 
635 {
636  // nVidia SDK implementation
637 
638  vector3df uv, uuv;
639  vector3df qvec(X, Y, Z);
640  uv = qvec.crossProduct(v);
641  uuv = qvec.crossProduct(uv);
642  uv *= (2.0f * W);
643  uuv *= 2.0f;
644 
645  return v + uv + uuv;
646 }
647 
648 // set quaternion to identity
650 {
651  W = 1.f;
652  X = 0.f;
653  Y = 0.f;
654  Z = 0.f;
655  return *this;
656 }
657 
659 {
660  // Based on Stan Melax's article in Game Programming Gems
661  // Copy, since cannot modify local
662  vector3df v0 = from;
663  vector3df v1 = to;
664  v0.normalize();
665  v1.normalize();
666 
667  const f32 d = v0.dotProduct(v1);
668  if (d >= 1.0f) // If dot == 1, vectors are the same
669  {
670  return makeIdentity();
671  }
672  else if (d <= -1.0f) // exactly opposite
673  {
674  core::vector3df axis(1.0f, 0.f, 0.f);
675  axis = axis.crossProduct(v0);
676  if (axis.getLength()==0)
677  {
678  axis.set(0.f,1.f,0.f);
679  axis = axis.crossProduct(v0);
680  }
681  // same as fromAngleAxis(core::PI, axis).normalize();
682  return set(axis.X, axis.Y, axis.Z, 0).normalize();
683  }
684 
685  const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
686  const f32 invs = 1.f / s;
687  const vector3df c = v0.crossProduct(v1)*invs;
688  return set(c.X, c.Y, c.Z, s * 0.5f).normalize();
689 }
690 
691 
692 } // end namespace core
693 } // end namespace irr
694 
695 #endif
696 
quaternion operator*(const quaternion &other) const
Multiplication operator.
Definition: quaternion.h:294
f32 dotProduct(const quaternion &other) const
Calculates the dot product.
Definition: quaternion.h:556
vector3d< T > crossProduct(const vector3d< T > &p) const
Calculates the cross product with another vector.
Definition: vector3d.h:147
quaternion & makeIdentity()
Set quaternion to identity.
Definition: quaternion.h:649
void toEuler(vector3df &euler) const
Output this quaternion to an euler angle (radians)
Definition: quaternion.h:596
const f64 PI64
Constant for 64bit PI.
Definition: irrMath.h:68
T Y
Y coordinate of the vector.
Definition: vector3d.h:411
bool iszero(const f64 a, const f64 tolerance=ROUNDING_ERROR_f64)
returns if a equals zero, taking rounding errors into account
Definition: irrMath.h:270
void setDefinitelyIdentityMatrix(bool isDefinitelyIdentityMatrix)
Sets if the matrix is definitely identity matrix.
Definition: matrix4.h:2190
float f32
32 bit floating point variable.
Definition: irrTypes.h:104
void toAngleAxis(f32 &angle, core::vector3df &axis) const
Fills an angle (radians) around an axis (unit vector)
Definition: quaternion.h:575
void setRotationCenter(const core::vector3df &center, const core::vector3df &translate)
Builds a combined matrix which translates to a center before rotation and translates from origin afte...
Definition: matrix4.h:2043
T X
X coordinate of the vector.
Definition: vector3d.h:408
quaternion & normalize()
Normalizes the quaternion.
Definition: quaternion.h:510
vector3d< T > & set(const T nx, const T ny, const T nz)
Definition: vector3d.h:113
Everything in the Irrlicht Engine can be found in this namespace.
Definition: aabbox3d.h:12
bool operator==(const quaternion &other) const
Equalilty operator.
Definition: quaternion.h:208
quaternion(f32 x, f32 y, f32 z, f32 w)
Constructor.
Definition: quaternion.h:34
double f64
64 bit floating point variable.
Definition: irrTypes.h:108
bool equals(const quaternion &other, const f32 tolerance=ROUNDING_ERROR_f32) const
returns if this quaternion equals the other one, taking floating point rounding errors into account ...
Definition: quaternion.h:500
const f32 ROUNDING_ERROR_f32
Definition: irrMath.h:49
bool operator!=(const quaternion &other) const
inequality operator
Definition: quaternion.h:217
quaternion operator+(const quaternion &other) const
Add operator.
Definition: quaternion.h:331
REALINLINE f32 reciprocal(const f32 f)
Definition: irrMath.h:535
quaternion & fromAngleAxis(f32 angle, const vector3df &axis)
Create quaternion from rotation angle and rotation axis.
Definition: quaternion.h:563
f32 X
Quaternion elements.
Definition: quaternion.h:179
void getMatrix_transposed(matrix4 &dest) const
Creates a matrix from this quaternion.
Definition: quaternion.h:411
quaternion & slerp(quaternion q1, quaternion q2, f32 time, f32 threshold=.05f)
Set this quaternion to the result of the spherical interpolation between two quaternions.
Definition: quaternion.h:531
quaternion & rotationFromTo(const vector3df &from, const vector3df &to)
Set quaternion to represent a rotation from one vector to another.
Definition: quaternion.h:658
void getMatrixCenter(matrix4 &dest, const core::vector3df &center, const core::vector3df &translation) const
Definition: quaternion.h:388
vector3d< T > & normalize()
Normalizes the vector.
Definition: vector3d.h:168
4x4 matrix. Mostly used as transformation matrix for 3d calculations.
Definition: matrix4.h:45
T dotProduct(const vector3d< T > &other) const
Get the dot product with another vector.
Definition: vector3d.h:125
quaternion & operator=(const quaternion &other)
Assignment operator.
Definition: quaternion.h:223
matrix4 getMatrix() const
Creates a matrix from this quaternion.
Definition: quaternion.h:338
T Z
Z coordinate of the vector.
Definition: vector3d.h:414
Quaternion class for representing rotations.
Definition: quaternion.h:26
T getLength() const
Get length of the vector.
Definition: vector3d.h:117
bool equals(const f64 a, const f64 b, const f64 tolerance=ROUNDING_ERROR_f64)
returns if a equals b, taking possible rounding errors into account
Definition: irrMath.h:185
REALINLINE f64 reciprocal_squareroot(const f64 x)
Definition: irrMath.h:497
quaternion & makeInverse()
Inverts this quaternion.
Definition: quaternion.h:438
quaternion & set(f32 x, f32 y, f32 z, f32 w)
Sets new quaternion.
Definition: quaternion.h:446
quaternion()
Default Constructor.
Definition: quaternion.h:31
const T clamp(const T &value, const T &low, const T &high)
clamps a value between low and high
Definition: irrMath.h:166
quaternion & lerp(quaternion q1, quaternion q2, f32 time)
Set this quaternion to the linear interpolation between two quaternions.
Definition: quaternion.h:523
quaternion & operator*=(f32 s)
Multiplication operator with scalar.
Definition: quaternion.h:315