root/trunk/dep/src/g3dlite/Matrix3.cpp @ 2

Revision 2, 52.4 kB (checked in by yumileroy, 17 years ago)

[svn] * Proper SVN structure

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