Home | Namespaces | Hierarchy | Alphabetical List | Class list | Files | Namespace Members | Class members | File members | Tutorials

# quaternion.h

Go to the documentation of this file.

    00001 // Copyright (C) 2002-2009 Nikolaus Gebhardt
    00002 // This file is part of the "Irrlicht Engine".
    00003 // For conditions of distribution and use, see copyright notice in irrlicht.h
    00004
    00005 #ifndef __IRR_QUATERNION_H_INCLUDED__
    00006 #define __IRR_QUATERNION_H_INCLUDED__
    00007
    00008 #include "irrTypes.h"
    00009 #include "irrMath.h"
    00010 #include "matrix4.h"
    00011 #include "vector3d.h"
    00012
    00013 namespace irr
    00014 {
    00015 namespace core
    00016 {
    00017
    00019
    00021 class quaternion
    00022 {
    00023         public:
    00024
    00026                 quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
    00027
    00029                 quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
    00030
    00032                 quaternion(f32 x, f32 y, f32 z);
    00033
    00035                 quaternion(const vector3df& vec);
    00036
    00038                 quaternion(const matrix4& mat);
    00039
    00041                 bool operator==(const quaternion& other) const;
    00042
    00044                 inline quaternion& operator=(const quaternion& other);
    00045
    00047                 inline quaternion& operator=(const matrix4& other);
    00048
    00050                 quaternion operator+(const quaternion& other) const;
    00051
    00053                 quaternion operator*(const quaternion& other) const;
    00054
    00056                 quaternion operator*(f32 s) const;
    00057
    00059                 quaternion& operator*=(f32 s);
    00060
    00062                 vector3df operator*(const vector3df& v) const;
    00063
    00065                 quaternion& operator*=(const quaternion& other);
    00066
    00068                 inline f32 dotProduct(const quaternion& other) const;
    00069
    00071                 inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
    00072
    00074                 inline quaternion& set(f32 x, f32 y, f32 z);
    00075
    00077                 inline quaternion& set(const core::vector3df& vec);
    00078
    00080                 inline quaternion& normalize();
    00081
    00083                 matrix4 getMatrix() const;
    00084
    00086                 void getMatrix( matrix4 &dest, const core::vector3df &translation ) const;
    00087
    00105                 void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;
    00106
    00108                 inline void getMatrix_transposed( matrix4 &dest ) const;
    00109
    00111                 quaternion& makeInverse();
    00112
    00114                 quaternion& slerp( quaternion q1, quaternion q2, f32 interpolate );
    00115
    00117
    00122                 quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
    00123
    00125                 void toAngleAxis (f32 &angle, core::vector3df& axis) const;
    00126
    00128                 void toEuler(vector3df& euler) const;
    00129
    00131                 quaternion& makeIdentity();
    00132
    00134                 quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
    00135
    00137                 f32 X; // vectorial (imaginary) part
    00138                 f32 Y;
    00139                 f32 Z;
    00140                 f32 W; // real part
    00141 };
    00142
    00143
    00144 // Constructor which converts euler angles to a quaternion
    00145 inline quaternion::quaternion(f32 x, f32 y, f32 z)
    00146 {
    00147         set(x,y,z);
    00148 }
    00149
    00150
    00151 // Constructor which converts euler angles to a quaternion
    00152 inline quaternion::quaternion(const vector3df& vec)
    00153 {
    00154         set(vec.X,vec.Y,vec.Z);
    00155 }
    00156
    00157
    00158 // Constructor which converts a matrix to a quaternion
    00159 inline quaternion::quaternion(const matrix4& mat)
    00160 {
    00161         (*this) = mat;
    00162 }
    00163
    00164
    00165 // equal operator
    00166 inline bool quaternion::operator==(const quaternion& other) const
    00167 {
    00168         return ((X == other.X) &&
    00169                 (Y == other.Y) &&
    00170                 (Z == other.Z) &&
    00171                 (W == other.W));
    00172 }
    00173
    00174
    00175 // assignment operator
    00176 inline quaternion& quaternion::operator=(const quaternion& other)
    00177 {
    00178         X = other.X;
    00179         Y = other.Y;
    00180         Z = other.Z;
    00181         W = other.W;
    00182         return *this;
    00183 }
    00184
    00185
    00186 // matrix assignment operator
    00187 inline quaternion& quaternion::operator=(const matrix4& m)
    00188 {
    00189         const f32 diag = m(0,0) + m(1,1) + m(2,2) + 1;
    00190
    00191         if( diag > 0.0f )
    00192         {
    00193                 const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
    00194
    00195                 // TODO: speed this up
    00196                 X = ( m(2,1) - m(1,2)) / scale;
    00197                 Y = ( m(0,2) - m(2,0)) / scale;
    00198                 Z = ( m(1,0) - m(0,1)) / scale;
    00199                 W = 0.25f * scale;
    00200         }
    00201         else
    00202         {
    00203                 if ( m(0,0) > m(1,1) && m(0,0) > m(2,2))
    00204                 {
    00205                         // 1st element of diag is greatest value
    00206                         // find scale according to 1st element, and double it
    00207                         const f32 scale = sqrtf( 1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;
    00208
    00209                         // TODO: speed this up
    00210                         X = 0.25f * scale;
    00211                         Y = (m(0,1) + m(1,0)) / scale;
    00212                         Z = (m(2,0) + m(0,2)) / scale;
    00213                         W = (m(2,1) - m(1,2)) / scale;
    00214                 }
    00215                 else if ( m(1,1) > m(2,2))
    00216                 {
    00217                         // 2nd element of diag is greatest value
    00218                         // find scale according to 2nd element, and double it
    00219                         const f32 scale = sqrtf( 1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;
    00220
    00221                         // TODO: speed this up
    00222                         X = (m(0,1) + m(1,0) ) / scale;
    00223                         Y = 0.25f * scale;
    00224                         Z = (m(1,2) + m(2,1) ) / scale;
    00225                         W = (m(0,2) - m(2,0) ) / scale;
    00226                 }
    00227                 else
    00228                 {
    00229                         // 3rd element of diag is greatest value
    00230                         // find scale according to 3rd element, and double it
    00231                         const f32 scale = sqrtf( 1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;
    00232
    00233                         // TODO: speed this up
    00234                         X = (m(0,2) + m(2,0)) / scale;
    00235                         Y = (m(1,2) + m(2,1)) / scale;
    00236                         Z = 0.25f * scale;
    00237                         W = (m(1,0) - m(0,1)) / scale;
    00238                 }
    00239         }
    00240
    00241         return normalize();
    00242 }
    00243
    00244
    00245 // multiplication operator
    00246 inline quaternion quaternion::operator*(const quaternion& other) const
    00247 {
    00248         quaternion tmp;
    00249
    00250         tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
    00251         tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
    00252         tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
    00253         tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
    00254
    00255         return tmp;
    00256 }
    00257
    00258
    00259 // multiplication operator
    00260 inline quaternion quaternion::operator*(f32 s) const
    00261 {
    00262         return quaternion(s*X, s*Y, s*Z, s*W);
    00263 }
    00264
    00265 // multiplication operator
    00266 inline quaternion& quaternion::operator*=(f32 s)
    00267 {
    00268         X*=s;
    00269         Y*=s;
    00270         Z*=s;
    00271         W*=s;
    00272         return *this;
    00273 }
    00274
    00275 // multiplication operator
    00276 inline quaternion& quaternion::operator*=(const quaternion& other)
    00277 {
    00278         return (*this = other * (*this));
    00279 }
    00280
    00281 // add operator
    00282 inline quaternion quaternion::operator+(const quaternion& b) const
    00283 {
    00284         return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
    00285 }
    00286
    00287
    00288 // Creates a matrix from this quaternion
    00289 inline matrix4 quaternion::getMatrix() const
    00290 {
    00291         core::matrix4 m;
    00292         getMatrix_transposed(m);
    00293         return m;
    00294 }
    00295
    00296
    00300 inline void quaternion::getMatrix( matrix4 &dest, const core::vector3df &center ) const
    00301 {
    00302         f32 * m = dest.pointer();
    00303
    00304         m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00305         m[1] = 2.0f*X*Y + 2.0f*Z*W;
    00306         m[2] = 2.0f*X*Z - 2.0f*Y*W;
    00307         m[3] = 0.0f;
    00308
    00309         m[4] = 2.0f*X*Y - 2.0f*Z*W;
    00310         m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00311         m[6] = 2.0f*Z*Y + 2.0f*X*W;
    00312         m[7] = 0.0f;
    00313
    00314         m[8] = 2.0f*X*Z + 2.0f*Y*W;
    00315         m[9] = 2.0f*Z*Y - 2.0f*X*W;
    00316         m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00317         m[11] = 0.0f;
    00318
    00319         m[12] = center.X;
    00320         m[13] = center.Y;
    00321         m[14] = center.Z;
    00322         m[15] = 1.f;
    00323
    00324         //dest.setDefinitelyIdentityMatrix ( matrix4::BIT_IS_NOT_IDENTITY );
    00325         dest.setDefinitelyIdentityMatrix ( false );
    00326 }
    00327
    00328
    00329
    00342 inline void quaternion::getMatrixCenter(matrix4 &dest,
    00343                                         const core::vector3df &center,
    00344                                         const core::vector3df &translation) const
    00345 {
    00346         f32 * m = dest.pointer();
    00347
    00348         m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00349         m[1] = 2.0f*X*Y + 2.0f*Z*W;
    00350         m[2] = 2.0f*X*Z - 2.0f*Y*W;
    00351         m[3] = 0.0f;
    00352
    00353         m[4] = 2.0f*X*Y - 2.0f*Z*W;
    00354         m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00355         m[6] = 2.0f*Z*Y + 2.0f*X*W;
    00356         m[7] = 0.0f;
    00357
    00358         m[8] = 2.0f*X*Z + 2.0f*Y*W;
    00359         m[9] = 2.0f*Z*Y - 2.0f*X*W;
    00360         m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00361         m[11] = 0.0f;
    00362
    00363         dest.setRotationCenter ( center, translation );
    00364 }
    00365
    00366 // Creates a matrix from this quaternion
    00367 inline void quaternion::getMatrix_transposed( matrix4 &dest ) const
    00368 {
    00369         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00370         dest[4] = 2.0f*X*Y + 2.0f*Z*W;
    00371         dest[8] = 2.0f*X*Z - 2.0f*Y*W;
    00372         dest[12] = 0.0f;
    00373
    00374         dest[1] = 2.0f*X*Y - 2.0f*Z*W;
    00375         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00376         dest[9] = 2.0f*Z*Y + 2.0f*X*W;
    00377         dest[13] = 0.0f;
    00378
    00379         dest[2] = 2.0f*X*Z + 2.0f*Y*W;
    00380         dest[6] = 2.0f*Z*Y - 2.0f*X*W;
    00381         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00382         dest[14] = 0.0f;
    00383
    00384         dest[3] = 0.f;
    00385         dest[7] = 0.f;
    00386         dest[11] = 0.f;
    00387         dest[15] = 1.f;
    00388         //dest.setDefinitelyIdentityMatrix ( matrix4::BIT_IS_NOT_IDENTITY );
    00389         dest.setDefinitelyIdentityMatrix ( false );
    00390 }
    00391
    00392
    00393
    00394 // Inverts this quaternion
    00395 inline quaternion& quaternion::makeInverse()
    00396 {
    00397         X = -X; Y = -Y; Z = -Z;
    00398         return *this;
    00399 }
    00400
    00401 // sets new quaternion
    00402 inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)
    00403 {
    00404         X = x;
    00405         Y = y;
    00406         Z = z;
    00407         W = w;
    00408         return *this;
    00409 }
    00410
    00411
    00412 // sets new quaternion based on euler angles
    00413 inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
    00414 {
    00415         f64 angle;
    00416
    00417         angle = x * 0.5;
    00418         const f64 sr = sin(angle);
    00419         const f64 cr = cos(angle);
    00420
    00421         angle = y * 0.5;
    00422         const f64 sp = sin(angle);
    00423         const f64 cp = cos(angle);
    00424
    00425         angle = z * 0.5;
    00426         const f64 sy = sin(angle);
    00427         const f64 cy = cos(angle);
    00428
    00429         const f64 cpcy = cp * cy;
    00430         const f64 spcy = sp * cy;
    00431         const f64 cpsy = cp * sy;
    00432         const f64 spsy = sp * sy;
    00433
    00434         X = (f32)(sr * cpcy - cr * spsy);
    00435         Y = (f32)(cr * spcy + sr * cpsy);
    00436         Z = (f32)(cr * cpsy - sr * spcy);
    00437         W = (f32)(cr * cpcy + sr * spsy);
    00438
    00439         return normalize();
    00440 }
    00441
    00442 // sets new quaternion based on euler angles
    00443 inline quaternion& quaternion::set(const core::vector3df& vec)
    00444 {
    00445         return set(vec.X, vec.Y, vec.Z);
    00446 }
    00447
    00448 // normalizes the quaternion
    00449 inline quaternion& quaternion::normalize()
    00450 {
    00451         const f32 n = X*X + Y*Y + Z*Z + W*W;
    00452
    00453         if (n == 1)
    00454                 return *this;
    00455
    00456         //n = 1.0f / sqrtf(n);
    00457         return (*this *= reciprocal_squareroot ( n ));
    00458 }
    00459
    00460
    00461 // set this quaternion to the result of the interpolation between two quaternions
    00462 inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time)
    00463 {
    00464         f32 angle = q1.dotProduct(q2);
    00465
    00466         if (angle < 0.0f)
    00467         {
    00468                 q1 *= -1.0f;
    00469                 angle *= -1.0f;
    00470         }
    00471
    00472         f32 scale;
    00473         f32 invscale;
    00474
    00475         if ((angle + 1.0f) > 0.05f)
    00476         {
    00477                 if ((1.0f - angle) >= 0.05f) // spherical interpolation
    00478                 {
    00479                         const f32 theta = acosf(angle);
    00480                         const f32 invsintheta = reciprocal(sinf(theta));
    00481                         scale = sinf(theta * (1.0f-time)) * invsintheta;
    00482                         invscale = sinf(theta * time) * invsintheta;
    00483                 }
    00484                 else // linear interploation
    00485                 {
    00486                         scale = 1.0f - time;
    00487                         invscale = time;
    00488                 }
    00489         }
    00490         else
    00491         {
    00492                 q2.set(-q1.Y, q1.X, -q1.W, q1.Z);
    00493                 scale = sinf(PI * (0.5f - time));
    00494                 invscale = sinf(PI * time);
    00495         }
    00496
    00497         return (*this = (q1*scale) + (q2*invscale));
    00498 }
    00499
    00500
    00501 // calculates the dot product
    00502 inline f32 quaternion::dotProduct(const quaternion& q2) const
    00503 {
    00504         return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
    00505 }
    00506
    00507
    00510 inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
    00511 {
    00512         const f32 fHalfAngle = 0.5f*angle;
    00513         const f32 fSin = sinf(fHalfAngle);
    00514         W = cosf(fHalfAngle);
    00515         X = fSin*axis.X;
    00516         Y = fSin*axis.Y;
    00517         Z = fSin*axis.Z;
    00518         return *this;
    00519 }
    00520
    00521
    00522 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
    00523 {
    00524         const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
    00525
    00526         if (core::iszero(scale) || W > 1.0f || W < -1.0f)
    00527         {
    00528                 angle = 0.0f;
    00529                 axis.X = 0.0f;
    00530                 axis.Y = 1.0f;
    00531                 axis.Z = 0.0f;
    00532         }
    00533         else
    00534         {
    00535                 const f32 invscale = reciprocal(scale);
    00536                 angle = 2.0f * acosf(W);
    00537                 axis.X = X * invscale;
    00538                 axis.Y = Y * invscale;
    00539                 axis.Z = Z * invscale;
    00540         }
    00541 }
    00542
    00543 inline void quaternion::toEuler(vector3df& euler) const
    00544 {
    00545         const f64 sqw = W*W;
    00546         const f64 sqx = X*X;
    00547         const f64 sqy = Y*Y;
    00548         const f64 sqz = Z*Z;
    00549
    00550         // heading = rotation about z-axis
    00551         euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
    00552
    00553         // bank = rotation about x-axis
    00554         euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
    00555
    00556         // attitude = rotation about y-axis
    00557         euler.Y = asinf( clamp(-2.0f * (X*Z - Y*W), -1.0f, 1.0f) );
    00558 }
    00559
    00560
    00561 inline vector3df quaternion::operator* (const vector3df& v) const
    00562 {
    00563         // nVidia SDK implementation
    00564
    00565         vector3df uv, uuv;
    00566         vector3df qvec(X, Y, Z);
    00567         uv = qvec.crossProduct(v);
    00568         uuv = qvec.crossProduct(uv);
    00569         uv *= (2.0f * W);
    00570         uuv *= 2.0f;
    00571
    00572         return v + uv + uuv;
    00573 }
    00574
    00575 // set quaternion to identity
    00576 inline core::quaternion& quaternion::makeIdentity()
    00577 {
    00578         W = 1.f;
    00579         X = 0.f;
    00580         Y = 0.f;
    00581         Z = 0.f;
    00582         return *this;
    00583 }
    00584
    00585 inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
    00586 {
    00587         // Based on Stan Melax's article in Game Programming Gems
    00588         // Copy, since cannot modify local
    00589         vector3df v0 = from;
    00590         vector3df v1 = to;
    00591         v0.normalize();
    00592         v1.normalize();
    00593
    00594         const f32 d = v0.dotProduct(v1);
    00595         if (d >= 1.0f) // If dot == 1, vectors are the same
    00596         {
    00597                 return makeIdentity();
    00598         }
    00599
    00600         const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
    00601         const f32 invs = 1.f / s;
    00602         const vector3df c = v0.crossProduct(v1)*invs;
    00603         X = c.X;
    00604         Y = c.Y;
    00605         Z = c.Z;
    00606         W = s * 0.5f;
    00607
    00608         return *this;
    00609 }
    00610
    00611
    00612 } // end namespace core
    00613 } // end namespace irr
    00614
    00615 #endif
    00616

| The Irrlicht Engine Documentation © 2003-2009 by Nikolaus Gebhardt. Generated on Sun Jan 10 09:24:04 2010 by Doxygen (1.5.6)