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

OgreMatrix3.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 #include "OgreMatrix3.h"
00027 
00028 #include "OgreMath.h"
00029 
00030 // Adapted from Matrix math by Wild Magic http://www.magic-software.com
00031 
00032 namespace Ogre
00033 {
00034     const Real Matrix3::EPSILON = 1e-06;
00035     const Matrix3 Matrix3::ZERO(0,0,0,0,0,0,0,0,0);
00036     const Matrix3 Matrix3::IDENTITY(1,0,0,0,1,0,0,0,1);
00037     const Real Matrix3::ms_fSvdEpsilon = 1e-04;
00038     const unsigned int Matrix3::ms_iSvdMaxIterations = 32;
00039 
00040     //-----------------------------------------------------------------------
00041     Vector3 Matrix3::GetColumn (size_t iCol) const
00042     {
00043         assert( 0 <= iCol && iCol < 3 );
00044         return Vector3(m[0][iCol],m[1][iCol],
00045             m[2][iCol]);
00046     }
00047     //-----------------------------------------------------------------------
00048     void Matrix3::SetColumn(size_t iCol, const Vector3& vec)
00049     {
00050         assert( 0 <= iCol && iCol < 3 );
00051         m[0][iCol] = vec.x;
00052         m[1][iCol] = vec.y;
00053         m[2][iCol] = vec.z;
00054 
00055     }
00056     //-----------------------------------------------------------------------
00057     void Matrix3::FromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
00058     {
00059         SetColumn(0,xAxis);
00060         SetColumn(1,yAxis);
00061         SetColumn(2,zAxis);
00062 
00063     }
00064 
00065     //-----------------------------------------------------------------------
00066     bool Matrix3::operator== (const Matrix3& rkMatrix) const
00067     {
00068         for (size_t iRow = 0; iRow < 3; iRow++)
00069         {
00070             for (size_t iCol = 0; iCol < 3; iCol++)
00071             {
00072                 if ( m[iRow][iCol] != rkMatrix.m[iRow][iCol] )
00073                     return false;
00074             }
00075         }
00076 
00077         return true;
00078     }
00079     //-----------------------------------------------------------------------
00080     Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const
00081     {
00082         Matrix3 kSum;
00083         for (size_t iRow = 0; iRow < 3; iRow++)
00084         {
00085             for (size_t iCol = 0; iCol < 3; iCol++)
00086             {
00087                 kSum.m[iRow][iCol] = m[iRow][iCol] +
00088                     rkMatrix.m[iRow][iCol];
00089             }
00090         }
00091         return kSum;
00092     }
00093     //-----------------------------------------------------------------------
00094     Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const
00095     {
00096         Matrix3 kDiff;
00097         for (size_t iRow = 0; iRow < 3; iRow++)
00098         {
00099             for (size_t iCol = 0; iCol < 3; iCol++)
00100             {
00101                 kDiff.m[iRow][iCol] = m[iRow][iCol] -
00102                     rkMatrix.m[iRow][iCol];
00103             }
00104         }
00105         return kDiff;
00106     }
00107     //-----------------------------------------------------------------------
00108     Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const
00109     {
00110         Matrix3 kProd;
00111         for (size_t iRow = 0; iRow < 3; iRow++)
00112         {
00113             for (size_t iCol = 0; iCol < 3; iCol++)
00114             {
00115                 kProd.m[iRow][iCol] =
00116                     m[iRow][0]*rkMatrix.m[0][iCol] +
00117                     m[iRow][1]*rkMatrix.m[1][iCol] +
00118                     m[iRow][2]*rkMatrix.m[2][iCol];
00119             }
00120         }
00121         return kProd;
00122     }
00123     //-----------------------------------------------------------------------
00124     Vector3 Matrix3::operator* (const Vector3& rkPoint) const
00125     {
00126         Vector3 kProd;
00127         for (size_t iRow = 0; iRow < 3; iRow++)
00128         {
00129             kProd[iRow] =
00130                 m[iRow][0]*rkPoint[0] +
00131                 m[iRow][1]*rkPoint[1] +
00132                 m[iRow][2]*rkPoint[2];
00133         }
00134         return kProd;
00135     }
00136     //-----------------------------------------------------------------------
00137     Vector3 operator* (const Vector3& rkPoint, const Matrix3& rkMatrix)
00138     {
00139         Vector3 kProd;
00140         for (size_t iRow = 0; iRow < 3; iRow++)
00141         {
00142             kProd[iRow] =
00143                 rkPoint[0]*rkMatrix.m[0][iRow] +
00144                 rkPoint[1]*rkMatrix.m[1][iRow] +
00145                 rkPoint[2]*rkMatrix.m[2][iRow];
00146         }
00147         return kProd;
00148     }
00149     //-----------------------------------------------------------------------
00150     Matrix3 Matrix3::operator- () const
00151     {
00152         Matrix3 kNeg;
00153         for (size_t iRow = 0; iRow < 3; iRow++)
00154         {
00155             for (size_t iCol = 0; iCol < 3; iCol++)
00156                 kNeg[iRow][iCol] = -m[iRow][iCol];
00157         }
00158         return kNeg;
00159     }
00160     //-----------------------------------------------------------------------
00161     Matrix3 Matrix3::operator* (Real fScalar) const
00162     {
00163         Matrix3 kProd;
00164         for (size_t iRow = 0; iRow < 3; iRow++)
00165         {
00166             for (size_t iCol = 0; iCol < 3; iCol++)
00167                 kProd[iRow][iCol] = fScalar*m[iRow][iCol];
00168         }
00169         return kProd;
00170     }
00171     //-----------------------------------------------------------------------
00172     Matrix3 operator* (Real fScalar, const Matrix3& rkMatrix)
00173     {
00174         Matrix3 kProd;
00175         for (size_t iRow = 0; iRow < 3; iRow++)
00176         {
00177             for (size_t iCol = 0; iCol < 3; iCol++)
00178                 kProd[iRow][iCol] = fScalar*rkMatrix.m[iRow][iCol];
00179         }
00180         return kProd;
00181     }
00182     //-----------------------------------------------------------------------
00183     Matrix3 Matrix3::Transpose () const
00184     {
00185         Matrix3 kTranspose;
00186         for (size_t iRow = 0; iRow < 3; iRow++)
00187         {
00188             for (size_t iCol = 0; iCol < 3; iCol++)
00189                 kTranspose[iRow][iCol] = m[iCol][iRow];
00190         }
00191         return kTranspose;
00192     }
00193     //-----------------------------------------------------------------------
00194     bool Matrix3::Inverse (Matrix3& rkInverse, Real fTolerance) const
00195     {
00196         // Invert a 3x3 using cofactors.  This is about 8 times faster than
00197         // the Numerical Recipes code which uses Gaussian elimination.
00198 
00199         rkInverse[0][0] = m[1][1]*m[2][2] -
00200             m[1][2]*m[2][1];
00201         rkInverse[0][1] = m[0][2]*m[2][1] -
00202             m[0][1]*m[2][2];
00203         rkInverse[0][2] = m[0][1]*m[1][2] -
00204             m[0][2]*m[1][1];
00205         rkInverse[1][0] = m[1][2]*m[2][0] -
00206             m[1][0]*m[2][2];
00207         rkInverse[1][1] = m[0][0]*m[2][2] -
00208             m[0][2]*m[2][0];
00209         rkInverse[1][2] = m[0][2]*m[1][0] -
00210             m[0][0]*m[1][2];
00211         rkInverse[2][0] = m[1][0]*m[2][1] -
00212             m[1][1]*m[2][0];
00213         rkInverse[2][1] = m[0][1]*m[2][0] -
00214             m[0][0]*m[2][1];
00215         rkInverse[2][2] = m[0][0]*m[1][1] -
00216             m[0][1]*m[1][0];
00217 
00218         Real fDet =
00219             m[0][0]*rkInverse[0][0] +
00220             m[0][1]*rkInverse[1][0]+
00221             m[0][2]*rkInverse[2][0];
00222 
00223         if ( Math::Abs(fDet) <= fTolerance )
00224             return false;
00225 
00226         Real fInvDet = 1.0/fDet;
00227         for (size_t iRow = 0; iRow < 3; iRow++)
00228         {
00229             for (size_t iCol = 0; iCol < 3; iCol++)
00230                 rkInverse[iRow][iCol] *= fInvDet;
00231         }
00232 
00233         return true;
00234     }
00235     //-----------------------------------------------------------------------
00236     Matrix3 Matrix3::Inverse (Real fTolerance) const
00237     {
00238         Matrix3 kInverse = Matrix3::ZERO;
00239         Inverse(kInverse,fTolerance);
00240         return kInverse;
00241     }
00242     //-----------------------------------------------------------------------
00243     Real Matrix3::Determinant () const
00244     {
00245         Real fCofactor00 = m[1][1]*m[2][2] -
00246             m[1][2]*m[2][1];
00247         Real fCofactor10 = m[1][2]*m[2][0] -
00248             m[1][0]*m[2][2];
00249         Real fCofactor20 = m[1][0]*m[2][1] -
00250             m[1][1]*m[2][0];
00251 
00252         Real fDet =
00253             m[0][0]*fCofactor00 +
00254             m[0][1]*fCofactor10 +
00255             m[0][2]*fCofactor20;
00256 
00257         return fDet;
00258     }
00259     //-----------------------------------------------------------------------
00260     void Matrix3::Bidiagonalize (Matrix3& kA, Matrix3& kL,
00261         Matrix3& kR)
00262     {
00263         Real afV[3], afW[3];
00264         Real fLength, fSign, fT1, fInvT1, fT2;
00265         bool bIdentity;
00266 
00267         // map first column to (*,0,0)
00268         fLength = Math::Sqrt(kA[0][0]*kA[0][0] + kA[1][0]*kA[1][0] +
00269             kA[2][0]*kA[2][0]);
00270         if ( fLength > 0.0 )
00271         {
00272             fSign = (kA[0][0] > 0.0 ? 1.0 : -1.0);
00273             fT1 = kA[0][0] + fSign*fLength;
00274             fInvT1 = 1.0/fT1;
00275             afV[1] = kA[1][0]*fInvT1;
00276             afV[2] = kA[2][0]*fInvT1;
00277 
00278             fT2 = -2.0/(1.0+afV[1]*afV[1]+afV[2]*afV[2]);
00279             afW[0] = fT2*(kA[0][0]+kA[1][0]*afV[1]+kA[2][0]*afV[2]);
00280             afW[1] = fT2*(kA[0][1]+kA[1][1]*afV[1]+kA[2][1]*afV[2]);
00281             afW[2] = fT2*(kA[0][2]+kA[1][2]*afV[1]+kA[2][2]*afV[2]);
00282             kA[0][0] += afW[0];
00283             kA[0][1] += afW[1];
00284             kA[0][2] += afW[2];
00285             kA[1][1] += afV[1]*afW[1];
00286             kA[1][2] += afV[1]*afW[2];
00287             kA[2][1] += afV[2]*afW[1];
00288             kA[2][2] += afV[2]*afW[2];
00289 
00290             kL[0][0] = 1.0+fT2;
00291             kL[0][1] = kL[1][0] = fT2*afV[1];
00292             kL[0][2] = kL[2][0] = fT2*afV[2];
00293             kL[1][1] = 1.0+fT2*afV[1]*afV[1];
00294             kL[1][2] = kL[2][1] = fT2*afV[1]*afV[2];
00295             kL[2][2] = 1.0+fT2*afV[2]*afV[2];
00296             bIdentity = false;
00297         }
00298         else
00299         {
00300             kL = Matrix3::IDENTITY;
00301             bIdentity = true;
00302         }
00303 
00304         // map first row to (*,*,0)
00305         fLength = Math::Sqrt(kA[0][1]*kA[0][1]+kA[0][2]*kA[0][2]);
00306         if ( fLength > 0.0 )
00307         {
00308             fSign = (kA[0][1] > 0.0 ? 1.0 : -1.0);
00309             fT1 = kA[0][1] + fSign*fLength;
00310             afV[2] = kA[0][2]/fT1;
00311 
00312             fT2 = -2.0/(1.0+afV[2]*afV[2]);
00313             afW[0] = fT2*(kA[0][1]+kA[0][2]*afV[2]);
00314             afW[1] = fT2*(kA[1][1]+kA[1][2]*afV[2]);
00315             afW[2] = fT2*(kA[2][1]+kA[2][2]*afV[2]);
00316             kA[0][1] += afW[0];
00317             kA[1][1] += afW[1];
00318             kA[1][2] += afW[1]*afV[2];
00319             kA[2][1] += afW[2];
00320             kA[2][2] += afW[2]*afV[2];
00321 
00322             kR[0][0] = 1.0;
00323             kR[0][1] = kR[1][0] = 0.0;
00324             kR[0][2] = kR[2][0] = 0.0;
00325             kR[1][1] = 1.0+fT2;
00326             kR[1][2] = kR[2][1] = fT2*afV[2];
00327             kR[2][2] = 1.0+fT2*afV[2]*afV[2];
00328         }
00329         else
00330         {
00331             kR = Matrix3::IDENTITY;
00332         }
00333 
00334         // map second column to (*,*,0)
00335         fLength = Math::Sqrt(kA[1][1]*kA[1][1]+kA[2][1]*kA[2][1]);
00336         if ( fLength > 0.0 )
00337         {
00338             fSign = (kA[1][1] > 0.0 ? 1.0 : -1.0);
00339             fT1 = kA[1][1] + fSign*fLength;
00340             afV[2] = kA[2][1]/fT1;
00341 
00342             fT2 = -2.0/(1.0+afV[2]*afV[2]);
00343             afW[1] = fT2*(kA[1][1]+kA[2][1]*afV[2]);
00344             afW[2] = fT2*(kA[1][2]+kA[2][2]*afV[2]);
00345             kA[1][1] += afW[1];
00346             kA[1][2] += afW[2];
00347             kA[2][2] += afV[2]*afW[2];
00348 
00349             Real fA = 1.0+fT2;
00350             Real fB = fT2*afV[2];
00351             Real fC = 1.0+fB*afV[2];
00352 
00353             if ( bIdentity )
00354             {
00355                 kL[0][0] = 1.0;
00356                 kL[0][1] = kL[1][0] = 0.0;
00357                 kL[0][2] = kL[2][0] = 0.0;
00358                 kL[1][1] = fA;
00359                 kL[1][2] = kL[2][1] = fB;
00360                 kL[2][2] = fC;
00361             }
00362             else
00363             {
00364                 for (int iRow = 0; iRow < 3; iRow++)
00365                 {
00366                     Real fTmp0 = kL[iRow][1];
00367                     Real fTmp1 = kL[iRow][2];
00368                     kL[iRow][1] = fA*fTmp0+fB*fTmp1;
00369                     kL[iRow][2] = fB*fTmp0+fC*fTmp1;
00370                 }
00371             }
00372         }
00373     }
00374     //-----------------------------------------------------------------------
00375     void Matrix3::GolubKahanStep (Matrix3& kA, Matrix3& kL,
00376         Matrix3& kR)
00377     {
00378         Real fT11 = kA[0][1]*kA[0][1]+kA[1][1]*kA[1][1];
00379         Real fT22 = kA[1][2]*kA[1][2]+kA[2][2]*kA[2][2];
00380         Real fT12 = kA[1][1]*kA[1][2];
00381         Real fTrace = fT11+fT22;
00382         Real fDiff = fT11-fT22;
00383         Real fDiscr = Math::Sqrt(fDiff*fDiff+4.0*fT12*fT12);
00384         Real fRoot1 = 0.5*(fTrace+fDiscr);
00385         Real fRoot2 = 0.5*(fTrace-fDiscr);
00386 
00387         // adjust right
00388         Real fY = kA[0][0] - (Math::Abs(fRoot1-fT22) <=
00389             Math::Abs(fRoot2-fT22) ? fRoot1 : fRoot2);
00390         Real fZ = kA[0][1];
00391         Real fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
00392         Real fSin = fZ*fInvLength;
00393         Real fCos = -fY*fInvLength;
00394 
00395         Real fTmp0 = kA[0][0];
00396         Real fTmp1 = kA[0][1];
00397         kA[0][0] = fCos*fTmp0-fSin*fTmp1;
00398         kA[0][1] = fSin*fTmp0+fCos*fTmp1;
00399         kA[1][0] = -fSin*kA[1][1];
00400         kA[1][1] *= fCos;
00401 
00402         size_t iRow;
00403         for (iRow = 0; iRow < 3; iRow++)
00404         {
00405             fTmp0 = kR[0][iRow];
00406             fTmp1 = kR[1][iRow];
00407             kR[0][iRow] = fCos*fTmp0-fSin*fTmp1;
00408             kR[1][iRow] = fSin*fTmp0+fCos*fTmp1;
00409         }
00410 
00411         // adjust left
00412         fY = kA[0][0];
00413         fZ = kA[1][0];
00414         fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
00415         fSin = fZ*fInvLength;
00416         fCos = -fY*fInvLength;
00417 
00418         kA[0][0] = fCos*kA[0][0]-fSin*kA[1][0];
00419         fTmp0 = kA[0][1];
00420         fTmp1 = kA[1][1];
00421         kA[0][1] = fCos*fTmp0-fSin*fTmp1;
00422         kA[1][1] = fSin*fTmp0+fCos*fTmp1;
00423         kA[0][2] = -fSin*kA[1][2];
00424         kA[1][2] *= fCos;
00425 
00426         size_t iCol;
00427         for (iCol = 0; iCol < 3; iCol++)
00428         {
00429             fTmp0 = kL[iCol][0];
00430             fTmp1 = kL[iCol][1];
00431             kL[iCol][0] = fCos*fTmp0-fSin*fTmp1;
00432             kL[iCol][1] = fSin*fTmp0+fCos*fTmp1;
00433         }
00434 
00435         // adjust right
00436         fY = kA[0][1];
00437         fZ = kA[0][2];
00438         fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
00439         fSin = fZ*fInvLength;
00440         fCos = -fY*fInvLength;
00441 
00442         kA[0][1] = fCos*kA[0][1]-fSin*kA[0][2];
00443         fTmp0 = kA[1][1];
00444         fTmp1 = kA[1][2];
00445         kA[1][1] = fCos*fTmp0-fSin*fTmp1;
00446         kA[1][2] = fSin*fTmp0+fCos*fTmp1;
00447         kA[2][1] = -fSin*kA[2][2];
00448         kA[2][2] *= fCos;
00449 
00450         for (iRow = 0; iRow < 3; iRow++)
00451         {
00452             fTmp0 = kR[1][iRow];
00453             fTmp1 = kR[2][iRow];
00454             kR[1][iRow] = fCos*fTmp0-fSin*fTmp1;
00455             kR[2][iRow] = fSin*fTmp0+fCos*fTmp1;
00456         }
00457 
00458         // adjust left
00459         fY = kA[1][1];
00460         fZ = kA[2][1];
00461         fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
00462         fSin = fZ*fInvLength;
00463         fCos = -fY*fInvLength;
00464 
00465         kA[1][1] = fCos*kA[1][1]-fSin*kA[2][1];
00466         fTmp0 = kA[1][2];
00467         fTmp1 = kA[2][2];
00468         kA[1][2] = fCos*fTmp0-fSin*fTmp1;
00469         kA[2][2] = fSin*fTmp0+fCos*fTmp1;
00470 
00471         for (iCol = 0; iCol < 3; iCol++)
00472         {
00473             fTmp0 = kL[iCol][1];
00474             fTmp1 = kL[iCol][2];
00475             kL[iCol][1] = fCos*fTmp0-fSin*fTmp1;
00476             kL[iCol][2] = fSin*fTmp0+fCos*fTmp1;
00477         }
00478     }
00479     //-----------------------------------------------------------------------
00480     void Matrix3::SingularValueDecomposition (Matrix3& kL, Vector3& kS,
00481         Matrix3& kR) const
00482     {
00483         // temas: currently unused
00484         //const int iMax = 16;
00485         size_t iRow, iCol;
00486 
00487         Matrix3 kA = *this;
00488         Bidiagonalize(kA,kL,kR);
00489 
00490         for (unsigned int i = 0; i < ms_iSvdMaxIterations; i++)
00491         {
00492             Real fTmp, fTmp0, fTmp1;
00493             Real fSin0, fCos0, fTan0;
00494             Real fSin1, fCos1, fTan1;
00495 
00496             bool bTest1 = (Math::Abs(kA[0][1]) <=
00497                 ms_fSvdEpsilon*(Math::Abs(kA[0][0])+Math::Abs(kA[1][1])));
00498             bool bTest2 = (Math::Abs(kA[1][2]) <=
00499                 ms_fSvdEpsilon*(Math::Abs(kA[1][1])+Math::Abs(kA[2][2])));
00500             if ( bTest1 )
00501             {
00502                 if ( bTest2 )
00503                 {
00504                     kS[0] = kA[0][0];
00505                     kS[1] = kA[1][1];
00506                     kS[2] = kA[2][2];
00507                     break;
00508                 }
00509                 else
00510                 {
00511                     // 2x2 closed form factorization
00512                     fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] +
00513                         kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]);
00514                     fTan0 = 0.5*(fTmp+Math::Sqrt(fTmp*fTmp + 4.0));
00515                     fCos0 = Math::InvSqrt(1.0+fTan0*fTan0);
00516                     fSin0 = fTan0*fCos0;
00517 
00518                     for (iCol = 0; iCol < 3; iCol++)
00519                     {
00520                         fTmp0 = kL[iCol][1];
00521                         fTmp1 = kL[iCol][2];
00522                         kL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1;
00523                         kL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1;
00524                     }
00525 
00526                     fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1];
00527                     fCos1 = Math::InvSqrt(1.0+fTan1*fTan1);
00528                     fSin1 = -fTan1*fCos1;
00529 
00530                     for (iRow = 0; iRow < 3; iRow++)
00531                     {
00532                         fTmp0 = kR[1][iRow];
00533                         fTmp1 = kR[2][iRow];
00534                         kR[1][iRow] = fCos1*fTmp0-fSin1*fTmp1;
00535                         kR[2][iRow] = fSin1*fTmp0+fCos1*fTmp1;
00536                     }
00537 
00538                     kS[0] = kA[0][0];
00539                     kS[1] = fCos0*fCos1*kA[1][1] -
00540                         fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]);
00541                     kS[2] = fSin0*fSin1*kA[1][1] +
00542                         fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]);
00543                     break;
00544                 }
00545             }
00546             else
00547             {
00548                 if ( bTest2 )
00549                 {
00550                     // 2x2 closed form factorization
00551                     fTmp = (kA[0][0]*kA[0][0] + kA[1][1]*kA[1][1] -
00552                         kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]);
00553                     fTan0 = 0.5*(-fTmp+Math::Sqrt(fTmp*fTmp + 4.0));
00554                     fCos0 = Math::InvSqrt(1.0+fTan0*fTan0);
00555                     fSin0 = fTan0*fCos0;
00556 
00557                     for (iCol = 0; iCol < 3; iCol++)
00558                     {
00559                         fTmp0 = kL[iCol][0];
00560                         fTmp1 = kL[iCol][1];
00561                         kL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1;
00562                         kL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1;
00563                     }
00564 
00565                     fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0];
00566                     fCos1 = Math::InvSqrt(1.0+fTan1*fTan1);
00567                     fSin1 = -fTan1*fCos1;
00568 
00569                     for (iRow = 0; iRow < 3; iRow++)
00570                     {
00571                         fTmp0 = kR[0][iRow];
00572                         fTmp1 = kR[1][iRow];
00573                         kR[0][iRow] = fCos1*fTmp0-fSin1*fTmp1;
00574                         kR[1][iRow] = fSin1*fTmp0+fCos1*fTmp1;
00575                     }
00576 
00577                     kS[0] = fCos0*fCos1*kA[0][0] -
00578                         fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]);
00579                     kS[1] = fSin0*fSin1*kA[0][0] +
00580                         fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]);
00581                     kS[2] = kA[2][2];
00582                     break;
00583                 }
00584                 else
00585                 {
00586                     GolubKahanStep(kA,kL,kR);
00587                 }
00588             }
00589         }
00590 
00591         // positize diagonal
00592         for (iRow = 0; iRow < 3; iRow++)
00593         {
00594             if ( kS[iRow] < 0.0 )
00595             {
00596                 kS[iRow] = -kS[iRow];
00597                 for (iCol = 0; iCol < 3; iCol++)
00598                     kR[iRow][iCol] = -kR[iRow][iCol];
00599             }
00600         }
00601     }
00602     //-----------------------------------------------------------------------
00603     void Matrix3::SingularValueComposition (const Matrix3& kL,
00604         const Vector3& kS, const Matrix3& kR)
00605     {
00606         size_t iRow, iCol;
00607         Matrix3 kTmp;
00608 
00609         // product S*R
00610         for (iRow = 0; iRow < 3; iRow++)
00611         {
00612             for (iCol = 0; iCol < 3; iCol++)
00613                 kTmp[iRow][iCol] = kS[iRow]*kR[iRow][iCol];
00614         }
00615 
00616         // product L*S*R
00617         for (iRow = 0; iRow < 3; iRow++)
00618         {
00619             for (iCol = 0; iCol < 3; iCol++)
00620             {
00621                 m[iRow][iCol] = 0.0;
00622                 for (int iMid = 0; iMid < 3; iMid++)
00623                     m[iRow][iCol] += kL[iRow][iMid]*kTmp[iMid][iCol];
00624             }
00625         }
00626     }
00627     //-----------------------------------------------------------------------
00628     void Matrix3::Orthonormalize ()
00629     {
00630         // Algorithm uses Gram-Schmidt orthogonalization.  If 'this' matrix is
00631         // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
00632         //
00633         //   q0 = m0/|m0|
00634         //   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
00635         //   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
00636         //
00637         // where |V| indicates length of vector V and A*B indicates dot
00638         // product of vectors A and B.
00639 
00640         // compute q0
00641         Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0]
00642             + m[1][0]*m[1][0] +
00643             m[2][0]*m[2][0]);
00644 
00645         m[0][0] *= fInvLength;
00646         m[1][0] *= fInvLength;
00647         m[2][0] *= fInvLength;
00648 
00649         // compute q1
00650         Real fDot0 =
00651             m[0][0]*m[0][1] +
00652             m[1][0]*m[1][1] +
00653             m[2][0]*m[2][1];
00654 
00655         m[0][1] -= fDot0*m[0][0];
00656         m[1][1] -= fDot0*m[1][0];
00657         m[2][1] -= fDot0*m[2][0];
00658 
00659         fInvLength = Math::InvSqrt(m[0][1]*m[0][1] +
00660             m[1][1]*m[1][1] +
00661             m[2][1]*m[2][1]);
00662 
00663         m[0][1] *= fInvLength;
00664         m[1][1] *= fInvLength;
00665         m[2][1] *= fInvLength;
00666 
00667         // compute q2
00668         Real fDot1 =
00669             m[0][1]*m[0][2] +
00670             m[1][1]*m[1][2] +
00671             m[2][1]*m[2][2];
00672 
00673         fDot0 =
00674             m[0][0]*m[0][2] +
00675             m[1][0]*m[1][2] +
00676             m[2][0]*m[2][2];
00677 
00678         m[0][2] -= fDot0*m[0][0] + fDot1*m[0][1];
00679         m[1][2] -= fDot0*m[1][0] + fDot1*m[1][1];
00680         m[2][2] -= fDot0*m[2][0] + fDot1*m[2][1];
00681 
00682         fInvLength = Math::InvSqrt(m[0][2]*m[0][2] +
00683             m[1][2]*m[1][2] +
00684             m[2][2]*m[2][2]);
00685 
00686         m[0][2] *= fInvLength;
00687         m[1][2] *= fInvLength;
00688         m[2][2] *= fInvLength;
00689     }
00690     //-----------------------------------------------------------------------
00691     void Matrix3::QDUDecomposition (Matrix3& kQ,
00692         Vector3& kD, Vector3& kU) const
00693     {
00694         // Factor M = QR = QDU where Q is orthogonal, D is diagonal,
00695         // and U is upper triangular with ones on its diagonal.  Algorithm uses
00696         // Gram-Schmidt orthogonalization (the QR algorithm).
00697         //
00698         // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
00699         //
00700         //   q0 = m0/|m0|
00701         //   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
00702         //   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
00703         //
00704         // where |V| indicates length of vector V and A*B indicates dot
00705         // product of vectors A and B.  The matrix R has entries
00706         //
00707         //   r00 = q0*m0  r01 = q0*m1  r02 = q0*m2
00708         //   r10 = 0      r11 = q1*m1  r12 = q1*m2
00709         //   r20 = 0      r21 = 0      r22 = q2*m2
00710         //
00711         // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
00712         // u02 = r02/r00, and u12 = r12/r11.
00713 
00714         // Q = rotation
00715         // D = scaling
00716         // U = shear
00717 
00718         // D stores the three diagonal entries r00, r11, r22
00719         // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12
00720 
00721         // build orthogonal matrix Q
00722         Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0]
00723             + m[1][0]*m[1][0] +
00724             m[2][0]*m[2][0]);
00725         kQ[0][0] = m[0][0]*fInvLength;
00726         kQ[1][0] = m[1][0]*fInvLength;
00727         kQ[2][0] = m[2][0]*fInvLength;
00728 
00729         Real fDot = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] +
00730             kQ[2][0]*m[2][1];
00731         kQ[0][1] = m[0][1]-fDot*kQ[0][0];
00732         kQ[1][1] = m[1][1]-fDot*kQ[1][0];
00733         kQ[2][1] = m[2][1]-fDot*kQ[2][0];
00734         fInvLength = Math::InvSqrt(kQ[0][1]*kQ[0][1] + kQ[1][1]*kQ[1][1] +
00735             kQ[2][1]*kQ[2][1]);
00736         kQ[0][1] *= fInvLength;
00737         kQ[1][1] *= fInvLength;
00738         kQ[2][1] *= fInvLength;
00739 
00740         fDot = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] +
00741             kQ[2][0]*m[2][2];
00742         kQ[0][2] = m[0][2]-fDot*kQ[0][0];
00743         kQ[1][2] = m[1][2]-fDot*kQ[1][0];
00744         kQ[2][2] = m[2][2]-fDot*kQ[2][0];
00745         fDot = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] +
00746             kQ[2][1]*m[2][2];
00747         kQ[0][2] -= fDot*kQ[0][1];
00748         kQ[1][2] -= fDot*kQ[1][1];
00749         kQ[2][2] -= fDot*kQ[2][1];
00750         fInvLength = Math::InvSqrt(kQ[0][2]*kQ[0][2] + kQ[1][2]*kQ[1][2] +
00751             kQ[2][2]*kQ[2][2]);
00752         kQ[0][2] *= fInvLength;
00753         kQ[1][2] *= fInvLength;
00754         kQ[2][2] *= fInvLength;
00755 
00756         // guarantee that orthogonal matrix has determinant 1 (no reflections)
00757         Real fDet = kQ[0][0]*kQ[1][1]*kQ[2][2] + kQ[0][1]*kQ[1][2]*kQ[2][0] +
00758             kQ[0][2]*kQ[1][0]*kQ[2][1] - kQ[0][2]*kQ[1][1]*kQ[2][0] -
00759             kQ[0][1]*kQ[1][0]*kQ[2][2] - kQ[0][0]*kQ[1][2]*kQ[2][1];
00760 
00761         if ( fDet < 0.0 )
00762         {
00763             for (size_t iRow = 0; iRow < 3; iRow++)
00764                 for (size_t iCol = 0; iCol < 3; iCol++)
00765                     kQ[iRow][iCol] = -kQ[iRow][iCol];
00766         }
00767 
00768         // build "right" matrix R
00769         Matrix3 kR;
00770         kR[0][0] = kQ[0][0]*m[0][0] + kQ[1][0]*m[1][0] +
00771             kQ[2][0]*m[2][0];
00772         kR[0][1] = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] +
00773             kQ[2][0]*m[2][1];
00774         kR[1][1] = kQ[0][1]*m[0][1] + kQ[1][1]*m[1][1] +
00775             kQ[2][1]*m[2][1];
00776         kR[0][2] = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] +
00777             kQ[2][0]*m[2][2];
00778         kR[1][2] = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] +
00779             kQ[2][1]*m[2][2];
00780         kR[2][2] = kQ[0][2]*m[0][2] + kQ[1][2]*m[1][2] +
00781             kQ[2][2]*m[2][2];
00782 
00783         // the scaling component
00784         kD[0] = kR[0][0];
00785         kD[1] = kR[1][1];
00786         kD[2] = kR[2][2];
00787 
00788         // the shear component
00789         Real fInvD0 = 1.0/kD[0];
00790         kU[0] = kR[0][1]*fInvD0;
00791         kU[1] = kR[0][2]*fInvD0;
00792         kU[2] = kR[1][2]/kD[1];
00793     }
00794     //-----------------------------------------------------------------------
00795     Real Matrix3::MaxCubicRoot (Real afCoeff[3])
00796     {
00797         // Spectral norm is for A^T*A, so characteristic polynomial
00798         // P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive real roots.
00799         // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1].
00800 
00801         // quick out for uniform scale (triple root)
00802         const Real fOneThird = 1.0/3.0;
00803         const Real fEpsilon = 1e-06;
00804         Real fDiscr = afCoeff[2]*afCoeff[2] - 3.0*afCoeff[1];
00805         if ( fDiscr <= fEpsilon )
00806             return -fOneThird*afCoeff[2];
00807 
00808         // Compute an upper bound on roots of P(x).  This assumes that A^T*A
00809         // has been scaled by its largest entry.
00810         Real fX = 1.0;
00811         Real fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
00812         if ( fPoly < 0.0 )
00813         {
00814             // uses a matrix norm to find an upper bound on maximum root
00815             fX = Math::Abs(afCoeff[0]);
00816             Real fTmp = 1.0+Math::Abs(afCoeff[1]);
00817             if ( fTmp > fX )
00818                 fX = fTmp;
00819             fTmp = 1.0+Math::Abs(afCoeff[2]);
00820             if ( fTmp > fX )
00821                 fX = fTmp;
00822         }
00823 
00824         // Newton's method to find root
00825         Real fTwoC2 = 2.0*afCoeff[2];
00826         for (int i = 0; i < 16; i++)
00827         {
00828             fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
00829             if ( Math::Abs(fPoly) <= fEpsilon )
00830                 return fX;
00831 
00832             Real fDeriv = afCoeff[1]+fX*(fTwoC2+3.0*fX);
00833             fX -= fPoly/fDeriv;
00834         }
00835 
00836         return fX;
00837     }
00838     //-----------------------------------------------------------------------
00839     Real Matrix3::SpectralNorm () const
00840     {
00841         Matrix3 kP;
00842         size_t iRow, iCol;
00843         Real fPmax = 0.0;
00844         for (iRow = 0; iRow < 3; iRow++)
00845         {
00846             for (iCol = 0; iCol < 3; iCol++)
00847             {
00848                 kP[iRow][iCol] = 0.0;
00849                 for (int iMid = 0; iMid < 3; iMid++)
00850                 {
00851                     kP[iRow][iCol] +=
00852                         m[iMid][iRow]*m[iMid][iCol];
00853                 }
00854                 if ( kP[iRow][iCol] > fPmax )
00855                     fPmax = kP[iRow][iCol];
00856             }
00857         }
00858 
00859         Real fInvPmax = 1.0/fPmax;
00860         for (iRow = 0; iRow < 3; iRow++)
00861         {
00862             for (iCol = 0; iCol < 3; iCol++)
00863                 kP[iRow][iCol] *= fInvPmax;
00864         }
00865 
00866         Real afCoeff[3];
00867         afCoeff[0] = -(kP[0][0]*(kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1]) +
00868             kP[0][1]*(kP[2][0]*kP[1][2]-kP[1][0]*kP[2][2]) +
00869             kP[0][2]*(kP[1][0]*kP[2][1]-kP[2][0]*kP[1][1]));
00870         afCoeff[1] = kP[0][0]*kP[1][1]-kP[0][1]*kP[1][0] +
00871             kP[0][0]*kP[2][2]-kP[0][2]*kP[2][0] +
00872             kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1];
00873         afCoeff[2] = -(kP[0][0]+kP[1][1]+kP[2][2]);
00874 
00875         Real fRoot = MaxCubicRoot(afCoeff);
00876         Real fNorm = Math::Sqrt(fPmax*fRoot);
00877         return fNorm;
00878     }
00879     //-----------------------------------------------------------------------
00880     void Matrix3::ToAxisAngle (Vector3& rkAxis, Real& rfRadians) const
00881     {
00882         // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
00883         // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
00884         // I is the identity and
00885         //
00886         //       +-        -+
00887         //   P = |  0 -z +y |
00888         //       | +z  0 -x |
00889         //       | -y +x  0 |
00890         //       +-        -+
00891         //
00892         // If A > 0, R represents a counterclockwise rotation about the axis in
00893         // the sense of looking from the tip of the axis vector towards the
00894         // origin.  Some algebra will show that
00895         //
00896         //   cos(A) = (trace(R)-1)/2  and  R - R^t = 2*sin(A)*P
00897         //
00898         // In the event that A = pi, R-R^t = 0 which prevents us from extracting
00899         // the axis through P.  Instead note that R = I+2*P^2 when A = pi, so
00900         // P^2 = (R-I)/2.  The diagonal entries of P^2 are x^2-1, y^2-1, and
00901         // z^2-1.  We can solve these for axis (x,y,z).  Because the angle is pi,
00902         // it does not matter which sign you choose on the square roots.
00903 
00904         Real fTrace = m[0][0] + m[1][1] + m[2][2];
00905         Real fCos = 0.5*(fTrace-1.0);
00906         rfRadians = Math::ACos(fCos);  // in [0,PI]
00907 
00908         if ( rfRadians > 0.0 )
00909         {
00910             if ( rfRadians < Math::PI )
00911             {
00912                 rkAxis.x = m[2][1]-m[1][2];
00913                 rkAxis.y = m[0][2]-m[2][0];
00914                 rkAxis.z = m[1][0]-m[0][1];
00915                 rkAxis.normalise();
00916             }
00917             else
00918             {
00919                 // angle is PI
00920                 float fHalfInverse;
00921                 if ( m[0][0] >= m[1][1] )
00922                 {
00923                     // r00 >= r11
00924                     if ( m[0][0] >= m[2][2] )
00925                     {
00926                         // r00 is maximum diagonal term
00927                         rkAxis.x = 0.5*Math::Sqrt(m[0][0] -
00928                             m[1][1] - m[2][2] + 1.0);
00929                         fHalfInverse = 0.5/rkAxis.x;
00930                         rkAxis.y = fHalfInverse*m[0][1];
00931                         rkAxis.z = fHalfInverse*m[0][2];
00932                     }
00933                     else
00934                     {
00935                         // r22 is maximum diagonal term
00936                         rkAxis.z = 0.5*Math::Sqrt(m[2][2] -
00937                             m[0][0] - m[1][1] + 1.0);
00938                         fHalfInverse = 0.5/rkAxis.z;
00939                         rkAxis.x = fHalfInverse*m[0][2];
00940                         rkAxis.y = fHalfInverse*m[1][2];
00941                     }
00942                 }
00943                 else
00944                 {
00945                     // r11 > r00
00946                     if ( m[1][1] >= m[2][2] )
00947                     {
00948                         // r11 is maximum diagonal term
00949                         rkAxis.y = 0.5*Math::Sqrt(m[1][1] -
00950                             m[0][0] - m[2][2] + 1.0);
00951                         fHalfInverse  = 0.5/rkAxis.y;
00952                         rkAxis.x = fHalfInverse*m[0][1];
00953                         rkAxis.z = fHalfInverse*m[1][2];
00954                     }
00955                     else
00956                     {
00957                         // r22 is maximum diagonal term
00958                         rkAxis.z = 0.5*Math::Sqrt(m[2][2] -
00959                             m[0][0] - m[1][1] + 1.0);
00960                         fHalfInverse = 0.5/rkAxis.z;
00961                         rkAxis.x = fHalfInverse*m[0][2];
00962                         rkAxis.y = fHalfInverse*m[1][2];
00963                     }
00964                 }
00965             }
00966         }
00967         else
00968         {
00969             // The angle is 0 and the matrix is the identity.  Any axis will
00970             // work, so just use the x-axis.
00971             rkAxis.x = 1.0;
00972             rkAxis.y = 0.0;
00973             rkAxis.z = 0.0;
00974         }
00975     }
00976     //-----------------------------------------------------------------------
00977     void Matrix3::FromAxisAngle (const Vector3& rkAxis, Real fRadians)
00978     {
00979         Real fCos = Math::Cos(fRadians);
00980         Real fSin = Math::Sin(fRadians);
00981         Real fOneMinusCos = 1.0-fCos;
00982         Real fX2 = rkAxis.x*rkAxis.x;
00983         Real fY2 = rkAxis.y*rkAxis.y;
00984         Real fZ2 = rkAxis.z*rkAxis.z;
00985         Real fXYM = rkAxis.x*rkAxis.y*fOneMinusCos;
00986         Real fXZM = rkAxis.x*rkAxis.z*fOneMinusCos;
00987         Real fYZM = rkAxis.y*rkAxis.z*fOneMinusCos;
00988         Real fXSin = rkAxis.x*fSin;
00989         Real fYSin = rkAxis.y*fSin;
00990         Real fZSin = rkAxis.z*fSin;
00991 
00992         m[0][0] = fX2*fOneMinusCos+fCos;
00993         m[0][1] = fXYM-fZSin;
00994         m[0][2] = fXZM+fYSin;
00995         m[1][0] = fXYM+fZSin;
00996         m[1][1] = fY2*fOneMinusCos+fCos;
00997         m[1][2] = fYZM-fXSin;
00998         m[2][0] = fXZM-fYSin;
00999         m[2][1] = fYZM+fXSin;
01000         m[2][2] = fZ2*fOneMinusCos+fCos;
01001     }
01002     //-----------------------------------------------------------------------
01003     bool Matrix3::ToEulerAnglesXYZ (float& rfYAngle, float& rfPAngle,
01004         float& rfRAngle) const
01005     {
01006         // rot =  cy*cz          -cy*sz           sy
01007         //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
01008         //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
01009 
01010         rfPAngle = Math::ASin(m[0][2]);
01011         if ( rfPAngle < Math::HALF_PI )
01012         {
01013             if ( rfPAngle > -Math::HALF_PI )
01014             {
01015                 rfYAngle = Math::ATan2(-m[1][2],m[2][2]);
01016                 rfRAngle = Math::ATan2(-m[0][1],m[0][0]);
01017                 return true;
01018             }
01019             else
01020             {
01021                 // WARNING.  Not a unique solution.
01022                 float fRmY = Math::ATan2(m[1][0],m[1][1]);
01023                 rfRAngle = 0.0;  // any angle works
01024                 rfYAngle = rfRAngle - fRmY;
01025                 return false;
01026             }
01027         }
01028         else
01029         {
01030             // WARNING.  Not a unique solution.
01031             float fRpY = Math::ATan2(m[1][0],m[1][1]);
01032             rfRAngle = 0.0;  // any angle works
01033             rfYAngle = fRpY - rfRAngle;
01034             return false;
01035         }
01036     }
01037     //-----------------------------------------------------------------------
01038     bool Matrix3::ToEulerAnglesXZY (float& rfYAngle, float& rfPAngle,
01039         float& rfRAngle) const
01040     {
01041         // rot =  cy*cz          -sz              cz*sy
01042         //        sx*sy+cx*cy*sz  cx*cz          -cy*sx+cx*sy*sz
01043         //       -cx*sy+cy*sx*sz  cz*sx           cx*cy+sx*sy*sz
01044 
01045         rfPAngle = Math::ASin(-m[0][1]);
01046         if ( rfPAngle < Math::HALF_PI )
01047         {
01048             if ( rfPAngle > -Math::HALF_PI )
01049             {
01050                 rfYAngle = Math::ATan2(m[2][1],m[1][1]);
01051                 rfRAngle = Math::ATan2(m[0][2],m[0][0]);
01052                 return true;
01053             }
01054             else
01055             {
01056                 // WARNING.  Not a unique solution.
01057                 float fRmY = Math::ATan2(-m[2][0],m[2][2]);
01058                 rfRAngle = 0.0;  // any angle works
01059                 rfYAngle = rfRAngle - fRmY;
01060                 return false;
01061             }
01062         }
01063         else
01064         {
01065             // WARNING.  Not a unique solution.
01066             float fRpY = Math::ATan2(-m[2][0],m[2][2]);
01067             rfRAngle = 0.0;  // any angle works
01068             rfYAngle = fRpY - rfRAngle;
01069             return false;
01070         }
01071     }
01072     //-----------------------------------------------------------------------
01073     bool Matrix3::ToEulerAnglesYXZ (float& rfYAngle, float& rfPAngle,
01074         float& rfRAngle) const
01075     {
01076         // rot =  cy*cz+sx*sy*sz  cz*sx*sy-cy*sz  cx*sy
01077         //        cx*sz           cx*cz          -sx
01078         //       -cz*sy+cy*sx*sz  cy*cz*sx+sy*sz  cx*cy
01079 
01080         rfPAngle = Math::ASin(-m[1][2]);
01081         if ( rfPAngle < Math::HALF_PI )
01082         {
01083             if ( rfPAngle > -Math::HALF_PI )
01084             {
01085                 rfYAngle = Math::ATan2(m[0][2],m[2][2]);
01086                 rfRAngle = Math::ATan2(m[1][0],m[1][1]);
01087                 return true;
01088             }
01089             else
01090             {
01091                 // WARNING.  Not a unique solution.
01092                 float fRmY = Math::ATan2(-m[0][1],m[0][0]);
01093                 rfRAngle = 0.0;  // any angle works
01094                 rfYAngle = rfRAngle - fRmY;
01095                 return false;
01096             }
01097         }
01098         else
01099         {
01100             // WARNING.  Not a unique solution.
01101             float fRpY = Math::ATan2(-m[0][1],m[0][0]);
01102             rfRAngle = 0.0;  // any angle works
01103             rfYAngle = fRpY - rfRAngle;
01104             return false;
01105         }
01106     }
01107     //-----------------------------------------------------------------------
01108     bool Matrix3::ToEulerAnglesYZX (float& rfYAngle, float& rfPAngle,
01109         float& rfRAngle) const
01110     {
01111         // rot =  cy*cz           sx*sy-cx*cy*sz  cx*sy+cy*sx*sz
01112         //        sz              cx*cz          -cz*sx
01113         //       -cz*sy           cy*sx+cx*sy*sz  cx*cy-sx*sy*sz
01114 
01115         rfPAngle = Math::ASin(m[1][0]);
01116         if ( rfPAngle < Math::HALF_PI )
01117         {
01118             if ( rfPAngle > -Math::HALF_PI )
01119             {
01120                 rfYAngle = Math::ATan2(-m[2][0],m[0][0]);
01121                 rfRAngle = Math::ATan2(-m[1][2],m[1][1]);
01122                 return true;
01123             }
01124             else
01125             {
01126                 // WARNING.  Not a unique solution.
01127                 float fRmY = Math::ATan2(m[2][1],m[2][2]);
01128                 rfRAngle = 0.0;  // any angle works
01129                 rfYAngle = rfRAngle - fRmY;
01130                 return false;
01131             }
01132         }
01133         else
01134         {
01135             // WARNING.  Not a unique solution.
01136             float fRpY = Math::ATan2(m[2][1],m[2][2]);
01137             rfRAngle = 0.0;  // any angle works
01138             rfYAngle = fRpY - rfRAngle;
01139             return false;
01140         }
01141     }
01142     //-----------------------------------------------------------------------
01143     bool Matrix3::ToEulerAnglesZXY (float& rfYAngle, float& rfPAngle,
01144         float& rfRAngle) const
01145     {
01146         // rot =  cy*cz-sx*sy*sz -cx*sz           cz*sy+cy*sx*sz
01147         //        cz*sx*sy+cy*sz  cx*cz          -cy*cz*sx+sy*sz
01148         //       -cx*sy           sx              cx*cy
01149 
01150         rfPAngle = Math::ASin(m[2][1]);
01151         if ( rfPAngle < Math::HALF_PI )
01152         {
01153             if ( rfPAngle > -Math::HALF_PI )
01154             {
01155                 rfYAngle = Math::ATan2(-m[0][1],m[1][1]);
01156                 rfRAngle = Math::ATan2(-m[2][0],m[2][2]);
01157                 return true;
01158             }
01159             else
01160             {
01161                 // WARNING.  Not a unique solution.
01162                 float fRmY = Math::ATan2(m[0][2],m[0][0]);
01163                 rfRAngle = 0.0;  // any angle works
01164                 rfYAngle = rfRAngle - fRmY;
01165                 return false;
01166             }
01167         }
01168         else
01169         {
01170             // WARNING.  Not a unique solution.
01171             float fRpY = Math::ATan2(m[0][2],m[0][0]);
01172             rfRAngle = 0.0;  // any angle works
01173             rfYAngle = fRpY - rfRAngle;
01174             return false;
01175         }
01176     }
01177     //-----------------------------------------------------------------------
01178     bool Matrix3::ToEulerAnglesZYX (float& rfYAngle, float& rfPAngle,
01179         float& rfRAngle) const
01180     {
01181         // rot =  cy*cz           cz*sx*sy-cx*sz  cx*cz*sy+sx*sz
01182         //        cy*sz           cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
01183         //       -sy              cy*sx           cx*cy
01184 
01185         rfPAngle = Math::ASin(-m[2][0]);
01186         if ( rfPAngle < Math::HALF_PI )
01187         {
01188             if ( rfPAngle > -Math::HALF_PI )
01189             {
01190                 rfYAngle = Math::ATan2(m[1][0],m[0][0]);
01191                 rfRAngle = Math::ATan2(m[2][1],m[2][2]);
01192                 return true;
01193             }
01194             else
01195             {
01196                 // WARNING.  Not a unique solution.
01197                 float fRmY = Math::ATan2(-m[0][1],m[0][2]);
01198                 rfRAngle = 0.0;  // any angle works
01199                 rfYAngle = rfRAngle - fRmY;
01200                 return false;
01201             }
01202         }
01203         else
01204         {
01205             // WARNING.  Not a unique solution.
01206             float fRpY = Math::ATan2(-m[0][1],m[0][2]);
01207             rfRAngle = 0.0;  // any angle works
01208             rfYAngle = fRpY - rfRAngle;
01209             return false;
01210         }
01211     }
01212     //-----------------------------------------------------------------------
01213     void Matrix3::FromEulerAnglesXYZ (float fYAngle, float fPAngle,
01214         float fRAngle)
01215     {
01216         Real fCos, fSin;
01217 
01218         fCos = Math::Cos(fYAngle);
01219         fSin = Math::Sin(fYAngle);
01220         Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
01221 
01222         fCos = Math::Cos(fPAngle);
01223         fSin = Math::Sin(fPAngle);
01224         Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
01225 
01226         fCos = Math::Cos(fRAngle);
01227         fSin = Math::Sin(fRAngle);
01228         Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
01229 
01230         *this = kXMat*(kYMat*kZMat);
01231     }
01232     //-----------------------------------------------------------------------
01233     void Matrix3::FromEulerAnglesXZY (float fYAngle, float fPAngle,
01234         float fRAngle)
01235     {
01236         Real fCos, fSin;
01237 
01238         fCos = Math::Cos(fYAngle);
01239         fSin = Math::Sin(fYAngle);
01240         Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
01241 
01242         fCos = Math::Cos(fPAngle);
01243         fSin = Math::Sin(fPAngle);
01244         Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
01245 
01246         fCos = Math::Cos(fRAngle);
01247         fSin = Math::Sin(fRAngle);
01248         Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
01249 
01250         *this = kXMat*(kZMat*kYMat);
01251     }
01252     //-----------------------------------------------------------------------
01253     void Matrix3::FromEulerAnglesYXZ (float fYAngle, float fPAngle,
01254         float fRAngle)
01255     {
01256         Real fCos, fSin;
01257 
01258         fCos = Math::Cos(fYAngle);
01259         fSin = Math::Sin(fYAngle);
01260         Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
01261 
01262         fCos = Math::Cos(fPAngle);
01263         fSin = Math::Sin(fPAngle);
01264         Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
01265 
01266         fCos = Math::Cos(fRAngle);
01267         fSin = Math::Sin(fRAngle);
01268         Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
01269 
01270         *this = kYMat*(kXMat*kZMat);
01271     }
01272     //-----------------------------------------------------------------------
01273     void Matrix3::FromEulerAnglesYZX (float fYAngle, float fPAngle,
01274         float fRAngle)
01275     {
01276         Real fCos, fSin;
01277 
01278         fCos = Math::Cos(fYAngle);
01279         fSin = Math::Sin(fYAngle);
01280         Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
01281 
01282         fCos = Math::Cos(fPAngle);
01283         fSin = Math::Sin(fPAngle);
01284         Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
01285 
01286         fCos = Math::Cos(fRAngle);
01287         fSin = Math::Sin(fRAngle);
01288         Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
01289 
01290         *this = kYMat*(kZMat*kXMat);
01291     }
01292     //-----------------------------------------------------------------------
01293     void Matrix3::FromEulerAnglesZXY (float fYAngle, float fPAngle,
01294         float fRAngle)
01295     {
01296         Real fCos, fSin;
01297 
01298         fCos = Math::Cos(fYAngle);
01299         fSin = Math::Sin(fYAngle);
01300         Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
01301 
01302         fCos = Math::Cos(fPAngle);
01303         fSin = Math::Sin(fPAngle);
01304         Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
01305 
01306         fCos = Math::Cos(fRAngle);
01307         fSin = Math::Sin(fRAngle);
01308         Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
01309 
01310         *this = kZMat*(kXMat*kYMat);
01311     }
01312     //-----------------------------------------------------------------------
01313     void Matrix3::FromEulerAnglesZYX (float fYAngle, float fPAngle,
01314         float fRAngle)
01315     {
01316         Real fCos, fSin;
01317 
01318         fCos = Math::Cos(fYAngle);
01319         fSin = Math::Sin(fYAngle);
01320         Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
01321 
01322         fCos = Math::Cos(fPAngle);
01323         fSin = Math::Sin(fPAngle);
01324         Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
01325 
01326         fCos = Math::Cos(fRAngle);
01327         fSin = Math::Sin(fRAngle);
01328         Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
01329 
01330         *this = kZMat*(kYMat*kXMat);
01331     }
01332     //-----------------------------------------------------------------------
01333     void Matrix3::Tridiagonal (Real afDiag[3], Real afSubDiag[3])
01334     {
01335         // Householder reduction T = Q^t M Q
01336         //   Input:
01337         //     mat, symmetric 3x3 matrix M
01338         //   Output:
01339         //     mat, orthogonal matrix Q
01340         //     diag, diagonal entries of T
01341         //     subd, subdiagonal entries of T (T is symmetric)
01342 
01343         Real fA = m[0][0];
01344         Real fB = m[0][1];
01345         Real fC = m[0][2];
01346         Real fD = m[1][1];
01347         Real fE = m[1][2];
01348         Real fF = m[2][2];
01349 
01350         afDiag[0] = fA;
01351         afSubDiag[2] = 0.0;
01352         if ( Math::Abs(fC) >= EPSILON )
01353         {
01354             Real fLength = Math::Sqrt(fB*fB+fC*fC);
01355             Real fInvLength = 1.0/fLength;
01356             fB *= fInvLength;
01357             fC *= fInvLength;
01358             Real fQ = 2.0*fB*fE+fC*(fF-fD);
01359             afDiag[1] = fD+fC*fQ;
01360             afDiag[2] = fF-fC*fQ;
01361             afSubDiag[0] = fLength;
01362             afSubDiag[1] = fE-fB*fQ;
01363             m[0][0] = 1.0;
01364             m[0][1] = 0.0;
01365             m[0][2] = 0.0;
01366             m[1][0] = 0.0;
01367             m[1][1] = fB;
01368             m[1][2] = fC;
01369             m[2][0] = 0.0;
01370             m[2][1] = fC;
01371             m[2][2] = -fB;
01372         }
01373         else
01374         {
01375             afDiag[1] = fD;
01376             afDiag[2] = fF;
01377             afSubDiag[0] = fB;
01378             afSubDiag[1] = fE;
01379             m[0][0] = 1.0;
01380             m[0][1] = 0.0;
01381             m[0][2] = 0.0;
01382             m[1][0] = 0.0;
01383             m[1][1] = 1.0;
01384             m[1][2] = 0.0;
01385             m[2][0] = 0.0;
01386             m[2][1] = 0.0;
01387             m[2][2] = 1.0;
01388         }
01389     }
01390     //-----------------------------------------------------------------------
01391     bool Matrix3::QLAlgorithm (Real afDiag[3], Real afSubDiag[3])
01392     {
01393         // QL iteration with implicit shifting to reduce matrix from tridiagonal
01394         // to diagonal
01395 
01396         for (size_t i0 = 0; i0 < 3; i0++)
01397         {
01398             const unsigned int iMaxIter = 32;
01399             unsigned int iIter;
01400             for (iIter = 0; iIter < iMaxIter; iIter++)
01401             {
01402                 size_t i1;
01403                 for (i1 = i0; i1 <= 1; i1++)
01404                 {
01405                     Real fSum = Math::Abs(afDiag[i1]) +
01406                         Math::Abs(afDiag[i1+1]);
01407                     if ( Math::Abs(afSubDiag[i1]) + fSum == fSum )
01408                         break;
01409                 }
01410                 if ( i1 == i0 )
01411                     break;
01412 
01413                 Real fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0*afSubDiag[i0]);
01414                 Real fTmp1 = Math::Sqrt(fTmp0*fTmp0+1.0);
01415                 if ( fTmp0 < 0.0 )
01416                     fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
01417                 else
01418                     fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
01419                 Real fSin = 1.0;
01420                 Real fCos = 1.0;
01421                 Real fTmp2 = 0.0;
01422                 for (size_t i2 = i1-1; i2 >= i0; i2--)
01423                 {
01424                     Real fTmp3 = fSin*afSubDiag[i2];
01425                     Real fTmp4 = fCos*afSubDiag[i2];
01426                     if ( Math::Abs(fTmp3) >= Math::Abs(fTmp0) )
01427                     {
01428                         fCos = fTmp0/fTmp3;
01429                         fTmp1 = Math::Sqrt(fCos*fCos+1.0);
01430                         afSubDiag[i2+1] = fTmp3*fTmp1;
01431                         fSin = 1.0/fTmp1;
01432                         fCos *= fSin;
01433                     }
01434                     else
01435                     {
01436                         fSin = fTmp3/fTmp0;
01437                         fTmp1 = Math::Sqrt(fSin*fSin+1.0);
01438                         afSubDiag[i2+1] = fTmp0*fTmp1;
01439                         fCos = 1.0/fTmp1;
01440                         fSin *= fCos;
01441                     }
01442                     fTmp0 = afDiag[i2+1]-fTmp2;
01443                     fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0*fTmp4*fCos;
01444                     fTmp2 = fSin*fTmp1;
01445                     afDiag[i2+1] = fTmp0+fTmp2;
01446                     fTmp0 = fCos*fTmp1-fTmp4;
01447 
01448                     for (size_t iRow = 0; iRow < 3; iRow++)
01449                     {
01450                         fTmp3 = m[iRow][i2+1];
01451                         m[iRow][i2+1] = fSin*m[iRow][i2] +
01452                             fCos*fTmp3;
01453                         m[iRow][i2] = fCos*m[iRow][i2] -
01454                             fSin*fTmp3;
01455                     }
01456                 }
01457                 afDiag[i0] -= fTmp2;
01458                 afSubDiag[i0] = fTmp0;
01459                 afSubDiag[i1] = 0.0;
01460             }
01461 
01462             if ( iIter == iMaxIter )
01463             {
01464                 // should not get here under normal circumstances
01465                 return false;
01466             }
01467         }
01468 
01469         return true;
01470     }
01471     //-----------------------------------------------------------------------
01472     void Matrix3::EigenSolveSymmetric (Real afEigenvalue[3],
01473         Vector3 akEigenvector[3]) const
01474     {
01475         Matrix3 kMatrix = *this;
01476         Real afSubDiag[3];
01477         kMatrix.Tridiagonal(afEigenvalue,afSubDiag);
01478         kMatrix.QLAlgorithm(afEigenvalue,afSubDiag);
01479 
01480         for (size_t i = 0; i < 3; i++)
01481         {
01482             akEigenvector[i][0] = kMatrix[0][i];
01483             akEigenvector[i][1] = kMatrix[1][i];
01484             akEigenvector[i][2] = kMatrix[2][i];
01485         }
01486 
01487         // make eigenvectors form a right--handed system
01488         Vector3 kCross = akEigenvector[1].crossProduct(akEigenvector[2]);
01489         Real fDet = akEigenvector[0].dotProduct(kCross);
01490         if ( fDet < 0.0 )
01491         {
01492             akEigenvector[2][0] = - akEigenvector[2][0];
01493             akEigenvector[2][1] = - akEigenvector[2][1];
01494             akEigenvector[2][2] = - akEigenvector[2][2];
01495         }
01496     }
01497     //-----------------------------------------------------------------------
01498     void Matrix3::TensorProduct (const Vector3& rkU, const Vector3& rkV,
01499         Matrix3& rkProduct)
01500     {
01501         for (size_t iRow = 0; iRow < 3; iRow++)
01502         {
01503             for (size_t iCol = 0; iCol < 3; iCol++)
01504                 rkProduct[iRow][iCol] = rkU[iRow]*rkV[iCol];
01505         }
01506     }
01507     //-----------------------------------------------------------------------
01508 }

Copyright © 2002-2003 by The OGRE Team
Last modified Fri May 14 23:22:23 2004