Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: downloads/OgreMain/src/OgreMatrix3.cpp @ 3

Last change on this file since 3 was 3, checked in by anonymous, 17 years ago

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