?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
[8901] Implement rage save part of talent 29723 buff and ranks.
[getmangos.git] / dep / src / g3dlite / Matrix3.cpp
blob1d6df3ff7d7866d22437da2e4666097a7df71164
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
20 namespace G3D {
21
22 const float Matrix3::EPSILON = 1e-06f;
23
24 const Matrix3& Matrix3::zero() {
25 static Matrix3 m(0, 0, 0, 0, 0, 0, 0, 0, 0);
26 return m;
27 }
28
29 const Matrix3& Matrix3::identity() {
30 static Matrix3 m(1, 0, 0, 0, 1, 0, 0, 0, 1);
31 return m;
32 }
33
34 // Deprecated.
35 const Matrix3 Matrix3::ZERO(0, 0, 0, 0, 0, 0, 0, 0, 0);
36 const Matrix3 Matrix3::IDENTITY(1, 0, 0, 0, 1, 0, 0, 0, 1);
37
38 const float Matrix3::ms_fSvdEpsilon = 1e-04f;
39 const int Matrix3::ms_iSvdMaxIterations = 32;
40
41 bool 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
53 bool 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 //----------------------------------------------------------------------------
68 Matrix3::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
91 Matrix3::Matrix3 (const float aafEntry[3][3]) {
92 memcpy(elt, aafEntry, 9*sizeof(float));
93 }
94
95 //----------------------------------------------------------------------------
96 Matrix3::Matrix3 (const Matrix3& rkMatrix) {
97 memcpy(elt, rkMatrix.elt, 9*sizeof(float));
98 }
99
100 //----------------------------------------------------------------------------
101 Matrix3::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
110 void 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 //----------------------------------------------------------------------------
128 Vector3 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
134 Vector3 Matrix3::getRow (int iRow) const {
135 return Vector3(elt[iRow][0], elt[iRow][1], elt[iRow][2]);
136 }
137
138 void 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
146 void 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 //----------------------------------------------------------------------------
155 bool 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 //----------------------------------------------------------------------------
167 bool Matrix3::operator!= (const Matrix3& rkMatrix) const {
168 return !operator==(rkMatrix);
169 }
170
171 //----------------------------------------------------------------------------
172 Matrix3 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 //----------------------------------------------------------------------------
186 Matrix3 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 //----------------------------------------------------------------------------
200 Matrix3 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
215 Matrix3& 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
225 Matrix3& 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
235 Matrix3& 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 //----------------------------------------------------------------------------
251 Matrix3 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 //----------------------------------------------------------------------------
264 Matrix3 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 //----------------------------------------------------------------------------
277 Matrix3 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
289 Matrix3 operator* (float fScalar, const Matrix3& rkMatrix) {
290 return (double)fScalar * rkMatrix;
291 }
292
293
294 Matrix3 operator* (int fScalar, const Matrix3& rkMatrix) {
295 return (double)fScalar * rkMatrix;
296 }
297 //----------------------------------------------------------------------------
298 Matrix3 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 //----------------------------------------------------------------------------
311 bool 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 //----------------------------------------------------------------------------
353 Matrix3 Matrix3::inverse (float fTolerance) const {
354 Matrix3 kInverse = Matrix3::zero();
355 inverse(kInverse, fTolerance);
356 return kInverse;
357 }
358
359 //----------------------------------------------------------------------------
360 float 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 //----------------------------------------------------------------------------
377 void 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 //----------------------------------------------------------------------------
484 void 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 //----------------------------------------------------------------------------
615 void 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 //----------------------------------------------------------------------------
722 void 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 //----------------------------------------------------------------------------
745 void 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 //----------------------------------------------------------------------------
808 void 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 //----------------------------------------------------------------------------
922 float 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 //----------------------------------------------------------------------------
973 float 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 //----------------------------------------------------------------------------
1014 void 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 //----------------------------------------------------------------------------
1097 Matrix3 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 //----------------------------------------------------------------------------
1127 bool 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 //----------------------------------------------------------------------------
1156 bool 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 //----------------------------------------------------------------------------
1185 bool 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 //----------------------------------------------------------------------------
1214 bool 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 //----------------------------------------------------------------------------
1243 bool 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 //----------------------------------------------------------------------------
1272 bool 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 //----------------------------------------------------------------------------
1301 Matrix3 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 //----------------------------------------------------------------------------
1321 Matrix3 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 //----------------------------------------------------------------------------
1342 Matrix3 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 //----------------------------------------------------------------------------
1365 Matrix3 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 //----------------------------------------------------------------------------
1388 Matrix3 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 //----------------------------------------------------------------------------
1408 Matrix3 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 //----------------------------------------------------------------------------
1428 void 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 //----------------------------------------------------------------------------
1484 bool 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 //----------------------------------------------------------------------------
1569 void 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 //----------------------------------------------------------------------------
1595 void 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
1613 void 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 //----------------------------------------------------------------------------
1663 void 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 //-----------------------------------------------------------------------------
1676 std::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
MaNGOS is a free, Open Source implementation of a game server compatible with the original World of Warcraft client.