Irrlicht 3D Engine: quaternion.h Source File

| Irrlicht 3D Engine |

quaternion.h
Go to the documentation of this file.

    00001 // Copyright (C) 2002-2012 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 // Between Irrlicht 1.7 and Irrlicht 1.8 the quaternion-matrix conversions got fixed.
    00014 // This define disables all involved functions completely to allow finding all places
    00015 // where the wrong conversions had been in use.
    00016 #define IRR_TEST_BROKEN_QUATERNION_USE 0
    00017
    00018 namespace irr
    00019 {
    00020 namespace core
    00021 {
    00022
    00024
    00026 class quaternion
    00027 {
    00028     public:
    00029
    00031         quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
    00032
    00034         quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
    00035
    00037         quaternion(f32 x, f32 y, f32 z);
    00038
    00040         quaternion(const vector3df& vec);
    00041
    00042 #if !IRR_TEST_BROKEN_QUATERNION_USE
    00043
    00044         quaternion(const matrix4& mat);
    00045 #endif
    00046
    00048         bool operator==(const quaternion& other) const;
    00049
    00051         bool operator!=(const quaternion& other) const;
    00052
    00054         inline quaternion& operator=(const quaternion& other);
    00055
    00056 #if !IRR_TEST_BROKEN_QUATERNION_USE
    00057
    00058         inline quaternion& operator=(const matrix4& other);
    00059 #endif
    00060
    00062         quaternion operator+(const quaternion& other) const;
    00063
    00065         quaternion operator*(const quaternion& other) const;
    00066
    00068         quaternion operator*(f32 s) const;
    00069
    00071         quaternion& operator*=(f32 s);
    00072
    00074         vector3df operator*(const vector3df& v) const;
    00075
    00077         quaternion& operator*=(const quaternion& other);
    00078
    00080         inline f32 dotProduct(const quaternion& other) const;
    00081
    00083         inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
    00084
    00086         inline quaternion& set(f32 x, f32 y, f32 z);
    00087
    00089         inline quaternion& set(const core::vector3df& vec);
    00090
    00092         inline quaternion& set(const core::quaternion& quat);
    00093
    00095         inline bool equals(const quaternion& other,
    00096                 const f32 tolerance = ROUNDING_ERROR_f32 ) const;
    00097
    00099         inline quaternion& normalize();
    00100
    00101 #if !IRR_TEST_BROKEN_QUATERNION_USE
    00102
    00103         matrix4 getMatrix() const;
    00104 #endif
    00105
    00107         void getMatrix( matrix4 &dest, const core::vector3df &translation=core::vector3df() ) const;
    00108
    00126         void getMatrixCenter( matrix4 &dest, const core::vector3df &center, const core::vector3df &translation ) const;
    00127
    00129         inline void getMatrix_transposed( matrix4 &dest ) const;
    00130
    00132         quaternion& makeInverse();
    00133
    00135
    00141         quaternion& lerp(quaternion q1, quaternion q2, f32 time);
    00142
    00144
    00155         quaternion& slerp(quaternion q1, quaternion q2,
    00156                 f32 time, f32 threshold=.05f);
    00157
    00159
    00164         quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
    00165
    00167         void toAngleAxis (f32 &angle, core::vector3df& axis) const;
    00168
    00170         void toEuler(vector3df& euler) const;
    00171
    00173         quaternion& makeIdentity();
    00174
    00176         quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
    00177
    00179         f32 X; // vectorial (imaginary) part
    00180         f32 Y;
    00181         f32 Z;
    00182         f32 W; // real part
    00183 };
    00184
    00185
    00186 // Constructor which converts euler angles to a quaternion
    00187 inline quaternion::quaternion(f32 x, f32 y, f32 z)
    00188 {
    00189     set(x,y,z);
    00190 }
    00191
    00192
    00193 // Constructor which converts euler angles to a quaternion
    00194 inline quaternion::quaternion(const vector3df& vec)
    00195 {
    00196     set(vec.X,vec.Y,vec.Z);
    00197 }
    00198
    00199 #if !IRR_TEST_BROKEN_QUATERNION_USE
    00200 // Constructor which converts a matrix to a quaternion
    00201 inline quaternion::quaternion(const matrix4& mat)
    00202 {
    00203     (*this) = mat;
    00204 }
    00205 #endif
    00206
    00207 // equal operator
    00208 inline bool quaternion::operator==(const quaternion& other) const
    00209 {
    00210     return ((X == other.X) &&
    00211         (Y == other.Y) &&
    00212         (Z == other.Z) &&
    00213         (W == other.W));
    00214 }
    00215
    00216 // inequality operator
    00217 inline bool quaternion::operator!=(const quaternion& other) const
    00218 {
    00219     return !(*this == other);
    00220 }
    00221
    00222 // assignment operator
    00223 inline quaternion& quaternion::operator=(const quaternion& other)
    00224 {
    00225     X = other.X;
    00226     Y = other.Y;
    00227     Z = other.Z;
    00228     W = other.W;
    00229     return *this;
    00230 }
    00231
    00232 #if !IRR_TEST_BROKEN_QUATERNION_USE
    00233 // matrix assignment operator
    00234 inline quaternion& quaternion::operator=(const matrix4& m)
    00235 {
    00236     const f32 diag = m[0] + m[5] + m[10] + 1;
    00237
    00238     if( diag > 0.0f )
    00239     {
    00240         const f32 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
    00241
    00242         // TODO: speed this up
    00243         X = (m[6] - m[9]) / scale;
    00244         Y = (m[8] - m[2]) / scale;
    00245         Z = (m[1] - m[4]) / scale;
    00246         W = 0.25f * scale;
    00247     }
    00248     else
    00249     {
    00250         if (m[0]>m[5] && m[0]>m[10])
    00251         {
    00252             // 1st element of diag is greatest value
    00253             // find scale according to 1st element, and double it
    00254             const f32 scale = sqrtf(1.0f + m[0] - m[5] - m[10]) * 2.0f;
    00255
    00256             // TODO: speed this up
    00257             X = 0.25f * scale;
    00258             Y = (m[4] + m[1]) / scale;
    00259             Z = (m[2] + m[8]) / scale;
    00260             W = (m[6] - m[9]) / scale;
    00261         }
    00262         else if (m[5]>m[10])
    00263         {
    00264             // 2nd element of diag is greatest value
    00265             // find scale according to 2nd element, and double it
    00266             const f32 scale = sqrtf(1.0f + m[5] - m[0] - m[10]) * 2.0f;
    00267
    00268             // TODO: speed this up
    00269             X = (m[4] + m[1]) / scale;
    00270             Y = 0.25f * scale;
    00271             Z = (m[9] + m[6]) / scale;
    00272             W = (m[8] - m[2]) / scale;
    00273         }
    00274         else
    00275         {
    00276             // 3rd element of diag is greatest value
    00277             // find scale according to 3rd element, and double it
    00278             const f32 scale = sqrtf(1.0f + m[10] - m[0] - m[5]) * 2.0f;
    00279
    00280             // TODO: speed this up
    00281             X = (m[8] + m[2]) / scale;
    00282             Y = (m[9] + m[6]) / scale;
    00283             Z = 0.25f * scale;
    00284             W = (m[1] - m[4]) / scale;
    00285         }
    00286     }
    00287
    00288     return normalize();
    00289 }
    00290 #endif
    00291
    00292
    00293 // multiplication operator
    00294 inline quaternion quaternion::operator*(const quaternion& other) const
    00295 {
    00296     quaternion tmp;
    00297
    00298     tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
    00299     tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
    00300     tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
    00301     tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
    00302
    00303     return tmp;
    00304 }
    00305
    00306
    00307 // multiplication operator
    00308 inline quaternion quaternion::operator*(f32 s) const
    00309 {
    00310     return quaternion(s*X, s*Y, s*Z, s*W);
    00311 }
    00312
    00313
    00314 // multiplication operator
    00315 inline quaternion& quaternion::operator*=(f32 s)
    00316 {
    00317     X*=s;
    00318     Y*=s;
    00319     Z*=s;
    00320     W*=s;
    00321     return *this;
    00322 }
    00323
    00324 // multiplication operator
    00325 inline quaternion& quaternion::operator*=(const quaternion& other)
    00326 {
    00327     return (*this = other * (*this));
    00328 }
    00329
    00330 // add operator
    00331 inline quaternion quaternion::operator+(const quaternion& b) const
    00332 {
    00333     return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
    00334 }
    00335
    00336 #if !IRR_TEST_BROKEN_QUATERNION_USE
    00337 // Creates a matrix from this quaternion
    00338 inline matrix4 quaternion::getMatrix() const
    00339 {
    00340     core::matrix4 m;
    00341     getMatrix(m);
    00342     return m;
    00343 }
    00344 #endif
    00345
    00349 inline void quaternion::getMatrix(matrix4 &dest,
    00350         const core::vector3df &center) const
    00351 {
    00352     dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00353     dest[1] = 2.0f*X*Y + 2.0f*Z*W;
    00354     dest[2] = 2.0f*X*Z - 2.0f*Y*W;
    00355     dest[3] = 0.0f;
    00356
    00357     dest[4] = 2.0f*X*Y - 2.0f*Z*W;
    00358     dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00359     dest[6] = 2.0f*Z*Y + 2.0f*X*W;
    00360     dest[7] = 0.0f;
    00361
    00362     dest[8] = 2.0f*X*Z + 2.0f*Y*W;
    00363     dest[9] = 2.0f*Z*Y - 2.0f*X*W;
    00364     dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00365     dest[11] = 0.0f;
    00366
    00367     dest[12] = center.X;
    00368     dest[13] = center.Y;
    00369     dest[14] = center.Z;
    00370     dest[15] = 1.f;
    00371
    00372     dest.setDefinitelyIdentityMatrix ( false );
    00373 }
    00374
    00375
    00388 inline void quaternion::getMatrixCenter(matrix4 &dest,
    00389                     const core::vector3df &center,
    00390                     const core::vector3df &translation) const
    00391 {
    00392     dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00393     dest[1] = 2.0f*X*Y + 2.0f*Z*W;
    00394     dest[2] = 2.0f*X*Z - 2.0f*Y*W;
    00395     dest[3] = 0.0f;
    00396
    00397     dest[4] = 2.0f*X*Y - 2.0f*Z*W;
    00398     dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00399     dest[6] = 2.0f*Z*Y + 2.0f*X*W;
    00400     dest[7] = 0.0f;
    00401
    00402     dest[8] = 2.0f*X*Z + 2.0f*Y*W;
    00403     dest[9] = 2.0f*Z*Y - 2.0f*X*W;
    00404     dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00405     dest[11] = 0.0f;
    00406
    00407     dest.setRotationCenter ( center, translation );
    00408 }
    00409
    00410 // Creates a matrix from this quaternion
    00411 inline void quaternion::getMatrix_transposed(matrix4 &dest) const
    00412 {
    00413     dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    00414     dest[4] = 2.0f*X*Y + 2.0f*Z*W;
    00415     dest[8] = 2.0f*X*Z - 2.0f*Y*W;
    00416     dest[12] = 0.0f;
    00417
    00418     dest[1] = 2.0f*X*Y - 2.0f*Z*W;
    00419     dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    00420     dest[9] = 2.0f*Z*Y + 2.0f*X*W;
    00421     dest[13] = 0.0f;
    00422
    00423     dest[2] = 2.0f*X*Z + 2.0f*Y*W;
    00424     dest[6] = 2.0f*Z*Y - 2.0f*X*W;
    00425     dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    00426     dest[14] = 0.0f;
    00427
    00428     dest[3] = 0.f;
    00429     dest[7] = 0.f;
    00430     dest[11] = 0.f;
    00431     dest[15] = 1.f;
    00432
    00433     dest.setDefinitelyIdentityMatrix(false);
    00434 }
    00435
    00436
    00437 // Inverts this quaternion
    00438 inline quaternion& quaternion::makeInverse()
    00439 {
    00440     X = -X; Y = -Y; Z = -Z;
    00441     return *this;
    00442 }
    00443
    00444
    00445 // sets new quaternion
    00446 inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)
    00447 {
    00448     X = x;
    00449     Y = y;
    00450     Z = z;
    00451     W = w;
    00452     return *this;
    00453 }
    00454
    00455
    00456 // sets new quaternion based on euler angles
    00457 inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
    00458 {
    00459     f64 angle;
    00460
    00461     angle = x * 0.5;
    00462     const f64 sr = sin(angle);
    00463     const f64 cr = cos(angle);
    00464
    00465     angle = y * 0.5;
    00466     const f64 sp = sin(angle);
    00467     const f64 cp = cos(angle);
    00468
    00469     angle = z * 0.5;
    00470     const f64 sy = sin(angle);
    00471     const f64 cy = cos(angle);
    00472
    00473     const f64 cpcy = cp * cy;
    00474     const f64 spcy = sp * cy;
    00475     const f64 cpsy = cp * sy;
    00476     const f64 spsy = sp * sy;
    00477
    00478     X = (f32)(sr * cpcy - cr * spsy);
    00479     Y = (f32)(cr * spcy + sr * cpsy);
    00480     Z = (f32)(cr * cpsy - sr * spcy);
    00481     W = (f32)(cr * cpcy + sr * spsy);
    00482
    00483     return normalize();
    00484 }
    00485
    00486 // sets new quaternion based on euler angles
    00487 inline quaternion& quaternion::set(const core::vector3df& vec)
    00488 {
    00489     return set(vec.X, vec.Y, vec.Z);
    00490 }
    00491
    00492 // sets new quaternion based on other quaternion
    00493 inline quaternion& quaternion::set(const core::quaternion& quat)
    00494 {
    00495     return (*this=quat);
    00496 }
    00497
    00498
    00500 inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const
    00501 {
    00502     return core::equals(X, other.X, tolerance) &&
    00503         core::equals(Y, other.Y, tolerance) &&
    00504         core::equals(Z, other.Z, tolerance) &&
    00505         core::equals(W, other.W, tolerance);
    00506 }
    00507
    00508
    00509 // normalizes the quaternion
    00510 inline quaternion& quaternion::normalize()
    00511 {
    00512     const f32 n = X*X + Y*Y + Z*Z + W*W;
    00513
    00514     if (n == 1)
    00515         return *this;
    00516
    00517     //n = 1.0f / sqrtf(n);
    00518     return (*this *= reciprocal_squareroot ( n ));
    00519 }
    00520
    00521
    00522 // set this quaternion to the result of the linear interpolation between two quaternions
    00523 inline quaternion& quaternion::lerp(quaternion q1, quaternion q2, f32 time)
    00524 {
    00525     const f32 scale = 1.0f - time;
    00526     return (*this = (q1*scale) + (q2*time));
    00527 }
    00528
    00529
    00530 // set this quaternion to the result of the interpolation between two quaternions
    00531 inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time, f32 threshold)
    00532 {
    00533     f32 angle = q1.dotProduct(q2);
    00534
    00535     // make sure we use the short rotation
    00536     if (angle < 0.0f)
    00537     {
    00538         q1 *= -1.0f;
    00539         angle *= -1.0f;
    00540     }
    00541
    00542     if (angle <= (1-threshold)) // spherical interpolation
    00543     {
    00544         const f32 theta = acosf(angle);
    00545         const f32 invsintheta = reciprocal(sinf(theta));
    00546         const f32 scale = sinf(theta * (1.0f-time)) * invsintheta;
    00547         const f32 invscale = sinf(theta * time) * invsintheta;
    00548         return (*this = (q1*scale) + (q2*invscale));
    00549     }
    00550     else // linear interploation
    00551         return lerp(q1,q2,time);
    00552 }
    00553
    00554
    00555 // calculates the dot product
    00556 inline f32 quaternion::dotProduct(const quaternion& q2) const
    00557 {
    00558     return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
    00559 }
    00560
    00561
    00563 inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
    00564 {
    00565     const f32 fHalfAngle = 0.5f*angle;
    00566     const f32 fSin = sinf(fHalfAngle);
    00567     W = cosf(fHalfAngle);
    00568     X = fSin*axis.X;
    00569     Y = fSin*axis.Y;
    00570     Z = fSin*axis.Z;
    00571     return *this;
    00572 }
    00573
    00574
    00575 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
    00576 {
    00577     const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
    00578
    00579     if (core::iszero(scale) || W > 1.0f || W < -1.0f)
    00580     {
    00581         angle = 0.0f;
    00582         axis.X = 0.0f;
    00583         axis.Y = 1.0f;
    00584         axis.Z = 0.0f;
    00585     }
    00586     else
    00587     {
    00588         const f32 invscale = reciprocal(scale);
    00589         angle = 2.0f * acosf(W);
    00590         axis.X = X * invscale;
    00591         axis.Y = Y * invscale;
    00592         axis.Z = Z * invscale;
    00593     }
    00594 }
    00595
    00596 inline void quaternion::toEuler(vector3df& euler) const
    00597 {
    00598     const f64 sqw = W*W;
    00599     const f64 sqx = X*X;
    00600     const f64 sqy = Y*Y;
    00601     const f64 sqz = Z*Z;
    00602     const f64 test = 2.0 * (Y*W - X*Z);
    00603
    00604     if (core::equals(test, 1.0, 0.000001))
    00605     {
    00606         // heading = rotation about z-axis
    00607         euler.Z = (f32) (-2.0*atan2(X, W));
    00608         // bank = rotation about x-axis
    00609         euler.X = 0;
    00610         // attitude = rotation about y-axis
    00611         euler.Y = (f32) (core::PI64/2.0);
    00612     }
    00613     else if (core::equals(test, -1.0, 0.000001))
    00614     {
    00615         // heading = rotation about z-axis
    00616         euler.Z = (f32) (2.0*atan2(X, W));
    00617         // bank = rotation about x-axis
    00618         euler.X = 0;
    00619         // attitude = rotation about y-axis
    00620         euler.Y = (f32) (core::PI64/-2.0);
    00621     }
    00622     else
    00623     {
    00624         // heading = rotation about z-axis
    00625         euler.Z = (f32) atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw));
    00626         // bank = rotation about x-axis
    00627         euler.X = (f32) atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw));
    00628         // attitude = rotation about y-axis
    00629         euler.Y = (f32) asin( clamp(test, -1.0, 1.0) );
    00630     }
    00631 }
    00632
    00633
    00634 inline vector3df quaternion::operator* (const vector3df& v) const
    00635 {
    00636     // nVidia SDK implementation
    00637
    00638     vector3df uv, uuv;
    00639     vector3df qvec(X, Y, Z);
    00640     uv = qvec.crossProduct(v);
    00641     uuv = qvec.crossProduct(uv);
    00642     uv *= (2.0f * W);
    00643     uuv *= 2.0f;
    00644
    00645     return v + uv + uuv;
    00646 }
    00647
    00648 // set quaternion to identity
    00649 inline core::quaternion& quaternion::makeIdentity()
    00650 {
    00651     W = 1.f;
    00652     X = 0.f;
    00653     Y = 0.f;
    00654     Z = 0.f;
    00655     return *this;
    00656 }
    00657
    00658 inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
    00659 {
    00660     // Based on Stan Melax's article in Game Programming Gems
    00661     // Copy, since cannot modify local
    00662     vector3df v0 = from;
    00663     vector3df v1 = to;
    00664     v0.normalize();
    00665     v1.normalize();
    00666
    00667     const f32 d = v0.dotProduct(v1);
    00668     if (d >= 1.0f) // If dot == 1, vectors are the same
    00669     {
    00670         return makeIdentity();
    00671     }
    00672     else if (d <= -1.0f) // exactly opposite
    00673     {
    00674         core::vector3df axis(1.0f, 0.f, 0.f);
    00675         axis = axis.crossProduct(v0);
    00676         if (axis.getLength()==0)
    00677         {
    00678             axis.set(0.f,1.f,0.f);
    00679             axis = axis.crossProduct(v0);
    00680         }
    00681         // same as fromAngleAxis(core::PI, axis).normalize();
    00682         return set(axis.X, axis.Y, axis.Z, 0).normalize();
    00683     }
    00684
    00685     const f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
    00686     const f32 invs = 1.f / s;
    00687     const vector3df c = v0.crossProduct(v1)*invs;
    00688     return set(c.X, c.Y, c.Z, s * 0.5f).normalize();
    00689 }
    00690
    00691
    00692 } // end namespace core
    00693 } // end namespace irr
    00694
    00695 #endif
    00696