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 Real& 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 Real 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 (Real& 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 = 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) 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) 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) 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 Real 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; 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 Real fAngle = Math::ACos(w); 00384 Real fSin = Math::Sin(fAngle); 00385 if ( Math::Abs(fSin) >= ms_fEpsilon ) 00386 { 00387 Real fCoeff = fAngle/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 Real fAngle = Math::ACos(fCos); 00421 00422 if ( Math::Abs(fAngle) < 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 Real fAngle = Math::ACos(fCos); 00449 00450 if ( Math::Abs(fAngle) < ms_fEpsilon ) 00451 return rkP; 00452 00453 Real fSin = Math::Sin(fAngle); 00454 Real 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 }
Copyright © 2002-2003 by The OGRE Team
Last modified Fri May 14 23:22:37 2004