Main Page Namespace List Class Hierarchy Alphabetical List Compound List File List Namespace Members Compound Members File Members Related Pages

# OgreQuaternion.cpp

Go to the documentation of this file.

    00001 /*
    00002 -----------------------------------------------------------------------------
    00003 This source file is part of OGRE
    00004     (Object-oriented Graphics Rendering Engine)
    00005 For the latest info, see http://www.ogre3d.org/
    00006
    00007 Copyright © 2000-2002 The OGRE Team
    00008 Also see acknowledgements in Readme.html
    00009
    00010 This program is free software; you can redistribute it and/or modify it under
    00011 the terms of the GNU Lesser General Public License as published by the Free Software
    00012 Foundation; either version 2 of the License, or (at your option) any later
    00013 version.
    00014
    00015 This program is distributed in the hope that it will be useful, but WITHOUT
    00016 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
    00017 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
    00018
    00019 You should have received a copy of the GNU Lesser General Public License along with
    00020 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
    00021 Place - Suite 330, Boston, MA 02111-1307, USA, or go to
    00022 http://www.gnu.org/copyleft/lesser.txt.
    00023 -----------------------------------------------------------------------------
    00024 */
    00025 #include "OgreStableHeaders.h"
    00026 // NOTE THAT THIS FILE IS BASED ON MATERIAL FROM:
    00027
    00028 // Magic Software, Inc.
    00029 // http://www.magic-software.com
    00030 // Copyright (c) 2000, All Rights Reserved
    00031 //
    00032 // Source code from Magic Software is supplied under the terms of a license
    00033 // agreement and may not be copied or disclosed except in accordance with the
    00034 // terms of that agreement.  The various license agreements may be found at
    00035 // the Magic Software web site.  This file is subject to the license
    00036 //
    00037 // FREE SOURCE CODE
    00038 // http://www.magic-software.com/License/free.pdf
    00039
    00040 #include "OgreQuaternion.h"
    00041
    00042 #include "OgreMath.h"
    00043 #include "OgreMatrix3.h"
    00044 #include "OgreVector3.h"
    00045
    00046 namespace Ogre {
    00047
    00048     const Real Quaternion::ms_fEpsilon = 1e-03;
    00049     const Quaternion Quaternion::ZERO(0.0,0.0,0.0,0.0);
    00050     const Quaternion Quaternion::IDENTITY(1.0,0.0,0.0,0.0);
    00051
    00052     //-----------------------------------------------------------------------
    00053     void Quaternion::FromRotationMatrix (const Matrix3& kRot)
    00054     {
    00055         // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    00056         // article "Quaternion Calculus and Fast Animation".
    00057
    00058         Real fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
    00059         Real fRoot;
    00060
    00061         if ( fTrace > 0.0 )
    00062         {
    00063             // |w| > 1/2, may as well choose w > 1/2
    00064             fRoot = Math::Sqrt(fTrace + 1.0);  // 2w
    00065             w = 0.5*fRoot;
    00066             fRoot = 0.5/fRoot;  // 1/(4w)
    00067             x = (kRot[2][1]-kRot[1][2])*fRoot;
    00068             y = (kRot[0][2]-kRot[2][0])*fRoot;
    00069             z = (kRot[1][0]-kRot[0][1])*fRoot;
    00070         }
    00071         else
    00072         {
    00073             // |w| <= 1/2
    00074             static size_t s_iNext[3] = { 1, 2, 0 };
    00075             size_t i = 0;
    00076             if ( kRot[1][1] > kRot[0][0] )
    00077                 i = 1;
    00078             if ( kRot[2][2] > kRot[i][i] )
    00079                 i = 2;
    00080             size_t j = s_iNext[i];
    00081             size_t k = s_iNext[j];
    00082
    00083             fRoot = Math::Sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0);
    00084             Real* apkQuat[3] = { &x, &y, &z };
    00085             *apkQuat[i] = 0.5*fRoot;
    00086             fRoot = 0.5/fRoot;
    00087             w = (kRot[k][j]-kRot[j][k])*fRoot;
    00088             *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
    00089             *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
    00090         }
    00091     }
    00092     //-----------------------------------------------------------------------
    00093     void Quaternion::ToRotationMatrix (Matrix3& kRot) const
    00094     {
    00095         Real fTx  = 2.0*x;
    00096         Real fTy  = 2.0*y;
    00097         Real fTz  = 2.0*z;
    00098         Real fTwx = fTx*w;
    00099         Real fTwy = fTy*w;
    00100         Real fTwz = fTz*w;
    00101         Real fTxx = fTx*x;
    00102         Real fTxy = fTy*x;
    00103         Real fTxz = fTz*x;
    00104         Real fTyy = fTy*y;
    00105         Real fTyz = fTz*y;
    00106         Real fTzz = fTz*z;
    00107
    00108         kRot[0][0] = 1.0-(fTyy+fTzz);
    00109         kRot[0][1] = fTxy-fTwz;
    00110         kRot[0][2] = fTxz+fTwy;
    00111         kRot[1][0] = fTxy+fTwz;
    00112         kRot[1][1] = 1.0-(fTxx+fTzz);
    00113         kRot[1][2] = fTyz-fTwx;
    00114         kRot[2][0] = fTxz-fTwy;
    00115         kRot[2][1] = fTyz+fTwx;
    00116         kRot[2][2] = 1.0-(fTxx+fTyy);
    00117     }
    00118     //-----------------------------------------------------------------------
    00119     void Quaternion::FromAngleAxis (const Radian& rfAngle,
    00120         const Vector3& rkAxis)
    00121     {
    00122         // assert:  axis[] is unit length
    00123         //
    00124         // The quaternion representing the rotation is
    00125         //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
    00126
    00127         Radian fHalfAngle ( 0.5*rfAngle );
    00128         Real fSin = Math::Sin(fHalfAngle);
    00129         w = Math::Cos(fHalfAngle);
    00130         x = fSin*rkAxis.x;
    00131         y = fSin*rkAxis.y;
    00132         z = fSin*rkAxis.z;
    00133     }
    00134     //-----------------------------------------------------------------------
    00135     void Quaternion::ToAngleAxis (Radian& rfAngle, Vector3& rkAxis) const
    00136     {
    00137         // The quaternion representing the rotation is
    00138         //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
    00139
    00140         Real fSqrLength = x*x+y*y+z*z;
    00141         if ( fSqrLength > 0.0 )
    00142         {
    00143             rfAngle = 2.0*Math::ACos(w);
    00144             Real fInvLength = Math::InvSqrt(fSqrLength);
    00145             rkAxis.x = x*fInvLength;
    00146             rkAxis.y = y*fInvLength;
    00147             rkAxis.z = z*fInvLength;
    00148         }
    00149         else
    00150         {
    00151             // angle is 0 (mod 2*pi), so any axis will do
    00152             rfAngle = Radian(0.0);
    00153             rkAxis.x = 1.0;
    00154             rkAxis.y = 0.0;
    00155             rkAxis.z = 0.0;
    00156         }
    00157     }
    00158     //-----------------------------------------------------------------------
    00159     void Quaternion::FromAxes (const Vector3* akAxis)
    00160     {
    00161         Matrix3 kRot;
    00162
    00163         for (size_t iCol = 0; iCol < 3; iCol++)
    00164         {
    00165             kRot[0][iCol] = akAxis[iCol].x;
    00166             kRot[1][iCol] = akAxis[iCol].y;
    00167             kRot[2][iCol] = akAxis[iCol].z;
    00168         }
    00169
    00170         FromRotationMatrix(kRot);
    00171     }
    00172     //-----------------------------------------------------------------------
    00173     void Quaternion::FromAxes (const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
    00174     {
    00175         Matrix3 kRot;
    00176
    00177         kRot[0][0] = xAxis.x;
    00178         kRot[1][0] = xAxis.y;
    00179         kRot[2][0] = xAxis.z;
    00180
    00181         kRot[0][1] = yAxis.x;
    00182         kRot[1][1] = yAxis.y;
    00183         kRot[2][1] = yAxis.z;
    00184
    00185         kRot[0][2] = zAxis.x;
    00186         kRot[1][2] = zAxis.y;
    00187         kRot[2][2] = zAxis.z;
    00188
    00189         FromRotationMatrix(kRot);
    00190
    00191     }
    00192     //-----------------------------------------------------------------------
    00193     void Quaternion::ToAxes (Vector3* akAxis) const
    00194     {
    00195         Matrix3 kRot;
    00196
    00197         ToRotationMatrix(kRot);
    00198
    00199         for (size_t iCol = 0; iCol < 3; iCol++)
    00200         {
    00201             akAxis[iCol].x = kRot[0][iCol];
    00202             akAxis[iCol].y = kRot[1][iCol];
    00203             akAxis[iCol].z = kRot[2][iCol];
    00204         }
    00205     }
    00206     //-----------------------------------------------------------------------
    00207     Vector3 Quaternion::xAxis(void) const
    00208     {
    00209         Real fTx  = 2.0*x;
    00210         Real fTy  = 2.0*y;
    00211         Real fTz  = 2.0*z;
    00212         Real fTwy = fTy*w;
    00213         Real fTwz = fTz*w;
    00214         Real fTxy = fTy*x;
    00215         Real fTxz = fTz*x;
    00216         Real fTyy = fTy*y;
    00217         Real fTzz = fTz*z;
    00218
    00219         return Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
    00220     }
    00221     //-----------------------------------------------------------------------
    00222     Vector3 Quaternion::yAxis(void) const
    00223     {
    00224         Real fTx  = 2.0*x;
    00225         Real fTy  = 2.0*y;
    00226         Real fTz  = 2.0*z;
    00227         Real fTwx = fTx*w;
    00228         Real fTwz = fTz*w;
    00229         Real fTxx = fTx*x;
    00230         Real fTxy = fTy*x;
    00231         Real fTyz = fTz*y;
    00232         Real fTzz = fTz*z;
    00233
    00234         return Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
    00235     }
    00236     //-----------------------------------------------------------------------
    00237     Vector3 Quaternion::zAxis(void) const
    00238     {
    00239         Real fTx  = 2.0*x;
    00240         Real fTy  = 2.0*y;
    00241         Real fTz  = 2.0*z;
    00242         Real fTwx = fTx*w;
    00243         Real fTwy = fTy*w;
    00244         Real fTxx = fTx*x;
    00245         Real fTxz = fTz*x;
    00246         Real fTyy = fTy*y;
    00247         Real fTyz = fTz*y;
    00248
    00249         return Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));
    00250     }
    00251     //-----------------------------------------------------------------------
    00252     void Quaternion::ToAxes (Vector3& xAxis, Vector3& yAxis, Vector3& zAxis) const
    00253     {
    00254         Matrix3 kRot;
    00255
    00256         ToRotationMatrix(kRot);
    00257
    00258         xAxis.x = kRot[0][0];
    00259         xAxis.y = kRot[1][0];
    00260         xAxis.z = kRot[2][0];
    00261
    00262         yAxis.x = kRot[0][1];
    00263         yAxis.y = kRot[1][1];
    00264         yAxis.z = kRot[2][1];
    00265
    00266         zAxis.x = kRot[0][2];
    00267         zAxis.y = kRot[1][2];
    00268         zAxis.z = kRot[2][2];
    00269     }
    00270
    00271     //-----------------------------------------------------------------------
    00272     Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
    00273     {
    00274         return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
    00275     }
    00276     //-----------------------------------------------------------------------
    00277     Quaternion Quaternion::operator- (const Quaternion& rkQ) const
    00278     {
    00279         return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
    00280     }
    00281     //-----------------------------------------------------------------------
    00282     Quaternion Quaternion::operator* (const Quaternion& rkQ) const
    00283     {
    00284         // NOTE:  Multiplication is not generally commutative, so in most
    00285         // cases p*q != q*p.
    00286
    00287         return Quaternion
    00288         (
    00289             w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
    00290             w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
    00291             w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
    00292             w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
    00293         );
    00294     }
    00295     //-----------------------------------------------------------------------
    00296     Quaternion Quaternion::operator* (Real fScalar) const
    00297     {
    00298         return Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
    00299     }
    00300     //-----------------------------------------------------------------------
    00301     Quaternion operator* (Real fScalar, const Quaternion& rkQ)
    00302     {
    00303         return Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
    00304             fScalar*rkQ.z);
    00305     }
    00306     //-----------------------------------------------------------------------
    00307     Quaternion Quaternion::operator- () const
    00308     {
    00309         return Quaternion(-w,-x,-y,-z);
    00310     }
    00311     //-----------------------------------------------------------------------
    00312     Real Quaternion::Dot (const Quaternion& rkQ) const
    00313     {
    00314         return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
    00315     }
    00316     //-----------------------------------------------------------------------
    00317     Real Quaternion::Norm () const
    00318     {
    00319         return w*w+x*x+y*y+z*z;
    00320     }
    00321     //-----------------------------------------------------------------------
    00322     Quaternion Quaternion::Inverse () const
    00323     {
    00324         Real fNorm = w*w+x*x+y*y+z*z;
    00325         if ( fNorm > 0.0 )
    00326         {
    00327             Real fInvNorm = 1.0/fNorm;
    00328             return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
    00329         }
    00330         else
    00331         {
    00332             // return an invalid result to flag the error
    00333             return ZERO;
    00334         }
    00335     }
    00336     //-----------------------------------------------------------------------
    00337     Quaternion Quaternion::UnitInverse () const
    00338     {
    00339         // assert:  'this' is unit length
    00340         return Quaternion(w,-x,-y,-z);
    00341     }
    00342     //-----------------------------------------------------------------------
    00343     Quaternion Quaternion::Exp () const
    00344     {
    00345         // If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    00346         // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
    00347         // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
    00348
    00349         Radian fAngle ( Math::Sqrt(x*x+y*y+z*z) );
    00350         Real fSin = Math::Sin(fAngle);
    00351
    00352         Quaternion kResult;
    00353         kResult.w = Math::Cos(fAngle);
    00354
    00355         if ( Math::Abs(fSin) >= ms_fEpsilon )
    00356         {
    00357             Real fCoeff = fSin/(fAngle.valueRadians());
    00358             kResult.x = fCoeff*x;
    00359             kResult.y = fCoeff*y;
    00360             kResult.z = fCoeff*z;
    00361         }
    00362         else
    00363         {
    00364             kResult.x = x;
    00365             kResult.y = y;
    00366             kResult.z = z;
    00367         }
    00368
    00369         return kResult;
    00370     }
    00371     //-----------------------------------------------------------------------
    00372     Quaternion Quaternion::Log () const
    00373     {
    00374         // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
    00375         // log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
    00376         // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
    00377
    00378         Quaternion kResult;
    00379         kResult.w = 0.0;
    00380
    00381         if ( Math::Abs(w) < 1.0 )
    00382         {
    00383             Radian fAngle ( Math::ACos(w) );
    00384             Real fSin = Math::Sin(fAngle);
    00385             if ( Math::Abs(fSin) >= ms_fEpsilon )
    00386             {
    00387                 Real fCoeff = fAngle.valueRadians()/fSin;
    00388                 kResult.x = fCoeff*x;
    00389                 kResult.y = fCoeff*y;
    00390                 kResult.z = fCoeff*z;
    00391                 return kResult;
    00392             }
    00393         }
    00394
    00395         kResult.x = x;
    00396         kResult.y = y;
    00397         kResult.z = z;
    00398
    00399         return kResult;
    00400     }
    00401     //-----------------------------------------------------------------------
    00402     Vector3 Quaternion::operator* (const Vector3& v) const
    00403     {
    00404         // nVidia SDK implementation
    00405         Vector3 uv, uuv;
    00406         Vector3 qvec(x, y, z);
    00407         uv = qvec.crossProduct(v);
    00408         uuv = qvec.crossProduct(uv);
    00409         uv *= (2.0f * w);
    00410         uuv *= 2.0f;
    00411
    00412         return v + uv + uuv;
    00413
    00414     }
    00415     //-----------------------------------------------------------------------
    00416     Quaternion Quaternion::Slerp (Real fT, const Quaternion& rkP,
    00417         const Quaternion& rkQ, bool shortestPath)
    00418     {
    00419         Real fCos = rkP.Dot(rkQ);
    00420         Radian fAngle ( Math::ACos(fCos) );
    00421
    00422         if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon )
    00423             return rkP;
    00424
    00425         Real fSin = Math::Sin(fAngle);
    00426         Real fInvSin = 1.0/fSin;
    00427         Real fCoeff0 = Math::Sin((1.0-fT)*fAngle)*fInvSin;
    00428         Real fCoeff1 = Math::Sin(fT*fAngle)*fInvSin;
    00429         // Do we need to invert rotation?
    00430         if (fCos < 0.0f && shortestPath)
    00431         {
    00432             fCoeff0 = -fCoeff0;
    00433             // taking the complement requires renormalisation
    00434             Quaternion t(fCoeff0*rkP + fCoeff1*rkQ);
    00435             t.normalise();
    00436             return t;
    00437         }
    00438         else
    00439         {
    00440             return fCoeff0*rkP + fCoeff1*rkQ;
    00441         }
    00442     }
    00443     //-----------------------------------------------------------------------
    00444     Quaternion Quaternion::SlerpExtraSpins (Real fT,
    00445         const Quaternion& rkP, const Quaternion& rkQ, int iExtraSpins)
    00446     {
    00447         Real fCos = rkP.Dot(rkQ);
    00448         Radian fAngle ( Math::ACos(fCos) );
    00449
    00450         if ( Math::Abs(fAngle.valueRadians()) < ms_fEpsilon )
    00451             return rkP;
    00452
    00453         Real fSin = Math::Sin(fAngle);
    00454         Radian fPhase ( Math::PI*iExtraSpins*fT );
    00455         Real fInvSin = 1.0/fSin;
    00456         Real fCoeff0 = Math::Sin((1.0-fT)*fAngle - fPhase)*fInvSin;
    00457         Real fCoeff1 = Math::Sin(fT*fAngle + fPhase)*fInvSin;
    00458         return fCoeff0*rkP + fCoeff1*rkQ;
    00459     }
    00460     //-----------------------------------------------------------------------
    00461     void Quaternion::Intermediate (const Quaternion& rkQ0,
    00462         const Quaternion& rkQ1, const Quaternion& rkQ2,
    00463         Quaternion& rkA, Quaternion& rkB)
    00464     {
    00465         // assert:  q0, q1, q2 are unit quaternions
    00466
    00467         Quaternion kQ0inv = rkQ0.UnitInverse();
    00468         Quaternion kQ1inv = rkQ1.UnitInverse();
    00469         Quaternion rkP0 = kQ0inv*rkQ1;
    00470         Quaternion rkP1 = kQ1inv*rkQ2;
    00471         Quaternion kArg = 0.25*(rkP0.Log()-rkP1.Log());
    00472         Quaternion kMinusArg = -kArg;
    00473
    00474         rkA = rkQ1*kArg.Exp();
    00475         rkB = rkQ1*kMinusArg.Exp();
    00476     }
    00477     //-----------------------------------------------------------------------
    00478     Quaternion Quaternion::Squad (Real fT,
    00479         const Quaternion& rkP, const Quaternion& rkA,
    00480         const Quaternion& rkB, const Quaternion& rkQ, bool shortestPath)
    00481     {
    00482         Real fSlerpT = 2.0*fT*(1.0-fT);
    00483         Quaternion kSlerpP = Slerp(fT, rkP, rkQ, shortestPath);
    00484         Quaternion kSlerpQ = Slerp(fT, rkA, rkB);
    00485         return Slerp(fSlerpT, kSlerpP ,kSlerpQ);
    00486     }
    00487     //-----------------------------------------------------------------------
    00488     Real Quaternion::normalise(void)
    00489     {
    00490         Real len = Norm();
    00491         Real factor = 1.0f / Math::Sqrt(len);
    00492         *this = *this * factor;
    00493         return len;
    00494     }
    00495     //-----------------------------------------------------------------------
    00496     Radian Quaternion::getRoll(void) const
    00497     {
    00498         return Radian(Math::ATan2(2*(y*z + w*x), w*w - x*x - y*y + z*z));
    00499     }
    00500     //-----------------------------------------------------------------------
    00501     Radian Quaternion::getPitch(void) const
    00502     {
    00503         return Radian(Math::ASin(-2*(x*z - w*y)));
    00504     }
    00505     //-----------------------------------------------------------------------
    00506     Radian Quaternion::getYaw(void) const
    00507     {
    00508         return Radian(Math::ATan2(2*(x*y + w*z), w*w + x*x - y*y - z*z));
    00509     }
    00510     //-----------------------------------------------------------------------
    00511     Quaternion Quaternion::nlerp(Real fT, const Quaternion& rkP,
    00512         const Quaternion& rkQ, bool shortestPath)
    00513     {
    00514         Quaternion result = rkP + fT * (rkQ - rkP);
    00515         result.normalise();
    00516         return result;
    00517     }
    00518 }

Copyright © 2002-2003 by The OGRE Team
Last modified Sun Nov 28 19:48:41 2004