doc/html/quaternion_8h_source.html

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.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
-