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