Home | Namespaces | Hierarchy | Alphabetical List | Class list | Files | Namespace Members | Class members | File members | Tutorials
quaternion.h
Go to the documentation of this file.
1 // Copyright (C) 2002-2010 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 namespace irr
14 {
15 namespace core
16 {
17 
19 
22 {
23  public:
24 
26  quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
27 
29  quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
30 
32  quaternion(f32 x, f32 y, f32 z);
33 
35  quaternion(const vector3df& vec);
36 
38  quaternion(const matrix4& mat);
39 
41  bool operator==(const quaternion& other) const;
42 
44  bool operator!=(const quaternion& other) const;
45 
47  inline quaternion& operator=(const quaternion& other);
48 
50  inline quaternion& operator=(const matrix4& other);
51 
53  quaternion operator+(const quaternion& other) const;
54 
56  quaternion operator*(const quaternion& other) const;
57 
59  quaternion operator*(f32 s) const;
60 
63 
65  vector3df operator*(const vector3df& v) const;
66 
68  quaternion& operator*=(const quaternion& other);
69 
71  inline f32 dotProduct(const quaternion& other) const;
72 
74  inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
75 
77  inline quaternion& set(f32 x, f32 y, f32 z);
78 
80  inline quaternion& set(const core::vector3df& vec);
81 
83  inline quaternion& set(const core::quaternion& quat);
84 
86  inline bool equals(const quaternion& other,
87  const f32 tolerance = ROUNDING_ERROR_f32 ) const;
88 
90  inline quaternion& normalize();
91 
93  matrix4 getMatrix() const;
94 
96  void getMatrix( matrix4 &dest, const core::vector3df &translation ) const;
97 
115  void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;
116 
118  inline void getMatrix_transposed( matrix4 &dest ) const;
119 
122 
124  quaternion& slerp( quaternion q1, quaternion q2, f32 interpolate );
125 
127 
132  quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
133 
135  void toAngleAxis (f32 &angle, core::vector3df& axis) const;
136 
138  void toEuler(vector3df& euler) const;
139 
142 
144  quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
145 
147  f32 X; // vectorial (imaginary) part
150  f32 W; // real part
151 };
152 
153 
154 // Constructor which converts euler angles to a quaternion
156 {
157  set(x,y,z);
158 }
159 
160 
161 // Constructor which converts euler angles to a quaternion
163 {
164  set(vec.X,vec.Y,vec.Z);
165 }
166 
167 
168 // Constructor which converts a matrix to a quaternion
169 inline quaternion::quaternion(const matrix4& mat)
170 {
171  (*this) = mat;
172 }
173 
174 
175 // equal operator
176 inline bool quaternion::operator==(const quaternion& other) const
177 {
178  return ((X == other.X) &&
179  (Y == other.Y) &&
180  (Z == other.Z) &&
181  (W == other.W));
182 }
183 
184 // inequality operator
185 inline bool quaternion::operator!=(const quaternion& other) const
186 {
187  return !(*this == other);
188 }
189 
190 // assignment operator
192 {
193  X = other.X;
194  Y = other.Y;
195  Z = other.Z;
196  W = other.W;
197  return *this;
198 }
199 
200 
201 // matrix assignment operator
203 {
204  const f32 diag = m(0,0) + m(1,1) + m(2,2) + 1;
205 
206  if( diag > 0.0f )
207  {
208  const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
209 
210  // TODO: speed this up
211  X = ( m(2,1) - m(1,2)) / scale;
212  Y = ( m(0,2) - m(2,0)) / scale;
213  Z = ( m(1,0) - m(0,1)) / scale;
214  W = 0.25f * scale;
215  }
216  else
217  {
218  if ( m(0,0) > m(1,1) && m(0,0) > m(2,2))
219  {
220  // 1st element of diag is greatest value
221  // find scale according to 1st element, and double it
222  const f32 scale = sqrtf( 1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;
223 
224  // TODO: speed this up
225  X = 0.25f * scale;
226  Y = (m(0,1) + m(1,0)) / scale;
227  Z = (m(2,0) + m(0,2)) / scale;
228  W = (m(2,1) - m(1,2)) / scale;
229  }
230  else if ( m(1,1) > m(2,2))
231  {
232  // 2nd element of diag is greatest value
233  // find scale according to 2nd element, and double it
234  const f32 scale = sqrtf( 1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;
235 
236  // TODO: speed this up
237  X = (m(0,1) + m(1,0) ) / scale;
238  Y = 0.25f * scale;
239  Z = (m(1,2) + m(2,1) ) / scale;
240  W = (m(0,2) - m(2,0) ) / scale;
241  }
242  else
243  {
244  // 3rd element of diag is greatest value
245  // find scale according to 3rd element, and double it
246  const f32 scale = sqrtf( 1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;
247 
248  // TODO: speed this up
249  X = (m(0,2) + m(2,0)) / scale;
250  Y = (m(1,2) + m(2,1)) / scale;
251  Z = 0.25f * scale;
252  W = (m(1,0) - m(0,1)) / scale;
253  }
254  }
255 
256  return normalize();
257 }
258 
259 
260 // multiplication operator
261 inline quaternion quaternion::operator*(const quaternion& other) const
262 {
263  quaternion tmp;
264 
265  tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
266  tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
267  tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
268  tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
269 
270  return tmp;
271 }
272 
273 
274 // multiplication operator
276 {
277  return quaternion(s*X, s*Y, s*Z, s*W);
278 }
279 
280 // multiplication operator
282 {
283  X*=s;
284  Y*=s;
285  Z*=s;
286  W*=s;
287  return *this;
288 }
289 
290 // multiplication operator
292 {
293  return (*this = other * (*this));
294 }
295 
296 // add operator
298 {
299  return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
300 }
301 
302 
303 // Creates a matrix from this quaternion
305 {
306  core::matrix4 m;
308  return m;
309 }
310 
311 
315 inline void quaternion::getMatrix( matrix4 &dest, const core::vector3df &center ) const
316 {
317  f32 * m = dest.pointer();
318 
319  m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
320  m[1] = 2.0f*X*Y + 2.0f*Z*W;
321  m[2] = 2.0f*X*Z - 2.0f*Y*W;
322  m[3] = 0.0f;
323 
324  m[4] = 2.0f*X*Y - 2.0f*Z*W;
325  m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
326  m[6] = 2.0f*Z*Y + 2.0f*X*W;
327  m[7] = 0.0f;
328 
329  m[8] = 2.0f*X*Z + 2.0f*Y*W;
330  m[9] = 2.0f*Z*Y - 2.0f*X*W;
331  m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
332  m[11] = 0.0f;
333 
334  m[12] = center.X;
335  m[13] = center.Y;
336  m[14] = center.Z;
337  m[15] = 1.f;
338 
339  //dest.setDefinitelyIdentityMatrix ( matrix4::BIT_IS_NOT_IDENTITY );
340  dest.setDefinitelyIdentityMatrix ( false );
341 }
342 
343 
344 
358  const core::vector3df &center,
359  const core::vector3df &translation) const
360 {
361  f32 * m = dest.pointer();
362 
363  m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
364  m[1] = 2.0f*X*Y + 2.0f*Z*W;
365  m[2] = 2.0f*X*Z - 2.0f*Y*W;
366  m[3] = 0.0f;
367 
368  m[4] = 2.0f*X*Y - 2.0f*Z*W;
369  m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
370  m[6] = 2.0f*Z*Y + 2.0f*X*W;
371  m[7] = 0.0f;
372 
373  m[8] = 2.0f*X*Z + 2.0f*Y*W;
374  m[9] = 2.0f*Z*Y - 2.0f*X*W;
375  m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
376  m[11] = 0.0f;
377 
378  dest.setRotationCenter ( center, translation );
379 }
380 
381 // Creates a matrix from this quaternion
382 inline void quaternion::getMatrix_transposed( matrix4 &dest ) const
383 {
384  dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
385  dest[4] = 2.0f*X*Y + 2.0f*Z*W;
386  dest[8] = 2.0f*X*Z - 2.0f*Y*W;
387  dest[12] = 0.0f;
388 
389  dest[1] = 2.0f*X*Y - 2.0f*Z*W;
390  dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
391  dest[9] = 2.0f*Z*Y + 2.0f*X*W;
392  dest[13] = 0.0f;
393 
394  dest[2] = 2.0f*X*Z + 2.0f*Y*W;
395  dest[6] = 2.0f*Z*Y - 2.0f*X*W;
396  dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
397  dest[14] = 0.0f;
398 
399  dest[3] = 0.f;
400  dest[7] = 0.f;
401  dest[11] = 0.f;
402  dest[15] = 1.f;
403  //dest.setDefinitelyIdentityMatrix ( matrix4::BIT_IS_NOT_IDENTITY );
404  dest.setDefinitelyIdentityMatrix ( false );
405 }
406 
407 
408 
409 // Inverts this quaternion
411 {
412  X = -X; Y = -Y; Z = -Z;
413  return *this;
414 }
415 
416 // sets new quaternion
418 {
419  X = x;
420  Y = y;
421  Z = z;
422  W = w;
423  return *this;
424 }
425 
426 
427 // sets new quaternion based on euler angles
429 {
430  f64 angle;
431 
432  angle = x * 0.5;
433  const f64 sr = sin(angle);
434  const f64 cr = cos(angle);
435 
436  angle = y * 0.5;
437  const f64 sp = sin(angle);
438  const f64 cp = cos(angle);
439 
440  angle = z * 0.5;
441  const f64 sy = sin(angle);
442  const f64 cy = cos(angle);
443 
444  const f64 cpcy = cp * cy;
445  const f64 spcy = sp * cy;
446  const f64 cpsy = cp * sy;
447  const f64 spsy = sp * sy;
448 
449  X = (f32)(sr * cpcy - cr * spsy);
450  Y = (f32)(cr * spcy + sr * cpsy);
451  Z = (f32)(cr * cpsy - sr * spcy);
452  W = (f32)(cr * cpcy + sr * spsy);
453 
454  return normalize();
455 }
456 
457 // sets new quaternion based on euler angles
459 {
460  return set(vec.X, vec.Y, vec.Z);
461 }
462 
463 // sets new quaternion based on other quaternion
465 {
466  return (*this=quat);
467 }
468 
469 
471 inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const
472 {
473  return core::equals(X, other.X, tolerance) &&
474  core::equals(Y, other.Y, tolerance) &&
475  core::equals(Z, other.Z, tolerance) &&
476  core::equals(W, other.W, tolerance);
477 }
478 
479 
480 // normalizes the quaternion
482 {
483  const f32 n = X*X + Y*Y + Z*Z + W*W;
484 
485  if (n == 1)
486  return *this;
487 
488  //n = 1.0f / sqrtf(n);
489  return (*this *= reciprocal_squareroot ( n ));
490 }
491 
492 
493 // set this quaternion to the result of the interpolation between two quaternions
495 {
496  f32 angle = q1.dotProduct(q2);
497 
498  if (angle < 0.0f)
499  {
500  q1 *= -1.0f;
501  angle *= -1.0f;
502  }
503 
504  f32 scale;
505  f32 invscale;
506 
507  if ((angle + 1.0f) > 0.05f)
508  {
509  if ((1.0f - angle) >= 0.05f) // spherical interpolation
510  {
511  const f32 theta = acosf(angle);
512  const f32 invsintheta = reciprocal(sinf(theta));
513  scale = sinf(theta * (1.0f-time)) * invsintheta;
514  invscale = sinf(theta * time) * invsintheta;
515  }
516  else // linear interploation
517  {
518  scale = 1.0f - time;
519  invscale = time;
520  }
521  }
522  else
523  {
524  q2.set(-q1.Y, q1.X, -q1.W, q1.Z);
525  scale = sinf(PI * (0.5f - time));
526  invscale = sinf(PI * time);
527  }
528 
529  return (*this = (q1*scale) + (q2*invscale));
530 }
531 
532 
533 // calculates the dot product
534 inline f32 quaternion::dotProduct(const quaternion& q2) const
535 {
536  return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
537 }
538 
539 
543 {
544  const f32 fHalfAngle = 0.5f*angle;
545  const f32 fSin = sinf(fHalfAngle);
546  W = cosf(fHalfAngle);
547  X = fSin*axis.X;
548  Y = fSin*axis.Y;
549  Z = fSin*axis.Z;
550  return *this;
551 }
552 
553 
554 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
555 {
556  const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
557 
558  if (core::iszero(scale) || W > 1.0f || W < -1.0f)
559  {
560  angle = 0.0f;
561  axis.X = 0.0f;
562  axis.Y = 1.0f;
563  axis.Z = 0.0f;
564  }
565  else
566  {
567  const f32 invscale = reciprocal(scale);
568  angle = 2.0f * acosf(W);
569  axis.X = X * invscale;
570  axis.Y = Y * invscale;
571  axis.Z = Z * invscale;
572  }
573 }
574 
575 inline void quaternion::toEuler(vector3df& euler) const
576 {
577  const f64 sqw = W*W;
578  const f64 sqx = X*X;
579  const f64 sqy = Y*Y;
580  const f64 sqz = Z*Z;
581 
582  // heading = rotation about z-axis
583  euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
584 
585  // bank = rotation about x-axis
586  euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
587 
588  // attitude = rotation about y-axis
589  euler.Y = asinf( clamp(-2.0f * (X*Z - Y*W), -1.0f, 1.0f) );
590 }
591 
592 
594 {
595  // nVidia SDK implementation
596 
597  vector3df uv, uuv;
598  vector3df qvec(X, Y, Z);
599  uv = qvec.crossProduct(v);
600  uuv = qvec.crossProduct(uv);
601  uv *= (2.0f * W);
602  uuv *= 2.0f;
603 
604  return v + uv + uuv;
605 }
606 
607 // set quaternion to identity
609 {
610  W = 1.f;
611  X = 0.f;
612  Y = 0.f;
613  Z = 0.f;
614  return *this;
615 }
616 
618 {
619  // Based on Stan Melax's article in Game Programming Gems
620  // Copy, since cannot modify local
621  vector3df v0 = from;
622  vector3df v1 = to;
623  v0.normalize();
624  v1.normalize();
625 
626  const f32 d = v0.dotProduct(v1);
627  if (d >= 1.0f) // If dot == 1, vectors are the same
628  {
629  return makeIdentity();
630  }
631  else if (d <= -1.0f) // exactly opposite
632  {
633  core::vector3df axis(1.0f, 0.f, 0.f);
634  axis = axis.crossProduct(core::vector3df(X,Y,Z));
635  if (axis.getLength()==0)
636  {
637  axis.set(0.f,1.f,0.f);
639  }
640  return this->fromAngleAxis(core::PI, axis);
641  }
642 
643  const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
644  const f32 invs = 1.f / s;
645  const vector3df c = v0.crossProduct(v1)*invs;
646  X = c.X;
647  Y = c.Y;
648  Z = c.Z;
649  W = s * 0.5f;
650 
651  return *this;
652 }
653 
654 
655 } // end namespace core
656 } // end namespace irr
657 
658 #endif
659 

The Irrlicht Engine
The Irrlicht Engine Documentation © 2003-2010 by Nikolaus Gebhardt. Generated on Tue Jun 5 2012 17:57:14 by Doxygen (1.8.1)