Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: sandbox_light/src/external/ogremath/OgreMatrix3.cpp @ 6042

Last change on this file since 6042 was 5789, checked in by rgrieder, 15 years ago

Stripped sandbox further down to get a light version that excludes almost all core features.
Also, the Ogre dependency has been removed and a little ogremath library been added.
  • Property svn:eol-style set to native
File size: 51.1 KB
Line 
1/*
2-----------------------------------------------------------------------------
3This source file is part of OGRE
4    (Object-oriented Graphics Rendering Engine)
5For the latest info, see http://www.ogre3d.org/
6
7Copyright (c) 2000-2006 Torus Knot Software Ltd
8Also see acknowledgements in Readme.html
9
10This program is free software; you can redistribute it and/or modify it under
11the terms of the GNU Lesser General Public License as published by the Free Software
12Foundation; either version 2 of the License, or (at your option) any later
13version.
14
15This program is distributed in the hope that it will be useful, but WITHOUT
16ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
17FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.
18
19You should have received a copy of the GNU Lesser General Public License along with
20this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21Place - Suite 330, Boston, MA 02111-1307, USA, or go to
22http://www.gnu.org/copyleft/lesser.txt.
23
24You may alternatively use this source under the terms of a specific version of
25the OGRE Unrestricted License provided you have obtained such a license from
26Torus Knot Software Ltd.
27-----------------------------------------------------------------------------
28*/
29#include "OgreMatrix3.h"
30
31#include "OgreMath.h"
32
33// Adapted from Matrix math by Wild Magic http://www.geometrictools.com/
34
35namespace Ogre
36{
37    const Real Matrix3::EPSILON = 1e-06;
38    const Matrix3 Matrix3::ZERO(0,0,0,0,0,0,0,0,0);
39    const Matrix3 Matrix3::IDENTITY(1,0,0,0,1,0,0,0,1);
40    const Real Matrix3::ms_fSvdEpsilon = 1e-04;
41    const unsigned int Matrix3::ms_iSvdMaxIterations = 32;
42
43    //-----------------------------------------------------------------------
44    Vector3 Matrix3::GetColumn (size_t iCol) const
45    {
46        assert( 0 <= iCol && iCol < 3 );
47        return Vector3(m[0][iCol],m[1][iCol],
48            m[2][iCol]);
49    }
50    //-----------------------------------------------------------------------
51    void Matrix3::SetColumn(size_t iCol, const Vector3& vec)
52    {
53        assert( 0 <= iCol && iCol < 3 );
54        m[0][iCol] = vec.x;
55        m[1][iCol] = vec.y;
56        m[2][iCol] = vec.z;
57
58    }
59    //-----------------------------------------------------------------------
60    void Matrix3::FromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
61    {
62        SetColumn(0,xAxis);
63        SetColumn(1,yAxis);
64        SetColumn(2,zAxis);
65
66    }
67
68    //-----------------------------------------------------------------------
69    bool Matrix3::operator== (const Matrix3& rkMatrix) const
70    {
71        for (size_t iRow = 0; iRow < 3; iRow++)
72        {
73            for (size_t iCol = 0; iCol < 3; iCol++)
74            {
75                if ( m[iRow][iCol] != rkMatrix.m[iRow][iCol] )
76                    return false;
77            }
78        }
79
80        return true;
81    }
82    //-----------------------------------------------------------------------
83    Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const
84    {
85        Matrix3 kSum;
86        for (size_t iRow = 0; iRow < 3; iRow++)
87        {
88            for (size_t iCol = 0; iCol < 3; iCol++)
89            {
90                kSum.m[iRow][iCol] = m[iRow][iCol] +
91                    rkMatrix.m[iRow][iCol];
92            }
93        }
94        return kSum;
95    }
96    //-----------------------------------------------------------------------
97    Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const
98    {
99        Matrix3 kDiff;
100        for (size_t iRow = 0; iRow < 3; iRow++)
101        {
102            for (size_t iCol = 0; iCol < 3; iCol++)
103            {
104                kDiff.m[iRow][iCol] = m[iRow][iCol] -
105                    rkMatrix.m[iRow][iCol];
106            }
107        }
108        return kDiff;
109    }
110    //-----------------------------------------------------------------------
111    Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const
112    {
113        Matrix3 kProd;
114        for (size_t iRow = 0; iRow < 3; iRow++)
115        {
116            for (size_t iCol = 0; iCol < 3; iCol++)
117            {
118                kProd.m[iRow][iCol] =
119                    m[iRow][0]*rkMatrix.m[0][iCol] +
120                    m[iRow][1]*rkMatrix.m[1][iCol] +
121                    m[iRow][2]*rkMatrix.m[2][iCol];
122            }
123        }
124        return kProd;
125    }
126    //-----------------------------------------------------------------------
127    Vector3 Matrix3::operator* (const Vector3& rkPoint) const
128    {
129        Vector3 kProd;
130        for (size_t iRow = 0; iRow < 3; iRow++)
131        {
132            kProd[iRow] =
133                m[iRow][0]*rkPoint[0] +
134                m[iRow][1]*rkPoint[1] +
135                m[iRow][2]*rkPoint[2];
136        }
137        return kProd;
138    }
139    //-----------------------------------------------------------------------
140    Vector3 operator* (const Vector3& rkPoint, const Matrix3& rkMatrix)
141    {
142        Vector3 kProd;
143        for (size_t iRow = 0; iRow < 3; iRow++)
144        {
145            kProd[iRow] =
146                rkPoint[0]*rkMatrix.m[0][iRow] +
147                rkPoint[1]*rkMatrix.m[1][iRow] +
148                rkPoint[2]*rkMatrix.m[2][iRow];
149        }
150        return kProd;
151    }
152    //-----------------------------------------------------------------------
153    Matrix3 Matrix3::operator- () const
154    {
155        Matrix3 kNeg;
156        for (size_t iRow = 0; iRow < 3; iRow++)
157        {
158            for (size_t iCol = 0; iCol < 3; iCol++)
159                kNeg[iRow][iCol] = -m[iRow][iCol];
160        }
161        return kNeg;
162    }
163    //-----------------------------------------------------------------------
164    Matrix3 Matrix3::operator* (Real fScalar) const
165    {
166        Matrix3 kProd;
167        for (size_t iRow = 0; iRow < 3; iRow++)
168        {
169            for (size_t iCol = 0; iCol < 3; iCol++)
170                kProd[iRow][iCol] = fScalar*m[iRow][iCol];
171        }
172        return kProd;
173    }
174    //-----------------------------------------------------------------------
175    Matrix3 operator* (Real fScalar, const Matrix3& rkMatrix)
176    {
177        Matrix3 kProd;
178        for (size_t iRow = 0; iRow < 3; iRow++)
179        {
180            for (size_t iCol = 0; iCol < 3; iCol++)
181                kProd[iRow][iCol] = fScalar*rkMatrix.m[iRow][iCol];
182        }
183        return kProd;
184    }
185    //-----------------------------------------------------------------------
186    Matrix3 Matrix3::Transpose () const
187    {
188        Matrix3 kTranspose;
189        for (size_t iRow = 0; iRow < 3; iRow++)
190        {
191            for (size_t iCol = 0; iCol < 3; iCol++)
192                kTranspose[iRow][iCol] = m[iCol][iRow];
193        }
194        return kTranspose;
195    }
196    //-----------------------------------------------------------------------
197    bool Matrix3::Inverse (Matrix3& rkInverse, Real fTolerance) const
198    {
199        // Invert a 3x3 using cofactors.  This is about 8 times faster than
200        // the Numerical Recipes code which uses Gaussian elimination.
201
202        rkInverse[0][0] = m[1][1]*m[2][2] -
203            m[1][2]*m[2][1];
204        rkInverse[0][1] = m[0][2]*m[2][1] -
205            m[0][1]*m[2][2];
206        rkInverse[0][2] = m[0][1]*m[1][2] -
207            m[0][2]*m[1][1];
208        rkInverse[1][0] = m[1][2]*m[2][0] -
209            m[1][0]*m[2][2];
210        rkInverse[1][1] = m[0][0]*m[2][2] -
211            m[0][2]*m[2][0];
212        rkInverse[1][2] = m[0][2]*m[1][0] -
213            m[0][0]*m[1][2];
214        rkInverse[2][0] = m[1][0]*m[2][1] -
215            m[1][1]*m[2][0];
216        rkInverse[2][1] = m[0][1]*m[2][0] -
217            m[0][0]*m[2][1];
218        rkInverse[2][2] = m[0][0]*m[1][1] -
219            m[0][1]*m[1][0];
220
221        Real fDet =
222            m[0][0]*rkInverse[0][0] +
223            m[0][1]*rkInverse[1][0]+
224            m[0][2]*rkInverse[2][0];
225
226        if ( Math::Abs(fDet) <= fTolerance )
227            return false;
228
229        Real fInvDet = 1.0/fDet;
230        for (size_t iRow = 0; iRow < 3; iRow++)
231        {
232            for (size_t iCol = 0; iCol < 3; iCol++)
233                rkInverse[iRow][iCol] *= fInvDet;
234        }
235
236        return true;
237    }
238    //-----------------------------------------------------------------------
239    Matrix3 Matrix3::Inverse (Real fTolerance) const
240    {
241        Matrix3 kInverse = Matrix3::ZERO;
242        Inverse(kInverse,fTolerance);
243        return kInverse;
244    }
245    //-----------------------------------------------------------------------
246    Real Matrix3::Determinant () const
247    {
248        Real fCofactor00 = m[1][1]*m[2][2] -
249            m[1][2]*m[2][1];
250        Real fCofactor10 = m[1][2]*m[2][0] -
251            m[1][0]*m[2][2];
252        Real fCofactor20 = m[1][0]*m[2][1] -
253            m[1][1]*m[2][0];
254
255        Real fDet =
256            m[0][0]*fCofactor00 +
257            m[0][1]*fCofactor10 +
258            m[0][2]*fCofactor20;
259
260        return fDet;
261    }
262    //-----------------------------------------------------------------------
263    void Matrix3::Bidiagonalize (Matrix3& kA, Matrix3& kL,
264        Matrix3& kR)
265    {
266        Real afV[3], afW[3];
267        Real fLength, fSign, fT1, fInvT1, fT2;
268        bool bIdentity;
269
270        // map first column to (*,0,0)
271        fLength = Math::Sqrt(kA[0][0]*kA[0][0] + kA[1][0]*kA[1][0] +
272            kA[2][0]*kA[2][0]);
273        if ( fLength > 0.0 )
274        {
275            fSign = (kA[0][0] > 0.0 ? 1.0 : -1.0);
276            fT1 = kA[0][0] + fSign*fLength;
277            fInvT1 = 1.0/fT1;
278            afV[1] = kA[1][0]*fInvT1;
279            afV[2] = kA[2][0]*fInvT1;
280
281            fT2 = -2.0/(1.0+afV[1]*afV[1]+afV[2]*afV[2]);
282            afW[0] = fT2*(kA[0][0]+kA[1][0]*afV[1]+kA[2][0]*afV[2]);
283            afW[1] = fT2*(kA[0][1]+kA[1][1]*afV[1]+kA[2][1]*afV[2]);
284            afW[2] = fT2*(kA[0][2]+kA[1][2]*afV[1]+kA[2][2]*afV[2]);
285            kA[0][0] += afW[0];
286            kA[0][1] += afW[1];
287            kA[0][2] += afW[2];
288            kA[1][1] += afV[1]*afW[1];
289            kA[1][2] += afV[1]*afW[2];
290            kA[2][1] += afV[2]*afW[1];
291            kA[2][2] += afV[2]*afW[2];
292
293            kL[0][0] = 1.0+fT2;
294            kL[0][1] = kL[1][0] = fT2*afV[1];
295            kL[0][2] = kL[2][0] = fT2*afV[2];
296            kL[1][1] = 1.0+fT2*afV[1]*afV[1];
297            kL[1][2] = kL[2][1] = fT2*afV[1]*afV[2];
298            kL[2][2] = 1.0+fT2*afV[2]*afV[2];
299            bIdentity = false;
300        }
301        else
302        {
303            kL = Matrix3::IDENTITY;
304            bIdentity = true;
305        }
306
307        // map first row to (*,*,0)
308        fLength = Math::Sqrt(kA[0][1]*kA[0][1]+kA[0][2]*kA[0][2]);
309        if ( fLength > 0.0 )
310        {
311            fSign = (kA[0][1] > 0.0 ? 1.0 : -1.0);
312            fT1 = kA[0][1] + fSign*fLength;
313            afV[2] = kA[0][2]/fT1;
314
315            fT2 = -2.0/(1.0+afV[2]*afV[2]);
316            afW[0] = fT2*(kA[0][1]+kA[0][2]*afV[2]);
317            afW[1] = fT2*(kA[1][1]+kA[1][2]*afV[2]);
318            afW[2] = fT2*(kA[2][1]+kA[2][2]*afV[2]);
319            kA[0][1] += afW[0];
320            kA[1][1] += afW[1];
321            kA[1][2] += afW[1]*afV[2];
322            kA[2][1] += afW[2];
323            kA[2][2] += afW[2]*afV[2];
324
325            kR[0][0] = 1.0;
326            kR[0][1] = kR[1][0] = 0.0;
327            kR[0][2] = kR[2][0] = 0.0;
328            kR[1][1] = 1.0+fT2;
329            kR[1][2] = kR[2][1] = fT2*afV[2];
330            kR[2][2] = 1.0+fT2*afV[2]*afV[2];
331        }
332        else
333        {
334            kR = Matrix3::IDENTITY;
335        }
336
337        // map second column to (*,*,0)
338        fLength = Math::Sqrt(kA[1][1]*kA[1][1]+kA[2][1]*kA[2][1]);
339        if ( fLength > 0.0 )
340        {
341            fSign = (kA[1][1] > 0.0 ? 1.0 : -1.0);
342            fT1 = kA[1][1] + fSign*fLength;
343            afV[2] = kA[2][1]/fT1;
344
345            fT2 = -2.0/(1.0+afV[2]*afV[2]);
346            afW[1] = fT2*(kA[1][1]+kA[2][1]*afV[2]);
347            afW[2] = fT2*(kA[1][2]+kA[2][2]*afV[2]);
348            kA[1][1] += afW[1];
349            kA[1][2] += afW[2];
350            kA[2][2] += afV[2]*afW[2];
351
352            Real fA = 1.0+fT2;
353            Real fB = fT2*afV[2];
354            Real fC = 1.0+fB*afV[2];
355
356            if ( bIdentity )
357            {
358                kL[0][0] = 1.0;
359                kL[0][1] = kL[1][0] = 0.0;
360                kL[0][2] = kL[2][0] = 0.0;
361                kL[1][1] = fA;
362                kL[1][2] = kL[2][1] = fB;
363                kL[2][2] = fC;
364            }
365            else
366            {
367                for (int iRow = 0; iRow < 3; iRow++)
368                {
369                    Real fTmp0 = kL[iRow][1];
370                    Real fTmp1 = kL[iRow][2];
371                    kL[iRow][1] = fA*fTmp0+fB*fTmp1;
372                    kL[iRow][2] = fB*fTmp0+fC*fTmp1;
373                }
374            }
375        }
376    }
377    //-----------------------------------------------------------------------
378    void Matrix3::GolubKahanStep (Matrix3& kA, Matrix3& kL,
379        Matrix3& kR)
380    {
381        Real fT11 = kA[0][1]*kA[0][1]+kA[1][1]*kA[1][1];
382        Real fT22 = kA[1][2]*kA[1][2]+kA[2][2]*kA[2][2];
383        Real fT12 = kA[1][1]*kA[1][2];
384        Real fTrace = fT11+fT22;
385        Real fDiff = fT11-fT22;
386        Real fDiscr = Math::Sqrt(fDiff*fDiff+4.0*fT12*fT12);
387        Real fRoot1 = 0.5*(fTrace+fDiscr);
388        Real fRoot2 = 0.5*(fTrace-fDiscr);
389
390        // adjust right
391        Real fY = kA[0][0] - (Math::Abs(fRoot1-fT22) <=
392            Math::Abs(fRoot2-fT22) ? fRoot1 : fRoot2);
393        Real fZ = kA[0][1];
394                Real fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
395        Real fSin = fZ*fInvLength;
396        Real fCos = -fY*fInvLength;
397
398        Real fTmp0 = kA[0][0];
399        Real fTmp1 = kA[0][1];
400        kA[0][0] = fCos*fTmp0-fSin*fTmp1;
401        kA[0][1] = fSin*fTmp0+fCos*fTmp1;
402        kA[1][0] = -fSin*kA[1][1];
403        kA[1][1] *= fCos;
404
405        size_t iRow;
406        for (iRow = 0; iRow < 3; iRow++)
407        {
408            fTmp0 = kR[0][iRow];
409            fTmp1 = kR[1][iRow];
410            kR[0][iRow] = fCos*fTmp0-fSin*fTmp1;
411            kR[1][iRow] = fSin*fTmp0+fCos*fTmp1;
412        }
413
414        // adjust left
415        fY = kA[0][0];
416        fZ = kA[1][0];
417        fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
418        fSin = fZ*fInvLength;
419        fCos = -fY*fInvLength;
420
421        kA[0][0] = fCos*kA[0][0]-fSin*kA[1][0];
422        fTmp0 = kA[0][1];
423        fTmp1 = kA[1][1];
424        kA[0][1] = fCos*fTmp0-fSin*fTmp1;
425        kA[1][1] = fSin*fTmp0+fCos*fTmp1;
426        kA[0][2] = -fSin*kA[1][2];
427        kA[1][2] *= fCos;
428
429        size_t iCol;
430        for (iCol = 0; iCol < 3; iCol++)
431        {
432            fTmp0 = kL[iCol][0];
433            fTmp1 = kL[iCol][1];
434            kL[iCol][0] = fCos*fTmp0-fSin*fTmp1;
435            kL[iCol][1] = fSin*fTmp0+fCos*fTmp1;
436        }
437
438        // adjust right
439        fY = kA[0][1];
440        fZ = kA[0][2];
441        fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
442        fSin = fZ*fInvLength;
443        fCos = -fY*fInvLength;
444
445        kA[0][1] = fCos*kA[0][1]-fSin*kA[0][2];
446        fTmp0 = kA[1][1];
447        fTmp1 = kA[1][2];
448        kA[1][1] = fCos*fTmp0-fSin*fTmp1;
449        kA[1][2] = fSin*fTmp0+fCos*fTmp1;
450        kA[2][1] = -fSin*kA[2][2];
451        kA[2][2] *= fCos;
452
453        for (iRow = 0; iRow < 3; iRow++)
454        {
455            fTmp0 = kR[1][iRow];
456            fTmp1 = kR[2][iRow];
457            kR[1][iRow] = fCos*fTmp0-fSin*fTmp1;
458            kR[2][iRow] = fSin*fTmp0+fCos*fTmp1;
459        }
460
461        // adjust left
462        fY = kA[1][1];
463        fZ = kA[2][1];
464        fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
465        fSin = fZ*fInvLength;
466        fCos = -fY*fInvLength;
467
468        kA[1][1] = fCos*kA[1][1]-fSin*kA[2][1];
469        fTmp0 = kA[1][2];
470        fTmp1 = kA[2][2];
471        kA[1][2] = fCos*fTmp0-fSin*fTmp1;
472        kA[2][2] = fSin*fTmp0+fCos*fTmp1;
473
474        for (iCol = 0; iCol < 3; iCol++)
475        {
476            fTmp0 = kL[iCol][1];
477            fTmp1 = kL[iCol][2];
478            kL[iCol][1] = fCos*fTmp0-fSin*fTmp1;
479            kL[iCol][2] = fSin*fTmp0+fCos*fTmp1;
480        }
481    }
482    //-----------------------------------------------------------------------
483    void Matrix3::SingularValueDecomposition (Matrix3& kL, Vector3& kS,
484        Matrix3& kR) const
485    {
486        // temas: currently unused
487        //const int iMax = 16;
488                size_t iRow, iCol;
489
490        Matrix3 kA = *this;
491        Bidiagonalize(kA,kL,kR);
492
493        for (unsigned int i = 0; i < ms_iSvdMaxIterations; i++)
494        {
495            Real fTmp, fTmp0, fTmp1;
496            Real fSin0, fCos0, fTan0;
497            Real fSin1, fCos1, fTan1;
498
499            bool bTest1 = (Math::Abs(kA[0][1]) <=
500                ms_fSvdEpsilon*(Math::Abs(kA[0][0])+Math::Abs(kA[1][1])));
501            bool bTest2 = (Math::Abs(kA[1][2]) <=
502                ms_fSvdEpsilon*(Math::Abs(kA[1][1])+Math::Abs(kA[2][2])));
503            if ( bTest1 )
504            {
505                if ( bTest2 )
506                {
507                    kS[0] = kA[0][0];
508                    kS[1] = kA[1][1];
509                    kS[2] = kA[2][2];
510                    break;
511                }
512                else
513                {
514                    // 2x2 closed form factorization
515                    fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] +
516                        kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]);
517                    fTan0 = 0.5*(fTmp+Math::Sqrt(fTmp*fTmp + 4.0));
518                    fCos0 = Math::InvSqrt(1.0+fTan0*fTan0);
519                    fSin0 = fTan0*fCos0;
520
521                    for (iCol = 0; iCol < 3; iCol++)
522                    {
523                        fTmp0 = kL[iCol][1];
524                        fTmp1 = kL[iCol][2];
525                        kL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1;
526                        kL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1;
527                    }
528
529                    fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1];
530                    fCos1 = Math::InvSqrt(1.0+fTan1*fTan1);
531                    fSin1 = -fTan1*fCos1;
532
533                    for (iRow = 0; iRow < 3; iRow++)
534                    {
535                        fTmp0 = kR[1][iRow];
536                        fTmp1 = kR[2][iRow];
537                        kR[1][iRow] = fCos1*fTmp0-fSin1*fTmp1;
538                        kR[2][iRow] = fSin1*fTmp0+fCos1*fTmp1;
539                    }
540
541                    kS[0] = kA[0][0];
542                    kS[1] = fCos0*fCos1*kA[1][1] -
543                        fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]);
544                    kS[2] = fSin0*fSin1*kA[1][1] +
545                        fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]);
546                    break;
547                }
548            }
549            else
550            {
551                if ( bTest2 )
552                {
553                    // 2x2 closed form factorization
554                    fTmp = (kA[0][0]*kA[0][0] + kA[1][1]*kA[1][1] -
555                        kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]);
556                    fTan0 = 0.5*(-fTmp+Math::Sqrt(fTmp*fTmp + 4.0));
557                    fCos0 = Math::InvSqrt(1.0+fTan0*fTan0);
558                    fSin0 = fTan0*fCos0;
559
560                    for (iCol = 0; iCol < 3; iCol++)
561                    {
562                        fTmp0 = kL[iCol][0];
563                        fTmp1 = kL[iCol][1];
564                        kL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1;
565                        kL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1;
566                    }
567
568                    fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0];
569                    fCos1 = Math::InvSqrt(1.0+fTan1*fTan1);
570                    fSin1 = -fTan1*fCos1;
571
572                    for (iRow = 0; iRow < 3; iRow++)
573                    {
574                        fTmp0 = kR[0][iRow];
575                        fTmp1 = kR[1][iRow];
576                        kR[0][iRow] = fCos1*fTmp0-fSin1*fTmp1;
577                        kR[1][iRow] = fSin1*fTmp0+fCos1*fTmp1;
578                    }
579
580                    kS[0] = fCos0*fCos1*kA[0][0] -
581                        fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]);
582                    kS[1] = fSin0*fSin1*kA[0][0] +
583                        fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]);
584                    kS[2] = kA[2][2];
585                    break;
586                }
587                else
588                {
589                    GolubKahanStep(kA,kL,kR);
590                }
591            }
592        }
593
594        // positize diagonal
595        for (iRow = 0; iRow < 3; iRow++)
596        {
597            if ( kS[iRow] < 0.0 )
598            {
599                kS[iRow] = -kS[iRow];
600                for (iCol = 0; iCol < 3; iCol++)
601                    kR[iRow][iCol] = -kR[iRow][iCol];
602            }
603        }
604    }
605    //-----------------------------------------------------------------------
606    void Matrix3::SingularValueComposition (const Matrix3& kL,
607        const Vector3& kS, const Matrix3& kR)
608    {
609        size_t iRow, iCol;
610        Matrix3 kTmp;
611
612        // product S*R
613        for (iRow = 0; iRow < 3; iRow++)
614        {
615            for (iCol = 0; iCol < 3; iCol++)
616                kTmp[iRow][iCol] = kS[iRow]*kR[iRow][iCol];
617        }
618
619        // product L*S*R
620        for (iRow = 0; iRow < 3; iRow++)
621        {
622            for (iCol = 0; iCol < 3; iCol++)
623            {
624                m[iRow][iCol] = 0.0;
625                for (int iMid = 0; iMid < 3; iMid++)
626                    m[iRow][iCol] += kL[iRow][iMid]*kTmp[iMid][iCol];
627            }
628        }
629    }
630    //-----------------------------------------------------------------------
631    void Matrix3::Orthonormalize ()
632    {
633        // Algorithm uses Gram-Schmidt orthogonalization.  If 'this' matrix is
634        // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
635        //
636        //   q0 = m0/|m0|
637        //   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
638        //   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
639        //
640        // where |V| indicates length of vector V and A*B indicates dot
641        // product of vectors A and B.
642
643        // compute q0
644        Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0]
645            + m[1][0]*m[1][0] +
646            m[2][0]*m[2][0]);
647
648        m[0][0] *= fInvLength;
649        m[1][0] *= fInvLength;
650        m[2][0] *= fInvLength;
651
652        // compute q1
653        Real fDot0 =
654            m[0][0]*m[0][1] +
655            m[1][0]*m[1][1] +
656            m[2][0]*m[2][1];
657
658        m[0][1] -= fDot0*m[0][0];
659        m[1][1] -= fDot0*m[1][0];
660        m[2][1] -= fDot0*m[2][0];
661
662        fInvLength = Math::InvSqrt(m[0][1]*m[0][1] +
663            m[1][1]*m[1][1] +
664            m[2][1]*m[2][1]);
665
666        m[0][1] *= fInvLength;
667        m[1][1] *= fInvLength;
668        m[2][1] *= fInvLength;
669
670        // compute q2
671        Real fDot1 =
672            m[0][1]*m[0][2] +
673            m[1][1]*m[1][2] +
674            m[2][1]*m[2][2];
675
676        fDot0 =
677            m[0][0]*m[0][2] +
678            m[1][0]*m[1][2] +
679            m[2][0]*m[2][2];
680
681        m[0][2] -= fDot0*m[0][0] + fDot1*m[0][1];
682        m[1][2] -= fDot0*m[1][0] + fDot1*m[1][1];
683        m[2][2] -= fDot0*m[2][0] + fDot1*m[2][1];
684
685        fInvLength = Math::InvSqrt(m[0][2]*m[0][2] +
686            m[1][2]*m[1][2] +
687            m[2][2]*m[2][2]);
688
689        m[0][2] *= fInvLength;
690        m[1][2] *= fInvLength;
691        m[2][2] *= fInvLength;
692    }
693    //-----------------------------------------------------------------------
694    void Matrix3::QDUDecomposition (Matrix3& kQ,
695        Vector3& kD, Vector3& kU) const
696    {
697        // Factor M = QR = QDU where Q is orthogonal, D is diagonal,
698        // and U is upper triangular with ones on its diagonal.  Algorithm uses
699        // Gram-Schmidt orthogonalization (the QR algorithm).
700        //
701        // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
702        //
703        //   q0 = m0/|m0|
704        //   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
705        //   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
706        //
707        // where |V| indicates length of vector V and A*B indicates dot
708        // product of vectors A and B.  The matrix R has entries
709        //
710        //   r00 = q0*m0  r01 = q0*m1  r02 = q0*m2
711        //   r10 = 0      r11 = q1*m1  r12 = q1*m2
712        //   r20 = 0      r21 = 0      r22 = q2*m2
713        //
714        // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
715        // u02 = r02/r00, and u12 = r12/r11.
716
717        // Q = rotation
718        // D = scaling
719        // U = shear
720
721        // D stores the three diagonal entries r00, r11, r22
722        // U stores the entries U[0] = u01, U[1] = u02, U[2] = u12
723
724        // build orthogonal matrix Q
725        Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0]
726            + m[1][0]*m[1][0] +
727            m[2][0]*m[2][0]);
728        kQ[0][0] = m[0][0]*fInvLength;
729        kQ[1][0] = m[1][0]*fInvLength;
730        kQ[2][0] = m[2][0]*fInvLength;
731
732        Real fDot = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] +
733            kQ[2][0]*m[2][1];
734        kQ[0][1] = m[0][1]-fDot*kQ[0][0];
735        kQ[1][1] = m[1][1]-fDot*kQ[1][0];
736        kQ[2][1] = m[2][1]-fDot*kQ[2][0];
737        fInvLength = Math::InvSqrt(kQ[0][1]*kQ[0][1] + kQ[1][1]*kQ[1][1] +
738            kQ[2][1]*kQ[2][1]);
739        kQ[0][1] *= fInvLength;
740        kQ[1][1] *= fInvLength;
741        kQ[2][1] *= fInvLength;
742
743        fDot = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] +
744            kQ[2][0]*m[2][2];
745        kQ[0][2] = m[0][2]-fDot*kQ[0][0];
746        kQ[1][2] = m[1][2]-fDot*kQ[1][0];
747        kQ[2][2] = m[2][2]-fDot*kQ[2][0];
748        fDot = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] +
749            kQ[2][1]*m[2][2];
750        kQ[0][2] -= fDot*kQ[0][1];
751        kQ[1][2] -= fDot*kQ[1][1];
752        kQ[2][2] -= fDot*kQ[2][1];
753        fInvLength = Math::InvSqrt(kQ[0][2]*kQ[0][2] + kQ[1][2]*kQ[1][2] +
754            kQ[2][2]*kQ[2][2]);
755        kQ[0][2] *= fInvLength;
756        kQ[1][2] *= fInvLength;
757        kQ[2][2] *= fInvLength;
758
759        // guarantee that orthogonal matrix has determinant 1 (no reflections)
760        Real fDet = kQ[0][0]*kQ[1][1]*kQ[2][2] + kQ[0][1]*kQ[1][2]*kQ[2][0] +
761            kQ[0][2]*kQ[1][0]*kQ[2][1] - kQ[0][2]*kQ[1][1]*kQ[2][0] -
762            kQ[0][1]*kQ[1][0]*kQ[2][2] - kQ[0][0]*kQ[1][2]*kQ[2][1];
763
764        if ( fDet < 0.0 )
765        {
766            for (size_t iRow = 0; iRow < 3; iRow++)
767                for (size_t iCol = 0; iCol < 3; iCol++)
768                    kQ[iRow][iCol] = -kQ[iRow][iCol];
769        }
770
771        // build "right" matrix R
772        Matrix3 kR;
773        kR[0][0] = kQ[0][0]*m[0][0] + kQ[1][0]*m[1][0] +
774            kQ[2][0]*m[2][0];
775        kR[0][1] = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] +
776            kQ[2][0]*m[2][1];
777        kR[1][1] = kQ[0][1]*m[0][1] + kQ[1][1]*m[1][1] +
778            kQ[2][1]*m[2][1];
779        kR[0][2] = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] +
780            kQ[2][0]*m[2][2];
781        kR[1][2] = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] +
782            kQ[2][1]*m[2][2];
783        kR[2][2] = kQ[0][2]*m[0][2] + kQ[1][2]*m[1][2] +
784            kQ[2][2]*m[2][2];
785
786        // the scaling component
787        kD[0] = kR[0][0];
788        kD[1] = kR[1][1];
789        kD[2] = kR[2][2];
790
791        // the shear component
792        Real fInvD0 = 1.0/kD[0];
793        kU[0] = kR[0][1]*fInvD0;
794        kU[1] = kR[0][2]*fInvD0;
795        kU[2] = kR[1][2]/kD[1];
796    }
797    //-----------------------------------------------------------------------
798    Real Matrix3::MaxCubicRoot (Real afCoeff[3])
799    {
800        // Spectral norm is for A^T*A, so characteristic polynomial
801        // P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive real roots.
802        // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1].
803
804        // quick out for uniform scale (triple root)
805        const Real fOneThird = 1.0/3.0;
806        const Real fEpsilon = 1e-06;
807        Real fDiscr = afCoeff[2]*afCoeff[2] - 3.0*afCoeff[1];
808        if ( fDiscr <= fEpsilon )
809            return -fOneThird*afCoeff[2];
810
811        // Compute an upper bound on roots of P(x).  This assumes that A^T*A
812        // has been scaled by its largest entry.
813        Real fX = 1.0;
814        Real fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
815        if ( fPoly < 0.0 )
816        {
817            // uses a matrix norm to find an upper bound on maximum root
818            fX = Math::Abs(afCoeff[0]);
819            Real fTmp = 1.0+Math::Abs(afCoeff[1]);
820            if ( fTmp > fX )
821                fX = fTmp;
822            fTmp = 1.0+Math::Abs(afCoeff[2]);
823            if ( fTmp > fX )
824                fX = fTmp;
825        }
826
827        // Newton's method to find root
828        Real fTwoC2 = 2.0*afCoeff[2];
829        for (int i = 0; i < 16; i++)
830        {
831            fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
832            if ( Math::Abs(fPoly) <= fEpsilon )
833                return fX;
834
835            Real fDeriv = afCoeff[1]+fX*(fTwoC2+3.0*fX);
836            fX -= fPoly/fDeriv;
837        }
838
839        return fX;
840    }
841    //-----------------------------------------------------------------------
842    Real Matrix3::SpectralNorm () const
843    {
844        Matrix3 kP;
845        size_t iRow, iCol;
846        Real fPmax = 0.0;
847        for (iRow = 0; iRow < 3; iRow++)
848        {
849            for (iCol = 0; iCol < 3; iCol++)
850            {
851                kP[iRow][iCol] = 0.0;
852                for (int iMid = 0; iMid < 3; iMid++)
853                {
854                    kP[iRow][iCol] +=
855                        m[iMid][iRow]*m[iMid][iCol];
856                }
857                if ( kP[iRow][iCol] > fPmax )
858                    fPmax = kP[iRow][iCol];
859            }
860        }
861
862        Real fInvPmax = 1.0/fPmax;
863        for (iRow = 0; iRow < 3; iRow++)
864        {
865            for (iCol = 0; iCol < 3; iCol++)
866                kP[iRow][iCol] *= fInvPmax;
867        }
868
869        Real afCoeff[3];
870        afCoeff[0] = -(kP[0][0]*(kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1]) +
871            kP[0][1]*(kP[2][0]*kP[1][2]-kP[1][0]*kP[2][2]) +
872            kP[0][2]*(kP[1][0]*kP[2][1]-kP[2][0]*kP[1][1]));
873        afCoeff[1] = kP[0][0]*kP[1][1]-kP[0][1]*kP[1][0] +
874            kP[0][0]*kP[2][2]-kP[0][2]*kP[2][0] +
875            kP[1][1]*kP[2][2]-kP[1][2]*kP[2][1];
876        afCoeff[2] = -(kP[0][0]+kP[1][1]+kP[2][2]);
877
878        Real fRoot = MaxCubicRoot(afCoeff);
879        Real fNorm = Math::Sqrt(fPmax*fRoot);
880        return fNorm;
881    }
882    //-----------------------------------------------------------------------
883    void Matrix3::ToAxisAngle (Vector3& rkAxis, Radian& rfRadians) const
884    {
885        // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
886        // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
887        // I is the identity and
888        //
889        //       +-        -+
890        //   P = |  0 -z +y |
891        //       | +z  0 -x |
892        //       | -y +x  0 |
893        //       +-        -+
894        //
895        // If A > 0, R represents a counterclockwise rotation about the axis in
896        // the sense of looking from the tip of the axis vector towards the
897        // origin.  Some algebra will show that
898        //
899        //   cos(A) = (trace(R)-1)/2  and  R - R^t = 2*sin(A)*P
900        //
901        // In the event that A = pi, R-R^t = 0 which prevents us from extracting
902        // the axis through P.  Instead note that R = I+2*P^2 when A = pi, so
903        // P^2 = (R-I)/2.  The diagonal entries of P^2 are x^2-1, y^2-1, and
904        // z^2-1.  We can solve these for axis (x,y,z).  Because the angle is pi,
905        // it does not matter which sign you choose on the square roots.
906
907        Real fTrace = m[0][0] + m[1][1] + m[2][2];
908        Real fCos = 0.5*(fTrace-1.0);
909        rfRadians = Math::ACos(fCos);  // in [0,PI]
910
911        if ( rfRadians > Radian(0.0) )
912        {
913            if ( rfRadians < Radian(Math::PI) )
914            {
915                rkAxis.x = m[2][1]-m[1][2];
916                rkAxis.y = m[0][2]-m[2][0];
917                rkAxis.z = m[1][0]-m[0][1];
918                rkAxis.normalise();
919            }
920            else
921            {
922                // angle is PI
923                float fHalfInverse;
924                if ( m[0][0] >= m[1][1] )
925                {
926                    // r00 >= r11
927                    if ( m[0][0] >= m[2][2] )
928                    {
929                        // r00 is maximum diagonal term
930                        rkAxis.x = 0.5*Math::Sqrt(m[0][0] -
931                            m[1][1] - m[2][2] + 1.0);
932                        fHalfInverse = 0.5/rkAxis.x;
933                        rkAxis.y = fHalfInverse*m[0][1];
934                        rkAxis.z = fHalfInverse*m[0][2];
935                    }
936                    else
937                    {
938                        // r22 is maximum diagonal term
939                        rkAxis.z = 0.5*Math::Sqrt(m[2][2] -
940                            m[0][0] - m[1][1] + 1.0);
941                        fHalfInverse = 0.5/rkAxis.z;
942                        rkAxis.x = fHalfInverse*m[0][2];
943                        rkAxis.y = fHalfInverse*m[1][2];
944                    }
945                }
946                else
947                {
948                    // r11 > r00
949                    if ( m[1][1] >= m[2][2] )
950                    {
951                        // r11 is maximum diagonal term
952                        rkAxis.y = 0.5*Math::Sqrt(m[1][1] -
953                            m[0][0] - m[2][2] + 1.0);
954                        fHalfInverse  = 0.5/rkAxis.y;
955                        rkAxis.x = fHalfInverse*m[0][1];
956                        rkAxis.z = fHalfInverse*m[1][2];
957                    }
958                    else
959                    {
960                        // r22 is maximum diagonal term
961                        rkAxis.z = 0.5*Math::Sqrt(m[2][2] -
962                            m[0][0] - m[1][1] + 1.0);
963                        fHalfInverse = 0.5/rkAxis.z;
964                        rkAxis.x = fHalfInverse*m[0][2];
965                        rkAxis.y = fHalfInverse*m[1][2];
966                    }
967                }
968            }
969        }
970        else
971        {
972            // The angle is 0 and the matrix is the identity.  Any axis will
973            // work, so just use the x-axis.
974            rkAxis.x = 1.0;
975            rkAxis.y = 0.0;
976            rkAxis.z = 0.0;
977        }
978    }
979    //-----------------------------------------------------------------------
980    void Matrix3::FromAxisAngle (const Vector3& rkAxis, const Radian& fRadians)
981    {
982        Real fCos = Math::Cos(fRadians);
983        Real fSin = Math::Sin(fRadians);
984        Real fOneMinusCos = 1.0-fCos;
985        Real fX2 = rkAxis.x*rkAxis.x;
986        Real fY2 = rkAxis.y*rkAxis.y;
987        Real fZ2 = rkAxis.z*rkAxis.z;
988        Real fXYM = rkAxis.x*rkAxis.y*fOneMinusCos;
989        Real fXZM = rkAxis.x*rkAxis.z*fOneMinusCos;
990        Real fYZM = rkAxis.y*rkAxis.z*fOneMinusCos;
991        Real fXSin = rkAxis.x*fSin;
992        Real fYSin = rkAxis.y*fSin;
993        Real fZSin = rkAxis.z*fSin;
994
995        m[0][0] = fX2*fOneMinusCos+fCos;
996        m[0][1] = fXYM-fZSin;
997        m[0][2] = fXZM+fYSin;
998        m[1][0] = fXYM+fZSin;
999        m[1][1] = fY2*fOneMinusCos+fCos;
1000        m[1][2] = fYZM-fXSin;
1001        m[2][0] = fXZM-fYSin;
1002        m[2][1] = fYZM+fXSin;
1003        m[2][2] = fZ2*fOneMinusCos+fCos;
1004    }
1005    //-----------------------------------------------------------------------
1006    bool Matrix3::ToEulerAnglesXYZ (Radian& rfYAngle, Radian& rfPAngle,
1007        Radian& rfRAngle) const
1008    {
1009        // rot =  cy*cz          -cy*sz           sy
1010        //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
1011        //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
1012
1013        rfPAngle = Radian(Math::ASin(m[0][2]));
1014        if ( rfPAngle < Radian(Math::HALF_PI) )
1015        {
1016            if ( rfPAngle > Radian(-Math::HALF_PI) )
1017            {
1018                rfYAngle = Math::ATan2(-m[1][2],m[2][2]);
1019                rfRAngle = Math::ATan2(-m[0][1],m[0][0]);
1020                return true;
1021            }
1022            else
1023            {
1024                // WARNING.  Not a unique solution.
1025                Radian fRmY = Math::ATan2(m[1][0],m[1][1]);
1026                rfRAngle = Radian(0.0);  // any angle works
1027                rfYAngle = rfRAngle - fRmY;
1028                return false;
1029            }
1030        }
1031        else
1032        {
1033            // WARNING.  Not a unique solution.
1034            Radian fRpY = Math::ATan2(m[1][0],m[1][1]);
1035            rfRAngle = Radian(0.0);  // any angle works
1036            rfYAngle = fRpY - rfRAngle;
1037            return false;
1038        }
1039    }
1040    //-----------------------------------------------------------------------
1041    bool Matrix3::ToEulerAnglesXZY (Radian& rfYAngle, Radian& rfPAngle,
1042        Radian& rfRAngle) const
1043    {
1044        // rot =  cy*cz          -sz              cz*sy
1045        //        sx*sy+cx*cy*sz  cx*cz          -cy*sx+cx*sy*sz
1046        //       -cx*sy+cy*sx*sz  cz*sx           cx*cy+sx*sy*sz
1047
1048        rfPAngle = Math::ASin(-m[0][1]);
1049        if ( rfPAngle < Radian(Math::HALF_PI) )
1050        {
1051            if ( rfPAngle > Radian(-Math::HALF_PI) )
1052            {
1053                rfYAngle = Math::ATan2(m[2][1],m[1][1]);
1054                rfRAngle = Math::ATan2(m[0][2],m[0][0]);
1055                return true;
1056            }
1057            else
1058            {
1059                // WARNING.  Not a unique solution.
1060                Radian fRmY = Math::ATan2(-m[2][0],m[2][2]);
1061                rfRAngle = Radian(0.0);  // any angle works
1062                rfYAngle = rfRAngle - fRmY;
1063                return false;
1064            }
1065        }
1066        else
1067        {
1068            // WARNING.  Not a unique solution.
1069            Radian fRpY = Math::ATan2(-m[2][0],m[2][2]);
1070            rfRAngle = Radian(0.0);  // any angle works
1071            rfYAngle = fRpY - rfRAngle;
1072            return false;
1073        }
1074    }
1075    //-----------------------------------------------------------------------
1076    bool Matrix3::ToEulerAnglesYXZ (Radian& rfYAngle, Radian& rfPAngle,
1077        Radian& rfRAngle) const
1078    {
1079        // rot =  cy*cz+sx*sy*sz  cz*sx*sy-cy*sz  cx*sy
1080        //        cx*sz           cx*cz          -sx
1081        //       -cz*sy+cy*sx*sz  cy*cz*sx+sy*sz  cx*cy
1082
1083        rfPAngle = Math::ASin(-m[1][2]);
1084        if ( rfPAngle < Radian(Math::HALF_PI) )
1085        {
1086            if ( rfPAngle > Radian(-Math::HALF_PI) )
1087            {
1088                rfYAngle = Math::ATan2(m[0][2],m[2][2]);
1089                rfRAngle = Math::ATan2(m[1][0],m[1][1]);
1090                return true;
1091            }
1092            else
1093            {
1094                // WARNING.  Not a unique solution.
1095                Radian fRmY = Math::ATan2(-m[0][1],m[0][0]);
1096                rfRAngle = Radian(0.0);  // any angle works
1097                rfYAngle = rfRAngle - fRmY;
1098                return false;
1099            }
1100        }
1101        else
1102        {
1103            // WARNING.  Not a unique solution.
1104            Radian fRpY = Math::ATan2(-m[0][1],m[0][0]);
1105            rfRAngle = Radian(0.0);  // any angle works
1106            rfYAngle = fRpY - rfRAngle;
1107            return false;
1108        }
1109    }
1110    //-----------------------------------------------------------------------
1111    bool Matrix3::ToEulerAnglesYZX (Radian& rfYAngle, Radian& rfPAngle,
1112        Radian& rfRAngle) const
1113    {
1114        // rot =  cy*cz           sx*sy-cx*cy*sz  cx*sy+cy*sx*sz
1115        //        sz              cx*cz          -cz*sx
1116        //       -cz*sy           cy*sx+cx*sy*sz  cx*cy-sx*sy*sz
1117
1118        rfPAngle = Math::ASin(m[1][0]);
1119        if ( rfPAngle < Radian(Math::HALF_PI) )
1120        {
1121            if ( rfPAngle > Radian(-Math::HALF_PI) )
1122            {
1123                rfYAngle = Math::ATan2(-m[2][0],m[0][0]);
1124                rfRAngle = Math::ATan2(-m[1][2],m[1][1]);
1125                return true;
1126            }
1127            else
1128            {
1129                // WARNING.  Not a unique solution.
1130                Radian fRmY = Math::ATan2(m[2][1],m[2][2]);
1131                rfRAngle = Radian(0.0);  // any angle works
1132                rfYAngle = rfRAngle - fRmY;
1133                return false;
1134            }
1135        }
1136        else
1137        {
1138            // WARNING.  Not a unique solution.
1139            Radian fRpY = Math::ATan2(m[2][1],m[2][2]);
1140            rfRAngle = Radian(0.0);  // any angle works
1141            rfYAngle = fRpY - rfRAngle;
1142            return false;
1143        }
1144    }
1145    //-----------------------------------------------------------------------
1146    bool Matrix3::ToEulerAnglesZXY (Radian& rfYAngle, Radian& rfPAngle,
1147        Radian& rfRAngle) const
1148    {
1149        // rot =  cy*cz-sx*sy*sz -cx*sz           cz*sy+cy*sx*sz
1150        //        cz*sx*sy+cy*sz  cx*cz          -cy*cz*sx+sy*sz
1151        //       -cx*sy           sx              cx*cy
1152
1153        rfPAngle = Math::ASin(m[2][1]);
1154        if ( rfPAngle < Radian(Math::HALF_PI) )
1155        {
1156            if ( rfPAngle > Radian(-Math::HALF_PI) )
1157            {
1158                rfYAngle = Math::ATan2(-m[0][1],m[1][1]);
1159                rfRAngle = Math::ATan2(-m[2][0],m[2][2]);
1160                return true;
1161            }
1162            else
1163            {
1164                // WARNING.  Not a unique solution.
1165                Radian fRmY = Math::ATan2(m[0][2],m[0][0]);
1166                rfRAngle = Radian(0.0);  // any angle works
1167                rfYAngle = rfRAngle - fRmY;
1168                return false;
1169            }
1170        }
1171        else
1172        {
1173            // WARNING.  Not a unique solution.
1174            Radian fRpY = Math::ATan2(m[0][2],m[0][0]);
1175            rfRAngle = Radian(0.0);  // any angle works
1176            rfYAngle = fRpY - rfRAngle;
1177            return false;
1178        }
1179    }
1180    //-----------------------------------------------------------------------
1181    bool Matrix3::ToEulerAnglesZYX (Radian& rfYAngle, Radian& rfPAngle,
1182        Radian& rfRAngle) const
1183    {
1184        // rot =  cy*cz           cz*sx*sy-cx*sz  cx*cz*sy+sx*sz
1185        //        cy*sz           cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
1186        //       -sy              cy*sx           cx*cy
1187
1188        rfPAngle = Math::ASin(-m[2][0]);
1189        if ( rfPAngle < Radian(Math::HALF_PI) )
1190        {
1191            if ( rfPAngle > Radian(-Math::HALF_PI) )
1192            {
1193                rfYAngle = Math::ATan2(m[1][0],m[0][0]);
1194                rfRAngle = Math::ATan2(m[2][1],m[2][2]);
1195                return true;
1196            }
1197            else
1198            {
1199                // WARNING.  Not a unique solution.
1200                Radian fRmY = Math::ATan2(-m[0][1],m[0][2]);
1201                rfRAngle = Radian(0.0);  // any angle works
1202                rfYAngle = rfRAngle - fRmY;
1203                return false;
1204            }
1205        }
1206        else
1207        {
1208            // WARNING.  Not a unique solution.
1209            Radian fRpY = Math::ATan2(-m[0][1],m[0][2]);
1210            rfRAngle = Radian(0.0);  // any angle works
1211            rfYAngle = fRpY - rfRAngle;
1212            return false;
1213        }
1214    }
1215    //-----------------------------------------------------------------------
1216    void Matrix3::FromEulerAnglesXYZ (const Radian& fYAngle, const Radian& fPAngle,
1217        const Radian& fRAngle)
1218    {
1219        Real fCos, fSin;
1220
1221        fCos = Math::Cos(fYAngle);
1222        fSin = Math::Sin(fYAngle);
1223        Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
1224
1225        fCos = Math::Cos(fPAngle);
1226        fSin = Math::Sin(fPAngle);
1227        Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
1228
1229        fCos = Math::Cos(fRAngle);
1230        fSin = Math::Sin(fRAngle);
1231        Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
1232
1233        *this = kXMat*(kYMat*kZMat);
1234    }
1235    //-----------------------------------------------------------------------
1236    void Matrix3::FromEulerAnglesXZY (const Radian& fYAngle, const Radian& fPAngle,
1237        const Radian& fRAngle)
1238    {
1239        Real fCos, fSin;
1240
1241        fCos = Math::Cos(fYAngle);
1242        fSin = Math::Sin(fYAngle);
1243        Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
1244
1245        fCos = Math::Cos(fPAngle);
1246        fSin = Math::Sin(fPAngle);
1247        Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
1248
1249        fCos = Math::Cos(fRAngle);
1250        fSin = Math::Sin(fRAngle);
1251        Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
1252
1253        *this = kXMat*(kZMat*kYMat);
1254    }
1255    //-----------------------------------------------------------------------
1256    void Matrix3::FromEulerAnglesYXZ (const Radian& fYAngle, const Radian& fPAngle,
1257        const Radian& fRAngle)
1258    {
1259        Real fCos, fSin;
1260
1261        fCos = Math::Cos(fYAngle);
1262        fSin = Math::Sin(fYAngle);
1263        Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
1264
1265        fCos = Math::Cos(fPAngle);
1266        fSin = Math::Sin(fPAngle);
1267        Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
1268
1269        fCos = Math::Cos(fRAngle);
1270        fSin = Math::Sin(fRAngle);
1271        Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
1272
1273        *this = kYMat*(kXMat*kZMat);
1274    }
1275    //-----------------------------------------------------------------------
1276    void Matrix3::FromEulerAnglesYZX (const Radian& fYAngle, const Radian& fPAngle,
1277        const Radian& fRAngle)
1278    {
1279        Real fCos, fSin;
1280
1281        fCos = Math::Cos(fYAngle);
1282        fSin = Math::Sin(fYAngle);
1283        Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
1284
1285        fCos = Math::Cos(fPAngle);
1286        fSin = Math::Sin(fPAngle);
1287        Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
1288
1289        fCos = Math::Cos(fRAngle);
1290        fSin = Math::Sin(fRAngle);
1291        Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
1292
1293        *this = kYMat*(kZMat*kXMat);
1294    }
1295    //-----------------------------------------------------------------------
1296    void Matrix3::FromEulerAnglesZXY (const Radian& fYAngle, const Radian& fPAngle,
1297        const Radian& fRAngle)
1298    {
1299        Real fCos, fSin;
1300
1301        fCos = Math::Cos(fYAngle);
1302        fSin = Math::Sin(fYAngle);
1303        Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
1304
1305        fCos = Math::Cos(fPAngle);
1306        fSin = Math::Sin(fPAngle);
1307        Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
1308
1309        fCos = Math::Cos(fRAngle);
1310        fSin = Math::Sin(fRAngle);
1311        Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
1312
1313        *this = kZMat*(kXMat*kYMat);
1314    }
1315    //-----------------------------------------------------------------------
1316    void Matrix3::FromEulerAnglesZYX (const Radian& fYAngle, const Radian& fPAngle,
1317        const Radian& fRAngle)
1318    {
1319        Real fCos, fSin;
1320
1321        fCos = Math::Cos(fYAngle);
1322        fSin = Math::Sin(fYAngle);
1323        Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
1324
1325        fCos = Math::Cos(fPAngle);
1326        fSin = Math::Sin(fPAngle);
1327        Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
1328
1329        fCos = Math::Cos(fRAngle);
1330        fSin = Math::Sin(fRAngle);
1331        Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
1332
1333        *this = kZMat*(kYMat*kXMat);
1334    }
1335    //-----------------------------------------------------------------------
1336    void Matrix3::Tridiagonal (Real afDiag[3], Real afSubDiag[3])
1337    {
1338        // Householder reduction T = Q^t M Q
1339        //   Input:
1340        //     mat, symmetric 3x3 matrix M
1341        //   Output:
1342        //     mat, orthogonal matrix Q
1343        //     diag, diagonal entries of T
1344        //     subd, subdiagonal entries of T (T is symmetric)
1345
1346        Real fA = m[0][0];
1347        Real fB = m[0][1];
1348        Real fC = m[0][2];
1349        Real fD = m[1][1];
1350        Real fE = m[1][2];
1351        Real fF = m[2][2];
1352
1353        afDiag[0] = fA;
1354        afSubDiag[2] = 0.0;
1355        if ( Math::Abs(fC) >= EPSILON )
1356        {
1357            Real fLength = Math::Sqrt(fB*fB+fC*fC);
1358            Real fInvLength = 1.0/fLength;
1359            fB *= fInvLength;
1360            fC *= fInvLength;
1361            Real fQ = 2.0*fB*fE+fC*(fF-fD);
1362            afDiag[1] = fD+fC*fQ;
1363            afDiag[2] = fF-fC*fQ;
1364            afSubDiag[0] = fLength;
1365            afSubDiag[1] = fE-fB*fQ;
1366            m[0][0] = 1.0;
1367            m[0][1] = 0.0;
1368            m[0][2] = 0.0;
1369            m[1][0] = 0.0;
1370            m[1][1] = fB;
1371            m[1][2] = fC;
1372            m[2][0] = 0.0;
1373            m[2][1] = fC;
1374            m[2][2] = -fB;
1375        }
1376        else
1377        {
1378            afDiag[1] = fD;
1379            afDiag[2] = fF;
1380            afSubDiag[0] = fB;
1381            afSubDiag[1] = fE;
1382            m[0][0] = 1.0;
1383            m[0][1] = 0.0;
1384            m[0][2] = 0.0;
1385            m[1][0] = 0.0;
1386            m[1][1] = 1.0;
1387            m[1][2] = 0.0;
1388            m[2][0] = 0.0;
1389            m[2][1] = 0.0;
1390            m[2][2] = 1.0;
1391        }
1392    }
1393    //-----------------------------------------------------------------------
1394    bool Matrix3::QLAlgorithm (Real afDiag[3], Real afSubDiag[3])
1395    {
1396        // QL iteration with implicit shifting to reduce matrix from tridiagonal
1397        // to diagonal
1398
1399        for (int i0 = 0; i0 < 3; i0++)
1400        {
1401            const unsigned int iMaxIter = 32;
1402            unsigned int iIter;
1403            for (iIter = 0; iIter < iMaxIter; iIter++)
1404            {
1405                int i1;
1406                for (i1 = i0; i1 <= 1; i1++)
1407                {
1408                    Real fSum = Math::Abs(afDiag[i1]) +
1409                        Math::Abs(afDiag[i1+1]);
1410                    if ( Math::Abs(afSubDiag[i1]) + fSum == fSum )
1411                        break;
1412                }
1413                if ( i1 == i0 )
1414                    break;
1415
1416                Real fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0*afSubDiag[i0]);
1417                Real fTmp1 = Math::Sqrt(fTmp0*fTmp0+1.0);
1418                if ( fTmp0 < 0.0 )
1419                    fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0-fTmp1);
1420                else
1421                    fTmp0 = afDiag[i1]-afDiag[i0]+afSubDiag[i0]/(fTmp0+fTmp1);
1422                Real fSin = 1.0;
1423                Real fCos = 1.0;
1424                Real fTmp2 = 0.0;
1425                for (int i2 = i1-1; i2 >= i0; i2--)
1426                {
1427                    Real fTmp3 = fSin*afSubDiag[i2];
1428                    Real fTmp4 = fCos*afSubDiag[i2];
1429                    if ( Math::Abs(fTmp3) >= Math::Abs(fTmp0) )
1430                    {
1431                        fCos = fTmp0/fTmp3;
1432                        fTmp1 = Math::Sqrt(fCos*fCos+1.0);
1433                        afSubDiag[i2+1] = fTmp3*fTmp1;
1434                        fSin = 1.0/fTmp1;
1435                        fCos *= fSin;
1436                    }
1437                    else
1438                    {
1439                        fSin = fTmp3/fTmp0;
1440                        fTmp1 = Math::Sqrt(fSin*fSin+1.0);
1441                        afSubDiag[i2+1] = fTmp0*fTmp1;
1442                        fCos = 1.0/fTmp1;
1443                        fSin *= fCos;
1444                    }
1445                    fTmp0 = afDiag[i2+1]-fTmp2;
1446                    fTmp1 = (afDiag[i2]-fTmp0)*fSin+2.0*fTmp4*fCos;
1447                    fTmp2 = fSin*fTmp1;
1448                    afDiag[i2+1] = fTmp0+fTmp2;
1449                    fTmp0 = fCos*fTmp1-fTmp4;
1450
1451                    for (int iRow = 0; iRow < 3; iRow++)
1452                    {
1453                        fTmp3 = m[iRow][i2+1];
1454                        m[iRow][i2+1] = fSin*m[iRow][i2] +
1455                            fCos*fTmp3;
1456                        m[iRow][i2] = fCos*m[iRow][i2] -
1457                            fSin*fTmp3;
1458                    }
1459                }
1460                afDiag[i0] -= fTmp2;
1461                afSubDiag[i0] = fTmp0;
1462                afSubDiag[i1] = 0.0;
1463            }
1464
1465            if ( iIter == iMaxIter )
1466            {
1467                // should not get here under normal circumstances
1468                return false;
1469            }
1470        }
1471
1472        return true;
1473    }
1474    //-----------------------------------------------------------------------
1475    void Matrix3::EigenSolveSymmetric (Real afEigenvalue[3],
1476        Vector3 akEigenvector[3]) const
1477    {
1478        Matrix3 kMatrix = *this;
1479        Real afSubDiag[3];
1480        kMatrix.Tridiagonal(afEigenvalue,afSubDiag);
1481        kMatrix.QLAlgorithm(afEigenvalue,afSubDiag);
1482
1483        for (size_t i = 0; i < 3; i++)
1484        {
1485            akEigenvector[i][0] = kMatrix[0][i];
1486            akEigenvector[i][1] = kMatrix[1][i];
1487            akEigenvector[i][2] = kMatrix[2][i];
1488        }
1489
1490        // make eigenvectors form a right--handed system
1491        Vector3 kCross = akEigenvector[1].crossProduct(akEigenvector[2]);
1492        Real fDet = akEigenvector[0].dotProduct(kCross);
1493        if ( fDet < 0.0 )
1494        {
1495            akEigenvector[2][0] = - akEigenvector[2][0];
1496            akEigenvector[2][1] = - akEigenvector[2][1];
1497            akEigenvector[2][2] = - akEigenvector[2][2];
1498        }
1499    }
1500    //-----------------------------------------------------------------------
1501    void Matrix3::TensorProduct (const Vector3& rkU, const Vector3& rkV,
1502        Matrix3& rkProduct)
1503    {
1504        for (size_t iRow = 0; iRow < 3; iRow++)
1505        {
1506            for (size_t iCol = 0; iCol < 3; iCol++)
1507                rkProduct[iRow][iCol] = rkU[iRow]*rkV[iCol];
1508        }
1509    }
1510    //-----------------------------------------------------------------------
1511}
Note: See TracBrowser for help on using the repository browser.