source: OGRE/trunk/ogrenew/OgreMain/src/OgreMatrix3.cpp @ 692

Revision 692, 52.4 KB checked in by mattausch, 19 years ago (diff)

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