Irrlicht Engine: quaternion.h Source File

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-2010 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                 bool operator!=(const quaternion& other) const;
    00045
    00047                 inline quaternion& operator=(const quaternion& other);
    00048
    00050                 inline quaternion& operator=(const matrix4& other);
    00051
    00053                 quaternion operator+(const quaternion& other) const;
    00054
    00056                 quaternion operator*(const quaternion& other) const;
    00057
    00059                 quaternion operator*(f32 s) const;
    00060
    00062                 quaternion& operator*=(f32 s);
    00063
    00065                 vector3df operator*(const vector3df& v) const;
    00066
    00068                 quaternion& operator*=(const quaternion& other);
    00069
    00071                 inline f32 dotProduct(const quaternion& other) const;
    00072
    00074                 inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
    00075
    00077                 inline quaternion& set(f32 x, f32 y, f32 z);
    00078
    00080                 inline quaternion& set(const core::vector3df& vec);
    00081
    00083                 inline quaternion& set(const core::quaternion& quat);
    00084
    00086                 inline bool equals(const quaternion& other,
    00087                                 const f32 tolerance = ROUNDING_ERROR_f32 ) const;
    00088
    00090                 inline quaternion& normalize();
    00091
    00093                 matrix4 getMatrix() const;
    00094
    00096                 void getMatrix( matrix4 &dest, const core::vector3df &translation ) const;
    00097
    00115                 void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;
    00116
    00118                 inline void getMatrix_transposed( matrix4 &dest ) const;
    00119
    00121                 quaternion& makeInverse();
    00122
    00124                 quaternion& slerp( quaternion q1, quaternion q2, f32 interpolate );
    00125
    00127
    00132                 quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
    00133
    00135                 void toAngleAxis (f32 &angle, core::vector3df& axis) const;
    00136
    00138                 void toEuler(vector3df& euler) const;
    00139
    00141                 quaternion& makeIdentity();
    00142
    00144                 quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
    00145
    00147                 f32 X; // vectorial (imaginary) part
    00148                 f32 Y;
    00149                 f32 Z;
    00150                 f32 W; // real part
    00151 };
    00152
    00153
    00154 // Constructor which converts euler angles to a quaternion
    00155 inline quaternion::quaternion(f32 x, f32 y, f32 z)
    00156 {
    00157         set(x,y,z);
    00158 }
    00159
    00160
    00161 // Constructor which converts euler angles to a quaternion
    00162 inline quaternion::quaternion(const vector3df& vec)
    00163 {
    00164         set(vec.X,vec.Y,vec.Z);
    00165 }
    00166
    00167
    00168 // Constructor which converts a matrix to a quaternion
    00169 inline quaternion::quaternion(const matrix4& mat)
    00170 {
    00171         (*this) = mat;
    00172 }
    00173
    00174
    00175 // equal operator
    00176 inline bool quaternion::operator==(const quaternion& other) const
    00177 {
    00178         return ((X == other.X) &&
    00179                 (Y == other.Y) &&
    00180                 (Z == other.Z) &&
    00181                 (W == other.W));
    00182 }
    00183
    00184 // inequality operator
    00185 inline bool quaternion::operator!=(const quaternion& other) const
    00186 {
    00187         return !(*this == other);
    00188 }
    00189
    00190 // assignment operator
    00191 inline quaternion& quaternion::operator=(const quaternion& other)
    00192 {
    00193         X = other.X;
    00194         Y = other.Y;
    00195         Z = other.Z;
    00196         W = other.W;
    00197         return *this;
    00198 }
    00199
    00200
    00201 // matrix assignment operator
    00202 inline quaternion& quaternion::operator=(const matrix4& m)
    00203 {
    00204         const f32 diag = m(0,0) + m(1,1) + m(2,2) + 1;
    00205
    00206         if( diag > 0.0f )
    00207         {
    00208                 const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
    00209
    00210                 // TODO: speed this up
    00211                 X = ( m(2,1) - m(1,2)) / scale;
    00212                 Y = ( m(0,2) - m(2,0)) / scale;
    00213                 Z = ( m(1,0) - m(0,1)) / scale;
    00214                 W = 0.25f * scale;
    00215         }
    00216         else
    00217         {
    00218                 if ( m(0,0) > m(1,1) && m(0,0) > m(2,2))
    00219                 {
    00220                         // 1st element of diag is greatest value
    00221                         // find scale according to 1st element, and double it
    00222                         const f32 scale = sqrtf( 1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;
    00223
    00224                         // TODO: speed this up
    00225                         X = 0.25f * scale;
    00226                         Y = (m(0,1) + m(1,0)) / scale;
    00227                         Z = (m(2,0) + m(0,2)) / scale;
    00228                         W = (m(2,1) - m(1,2)) / scale;
    00229                 }
    00230                 else if ( m(1,1) > m(2,2))
    00231                 {
    00232                         // 2nd element of diag is greatest value
    00233                         // find scale according to 2nd element, and double it
    00234                         const f32 scale = sqrtf( 1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;
    00235
    00236                         // TODO: speed this up
    00237                         X = (m(0,1) + m(1,0) ) / scale;
    00238                         Y = 0.25f * scale;
    00239                         Z = (m(1,2) + m(2,1) ) / scale;
    00240                         W = (m(0,2) - m(2,0) ) / scale;
    00241                 }
    00242                 else
    00243                 {
    00244                         // 3rd element of diag is greatest value
    00245                         // find scale according to 3rd element, and double it
    00246                         const f32 scale = sqrtf( 1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;
    00247
    00248                         // TODO: speed this up
    00249                         X = (m(0,2) + m(2,0)) / scale;
    00250                         Y = (m(1,2) + m(2,1)) / scale;
    00251                         Z = 0.25f * scale;
    00252                         W = (m(1,0) - m(0,1)) / scale;
    00253                 }
    00254         }
    00255
    00256         return normalize();
    00257 }
    00258
    00259
    00260 // multiplication operator
    00261 inline quaternion quaternion::operator*(const quaternion& other) const
    00262 {
    00263         quaternion tmp;
    00264
    00265         tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
    00266         tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
    00267         tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
    00268         tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
    00269
    00270         return tmp;
    00271 }
    00272
    00273
    00274 // multiplication operator
    00275 inline quaternion quaternion::operator*(f32 s) const
    00276 {
    00277         return quaternion(s*X, s*Y, s*Z, s*W);
    00278 }
    00279
    00280 // multiplication operator
    00281 inline quaternion& quaternion::operator*=(f32 s)
    00282 {
    00283         X*=s;
    00284         Y*=s;
    00285         Z*=s;
    00286         W*=s;
    00287         return *this;
    00288 }
    00289
    00290 // multiplication operator
    00291 inline quaternion& quaternion::operator*=(const quaternion& other)
    00292 {
    00293         return (*this = other * (*this));
    00294 }
    00295
    00296 // add operator
    00297 inline quaternion quaternion::operator+(const quaternion& b) const
    00298 {
    00299         return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
    00300 }
    00301
    00302
    00303 // Creates a matrix from this quaternion
    00304 inline matrix4 quaternion::getMatrix() const
    00305 {
    00306         core::matrix4 m;
    00307         getMatrix_transposed(m);
    00308         return m;
    00309 }
    00310
    00311
    00315 inline void quaternion::getMatrix( matrix4 &dest, const core::vector3df &center ) const
    00316 {
    00317         f32 * m = dest.pointer();
    00318
    00319         m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00320         m[1] = 2.0f*X*Y + 2.0f*Z*W;
    00321         m[2] = 2.0f*X*Z - 2.0f*Y*W;
    00322         m[3] = 0.0f;
    00323
    00324         m[4] = 2.0f*X*Y - 2.0f*Z*W;
    00325         m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00326         m[6] = 2.0f*Z*Y + 2.0f*X*W;
    00327         m[7] = 0.0f;
    00328
    00329         m[8] = 2.0f*X*Z + 2.0f*Y*W;
    00330         m[9] = 2.0f*Z*Y - 2.0f*X*W;
    00331         m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00332         m[11] = 0.0f;
    00333
    00334         m[12] = center.X;
    00335         m[13] = center.Y;
    00336         m[14] = center.Z;
    00337         m[15] = 1.f;
    00338
    00339         //dest.setDefinitelyIdentityMatrix ( matrix4::BIT_IS_NOT_IDENTITY );
    00340         dest.setDefinitelyIdentityMatrix ( false );
    00341 }
    00342
    00343
    00344
    00357 inline void quaternion::getMatrixCenter(matrix4 &dest,
    00358                                         const core::vector3df &center,
    00359                                         const core::vector3df &translation) const
    00360 {
    00361         f32 * m = dest.pointer();
    00362
    00363         m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00364         m[1] = 2.0f*X*Y + 2.0f*Z*W;
    00365         m[2] = 2.0f*X*Z - 2.0f*Y*W;
    00366         m[3] = 0.0f;
    00367
    00368         m[4] = 2.0f*X*Y - 2.0f*Z*W;
    00369         m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00370         m[6] = 2.0f*Z*Y + 2.0f*X*W;
    00371         m[7] = 0.0f;
    00372
    00373         m[8] = 2.0f*X*Z + 2.0f*Y*W;
    00374         m[9] = 2.0f*Z*Y - 2.0f*X*W;
    00375         m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00376         m[11] = 0.0f;
    00377
    00378         dest.setRotationCenter ( center, translation );
    00379 }
    00380
    00381 // Creates a matrix from this quaternion
    00382 inline void quaternion::getMatrix_transposed( matrix4 &dest ) const
    00383 {
    00384         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00385         dest[4] = 2.0f*X*Y + 2.0f*Z*W;
    00386         dest[8] = 2.0f*X*Z - 2.0f*Y*W;
    00387         dest[12] = 0.0f;
    00388
    00389         dest[1] = 2.0f*X*Y - 2.0f*Z*W;
    00390         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00391         dest[9] = 2.0f*Z*Y + 2.0f*X*W;
    00392         dest[13] = 0.0f;
    00393
    00394         dest[2] = 2.0f*X*Z + 2.0f*Y*W;
    00395         dest[6] = 2.0f*Z*Y - 2.0f*X*W;
    00396         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00397         dest[14] = 0.0f;
    00398
    00399         dest[3] = 0.f;
    00400         dest[7] = 0.f;
    00401         dest[11] = 0.f;
    00402         dest[15] = 1.f;
    00403         //dest.setDefinitelyIdentityMatrix ( matrix4::BIT_IS_NOT_IDENTITY );
    00404         dest.setDefinitelyIdentityMatrix ( false );
    00405 }
    00406
    00407
    00408
    00409 // Inverts this quaternion
    00410 inline quaternion& quaternion::makeInverse()
    00411 {
    00412         X = -X; Y = -Y; Z = -Z;
    00413         return *this;
    00414 }
    00415
    00416 // sets new quaternion
    00417 inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)
    00418 {
    00419         X = x;
    00420         Y = y;
    00421         Z = z;
    00422         W = w;
    00423         return *this;
    00424 }
    00425
    00426
    00427 // sets new quaternion based on euler angles
    00428 inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
    00429 {
    00430         f64 angle;
    00431
    00432         angle = x * 0.5;
    00433         const f64 sr = sin(angle);
    00434         const f64 cr = cos(angle);
    00435
    00436         angle = y * 0.5;
    00437         const f64 sp = sin(angle);
    00438         const f64 cp = cos(angle);
    00439
    00440         angle = z * 0.5;
    00441         const f64 sy = sin(angle);
    00442         const f64 cy = cos(angle);
    00443
    00444         const f64 cpcy = cp * cy;
    00445         const f64 spcy = sp * cy;
    00446         const f64 cpsy = cp * sy;
    00447         const f64 spsy = sp * sy;
    00448
    00449         X = (f32)(sr * cpcy - cr * spsy);
    00450         Y = (f32)(cr * spcy + sr * cpsy);
    00451         Z = (f32)(cr * cpsy - sr * spcy);
    00452         W = (f32)(cr * cpcy + sr * spsy);
    00453
    00454         return normalize();
    00455 }
    00456
    00457 // sets new quaternion based on euler angles
    00458 inline quaternion& quaternion::set(const core::vector3df& vec)
    00459 {
    00460         return set(vec.X, vec.Y, vec.Z);
    00461 }
    00462
    00463 // sets new quaternion based on other quaternion
    00464 inline quaternion& quaternion::set(const core::quaternion& quat)
    00465 {
    00466         return (*this=quat);
    00467 }
    00468
    00469
    00471 inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const
    00472 {
    00473         return core::equals(X, other.X, tolerance) &&
    00474                 core::equals(Y, other.Y, tolerance) &&
    00475                 core::equals(Z, other.Z, tolerance) &&
    00476                 core::equals(W, other.W, tolerance);
    00477 }
    00478
    00479
    00480 // normalizes the quaternion
    00481 inline quaternion& quaternion::normalize()
    00482 {
    00483         const f32 n = X*X + Y*Y + Z*Z + W*W;
    00484
    00485         if (n == 1)
    00486                 return *this;
    00487
    00488         //n = 1.0f / sqrtf(n);
    00489         return (*this *= reciprocal_squareroot ( n ));
    00490 }
    00491
    00492
    00493 // set this quaternion to the result of the interpolation between two quaternions
    00494 inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time)
    00495 {
    00496         f32 angle = q1.dotProduct(q2);
    00497
    00498         if (angle < 0.0f)
    00499         {
    00500                 q1 *= -1.0f;
    00501                 angle *= -1.0f;
    00502         }
    00503
    00504         f32 scale;
    00505         f32 invscale;
    00506
    00507         if ((angle + 1.0f) > 0.05f)
    00508         {
    00509                 if ((1.0f - angle) >= 0.05f) // spherical interpolation
    00510                 {
    00511                         const f32 theta = acosf(angle);
    00512                         const f32 invsintheta = reciprocal(sinf(theta));
    00513                         scale = sinf(theta * (1.0f-time)) * invsintheta;
    00514                         invscale = sinf(theta * time) * invsintheta;
    00515                 }
    00516                 else // linear interploation
    00517                 {
    00518                         scale = 1.0f - time;
    00519                         invscale = time;
    00520                 }
    00521         }
    00522         else
    00523         {
    00524                 q2.set(-q1.Y, q1.X, -q1.W, q1.Z);
    00525                 scale = sinf(PI * (0.5f - time));
    00526                 invscale = sinf(PI * time);
    00527         }
    00528
    00529         return (*this = (q1*scale) + (q2*invscale));
    00530 }
    00531
    00532
    00533 // calculates the dot product
    00534 inline f32 quaternion::dotProduct(const quaternion& q2) const
    00535 {
    00536         return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
    00537 }
    00538
    00539
    00542 inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
    00543 {
    00544         const f32 fHalfAngle = 0.5f*angle;
    00545         const f32 fSin = sinf(fHalfAngle);
    00546         W = cosf(fHalfAngle);
    00547         X = fSin*axis.X;
    00548         Y = fSin*axis.Y;
    00549         Z = fSin*axis.Z;
    00550         return *this;
    00551 }
    00552
    00553
    00554 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
    00555 {
    00556         const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
    00557
    00558         if (core::iszero(scale) || W > 1.0f || W < -1.0f)
    00559         {
    00560                 angle = 0.0f;
    00561                 axis.X = 0.0f;
    00562                 axis.Y = 1.0f;
    00563                 axis.Z = 0.0f;
    00564         }
    00565         else
    00566         {
    00567                 const f32 invscale = reciprocal(scale);
    00568                 angle = 2.0f * acosf(W);
    00569                 axis.X = X * invscale;
    00570                 axis.Y = Y * invscale;
    00571                 axis.Z = Z * invscale;
    00572         }
    00573 }
    00574
    00575 inline void quaternion::toEuler(vector3df& euler) const
    00576 {
    00577         const f64 sqw = W*W;
    00578         const f64 sqx = X*X;
    00579         const f64 sqy = Y*Y;
    00580         const f64 sqz = Z*Z;
    00581
    00582         // heading = rotation about z-axis
    00583         euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
    00584
    00585         // bank = rotation about x-axis
    00586         euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
    00587
    00588         // attitude = rotation about y-axis
    00589         euler.Y = asinf( clamp(-2.0f * (X*Z - Y*W), -1.0f, 1.0f) );
    00590 }
    00591
    00592
    00593 inline vector3df quaternion::operator* (const vector3df& v) const
    00594 {
    00595         // nVidia SDK implementation
    00596
    00597         vector3df uv, uuv;
    00598         vector3df qvec(X, Y, Z);
    00599         uv = qvec.crossProduct(v);
    00600         uuv = qvec.crossProduct(uv);
    00601         uv *= (2.0f * W);
    00602         uuv *= 2.0f;
    00603
    00604         return v + uv + uuv;
    00605 }
    00606
    00607 // set quaternion to identity
    00608 inline core::quaternion& quaternion::makeIdentity()
    00609 {
    00610         W = 1.f;
    00611         X = 0.f;
    00612         Y = 0.f;
    00613         Z = 0.f;
    00614         return *this;
    00615 }
    00616
    00617 inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
    00618 {
    00619         // Based on Stan Melax's article in Game Programming Gems
    00620         // Copy, since cannot modify local
    00621         vector3df v0 = from;
    00622         vector3df v1 = to;
    00623         v0.normalize();
    00624         v1.normalize();
    00625
    00626         const f32 d = v0.dotProduct(v1);
    00627         if (d >= 1.0f) // If dot == 1, vectors are the same
    00628         {
    00629                 return makeIdentity();
    00630         }
    00631         else if (d <= -1.0f) // exactly opposite
    00632         {
    00633                 core::vector3df axis(1.0f, 0.f, 0.f);
    00634                 axis = axis.crossProduct(core::vector3df(X,Y,Z));
    00635                 if (axis.getLength()==0)
    00636                 {
    00637                         axis.set(0.f,1.f,0.f);
    00638                         axis.crossProduct(core::vector3df(X,Y,Z));
    00639                 }
    00640                 return this->fromAngleAxis(core::PI, axis);
    00641         }
    00642
    00643         const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
    00644         const f32 invs = 1.f / s;
    00645         const vector3df c = v0.crossProduct(v1)*invs;
    00646         X = c.X;
    00647         Y = c.Y;
    00648         Z = c.Z;
    00649         W = s * 0.5f;
    00650
    00651         return *this;
    00652 }
    00653
    00654
    00655 } // end namespace core
    00656 } // end namespace irr
    00657
    00658 #endif
    00659

| The Irrlicht Engine Documentation © 2003-2010 by Nikolaus Gebhardt. Generated on Sun Oct 24 12:41:58 2010 by Doxygen (1.6.2)