1//************************************ bs::framework - Copyright 2018 Marko Pintera **************************************//
2//*********** Licensed under the MIT license. See LICENSE.md for full terms. This notice is not to be removed. ***********//
3#include "Math/BsMatrix3.h"
4#include "Math/BsQuaternion.h"
5#include "Math/BsMath.h"
6
7namespace bs
8{
9 const Matrix3 Matrix3::ZERO{BS_ZERO()};
10 const Matrix3 Matrix3::IDENTITY{BS_IDENTITY()};
11
12 Vector3 Matrix3::getColumn(UINT32 col) const
13 {
14 assert(col < 3);
15
16 return Vector3(m[0][col],m[1][col], m[2][col]);
17 }
18
19 void Matrix3::setColumn(UINT32 col, const Vector3& vec)
20 {
21 assert(col < 3);
22
23 m[0][col] = vec.x;
24 m[1][col] = vec.y;
25 m[2][col] = vec.z;
26 }
27
28 void Matrix3::fromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
29 {
30 setColumn(0, xAxis);
31 setColumn(1, yAxis);
32 setColumn(2, zAxis);
33 }
34
35 bool Matrix3::operator== (const Matrix3& rhs) const
36 {
37 for (UINT32 row = 0; row < 3; row++)
38 {
39 for (UINT32 col = 0; col < 3; col++)
40 {
41 if (m[row][col] != rhs.m[row][col])
42 return false;
43 }
44 }
45
46 return true;
47 }
48
49 bool Matrix3::operator!= (const Matrix3& rhs) const
50 {
51 return !operator==(rhs);
52 }
53
54 Matrix3 Matrix3::operator+ (const Matrix3& rhs) const
55 {
56 Matrix3 sum;
57 for (UINT32 row = 0; row < 3; row++)
58 {
59 for (UINT32 col = 0; col < 3; col++)
60 {
61 sum.m[row][col] = m[row][col] + rhs.m[row][col];
62 }
63 }
64
65 return sum;
66 }
67
68 Matrix3 Matrix3::operator- (const Matrix3& rhs) const
69 {
70 Matrix3 diff;
71 for (UINT32 row = 0; row < 3; row++)
72 {
73 for (UINT32 col = 0; col < 3; col++)
74 {
75 diff.m[row][col] = m[row][col] -
76 rhs.m[row][col];
77 }
78 }
79
80 return diff;
81 }
82
83 Matrix3 Matrix3::operator* (const Matrix3& rhs) const
84 {
85 Matrix3 prod;
86 for (UINT32 row = 0; row < 3; row++)
87 {
88 for (UINT32 col = 0; col < 3; col++)
89 {
90 prod.m[row][col] = m[row][0]*rhs.m[0][col] +
91 m[row][1]*rhs.m[1][col] + m[row][2]*rhs.m[2][col];
92 }
93 }
94
95 return prod;
96 }
97
98 Matrix3 Matrix3::operator- () const
99 {
100 Matrix3 neg;
101 for (UINT32 row = 0; row < 3; row++)
102 {
103 for (UINT32 col = 0; col < 3; col++)
104 neg[row][col] = -m[row][col];
105 }
106
107 return neg;
108 }
109
110 Matrix3 Matrix3::operator* (float rhs) const
111 {
112 Matrix3 prod;
113 for (UINT32 row = 0; row < 3; row++)
114 {
115 for (UINT32 col = 0; col < 3; col++)
116 prod[row][col] = rhs*m[row][col];
117 }
118
119 return prod;
120 }
121
122 Matrix3 operator* (float lhs, const Matrix3& rhs)
123 {
124 Matrix3 prod;
125 for (UINT32 row = 0; row < 3; row++)
126 {
127 for (UINT32 col = 0; col < 3; col++)
128 prod[row][col] = lhs*rhs.m[row][col];
129 }
130
131 return prod;
132 }
133
134 Vector3 Matrix3::multiply(const Vector3& vec) const
135 {
136 Vector3 prod;
137 for (UINT32 row = 0; row < 3; row++)
138 {
139 prod[row] =
140 m[row][0]*vec[0] +
141 m[row][1]*vec[1] +
142 m[row][2]*vec[2];
143 }
144
145 return prod;
146 }
147
148 Matrix3 Matrix3::transpose() const
149 {
150 Matrix3 matTranspose;
151 for (UINT32 row = 0; row < 3; row++)
152 {
153 for (UINT32 col = 0; col < 3; col++)
154 matTranspose[row][col] = m[col][row];
155 }
156
157 return matTranspose;
158 }
159
160 bool Matrix3::inverse(Matrix3& matInv, float tolerance) const
161 {
162 matInv[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
163 matInv[0][1] = m[0][2]*m[2][1] - m[0][1]*m[2][2];
164 matInv[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];
165 matInv[1][0] = m[1][2]*m[2][0] - m[1][0]*m[2][2];
166 matInv[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];
167 matInv[1][2] = m[0][2]*m[1][0] - m[0][0]*m[1][2];
168 matInv[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
169 matInv[2][1] = m[0][1]*m[2][0] - m[0][0]*m[2][1];
170 matInv[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
171
172 float det = m[0][0]*matInv[0][0] + m[0][1]*matInv[1][0] + m[0][2]*matInv[2][0];
173
174 if (Math::abs(det) <= tolerance)
175 return false;
176
177 float invDet = 1.0f/det;
178 for (UINT32 row = 0; row < 3; row++)
179 {
180 for (UINT32 col = 0; col < 3; col++)
181 matInv[row][col] *= invDet;
182 }
183
184 return true;
185 }
186
187 Matrix3 Matrix3::inverse(float tolerance) const
188 {
189 Matrix3 matInv = Matrix3::ZERO;
190 inverse(matInv, tolerance);
191 return matInv;
192 }
193
194 float Matrix3::determinant() const
195 {
196 float cofactor00 = m[1][1]*m[2][2] - m[1][2]*m[2][1];
197 float cofactor10 = m[1][2]*m[2][0] - m[1][0]*m[2][2];
198 float cofactor20 = m[1][0]*m[2][1] - m[1][1]*m[2][0];
199
200 float det = m[0][0]*cofactor00 + m[0][1]*cofactor10 + m[0][2]*cofactor20;
201
202 return det;
203 }
204
205 void Matrix3::bidiagonalize (Matrix3& matA, Matrix3& matL, Matrix3& matR)
206 {
207 float v[3], w[3];
208 float length, sign, t1, invT1, t2;
209 bool bIdentity;
210
211 // Map first column to (*,0,0)
212 length = Math::sqrt(matA[0][0]*matA[0][0] + matA[1][0]*matA[1][0] + matA[2][0]*matA[2][0]);
213 if (length > 0.0f)
214 {
215 sign = (matA[0][0] > 0.0f ? 1.0f : -1.0f);
216 t1 = matA[0][0] + sign*length;
217 invT1 = 1.0f/t1;
218 v[1] = matA[1][0]*invT1;
219 v[2] = matA[2][0]*invT1;
220
221 t2 = -2.0f/(1.0f+v[1]*v[1]+v[2]*v[2]);
222 w[0] = t2*(matA[0][0]+matA[1][0]*v[1]+matA[2][0]*v[2]);
223 w[1] = t2*(matA[0][1]+matA[1][1]*v[1]+matA[2][1]*v[2]);
224 w[2] = t2*(matA[0][2]+matA[1][2]*v[1]+matA[2][2]*v[2]);
225 matA[0][0] += w[0];
226 matA[0][1] += w[1];
227 matA[0][2] += w[2];
228 matA[1][1] += v[1]*w[1];
229 matA[1][2] += v[1]*w[2];
230 matA[2][1] += v[2]*w[1];
231 matA[2][2] += v[2]*w[2];
232
233 matL[0][0] = 1.0f+t2;
234 matL[0][1] = matL[1][0] = t2*v[1];
235 matL[0][2] = matL[2][0] = t2*v[2];
236 matL[1][1] = 1.0f+t2*v[1]*v[1];
237 matL[1][2] = matL[2][1] = t2*v[1]*v[2];
238 matL[2][2] = 1.0f+t2*v[2]*v[2];
239 bIdentity = false;
240 }
241 else
242 {
243 matL = Matrix3::IDENTITY;
244 bIdentity = true;
245 }
246
247 // Map first row to (*,*,0)
248 length = Math::sqrt(matA[0][1]*matA[0][1]+matA[0][2]*matA[0][2]);
249 if ( length > 0.0 )
250 {
251 sign = (matA[0][1] > 0.0f ? 1.0f : -1.0f);
252 t1 = matA[0][1] + sign*length;
253 v[2] = matA[0][2]/t1;
254
255 t2 = -2.0f/(1.0f+v[2]*v[2]);
256 w[0] = t2*(matA[0][1]+matA[0][2]*v[2]);
257 w[1] = t2*(matA[1][1]+matA[1][2]*v[2]);
258 w[2] = t2*(matA[2][1]+matA[2][2]*v[2]);
259 matA[0][1] += w[0];
260 matA[1][1] += w[1];
261 matA[1][2] += w[1]*v[2];
262 matA[2][1] += w[2];
263 matA[2][2] += w[2]*v[2];
264
265 matR[0][0] = 1.0;
266 matR[0][1] = matR[1][0] = 0.0;
267 matR[0][2] = matR[2][0] = 0.0;
268 matR[1][1] = 1.0f+t2;
269 matR[1][2] = matR[2][1] = t2*v[2];
270 matR[2][2] = 1.0f+t2*v[2]*v[2];
271 }
272 else
273 {
274 matR = Matrix3::IDENTITY;
275 }
276
277 // Map second column to (*,*,0)
278 length = Math::sqrt(matA[1][1]*matA[1][1]+matA[2][1]*matA[2][1]);
279 if ( length > 0.0 )
280 {
281 sign = (matA[1][1] > 0.0f ? 1.0f : -1.0f);
282 t1 = matA[1][1] + sign*length;
283 v[2] = matA[2][1]/t1;
284
285 t2 = -2.0f/(1.0f+v[2]*v[2]);
286 w[1] = t2*(matA[1][1]+matA[2][1]*v[2]);
287 w[2] = t2*(matA[1][2]+matA[2][2]*v[2]);
288 matA[1][1] += w[1];
289 matA[1][2] += w[2];
290 matA[2][2] += v[2]*w[2];
291
292 float a = 1.0f+t2;
293 float b = t2*v[2];
294 float c = 1.0f+b*v[2];
295
296 if (bIdentity)
297 {
298 matL[0][0] = 1.0;
299 matL[0][1] = matL[1][0] = 0.0;
300 matL[0][2] = matL[2][0] = 0.0;
301 matL[1][1] = a;
302 matL[1][2] = matL[2][1] = b;
303 matL[2][2] = c;
304 }
305 else
306 {
307 for (int row = 0; row < 3; row++)
308 {
309 float tmp0 = matL[row][1];
310 float tmp1 = matL[row][2];
311 matL[row][1] = a*tmp0+b*tmp1;
312 matL[row][2] = b*tmp0+c*tmp1;
313 }
314 }
315 }
316 }
317
318 void Matrix3::golubKahanStep (Matrix3& matA, Matrix3& matL, Matrix3& matR)
319 {
320 float f11 = matA[0][1]*matA[0][1]+matA[1][1]*matA[1][1];
321 float t22 = matA[1][2]*matA[1][2]+matA[2][2]*matA[2][2];
322 float t12 = matA[1][1]*matA[1][2];
323 float trace = f11+t22;
324 float diff = f11-t22;
325 float discr = Math::sqrt(diff*diff+4.0f*t12*t12);
326 float root1 = 0.5f*(trace+discr);
327 float root2 = 0.5f*(trace-discr);
328
329 // Adjust right
330 float y = matA[0][0] - (Math::abs(root1-t22) <= Math::abs(root2-t22) ? root1 : root2);
331 float z = matA[0][1];
332 float invLength = Math::invSqrt(y*y+z*z);
333 float sin = z*invLength;
334 float cos = -y*invLength;
335
336 float tmp0 = matA[0][0];
337 float tmp1 = matA[0][1];
338 matA[0][0] = cos*tmp0-sin*tmp1;
339 matA[0][1] = sin*tmp0+cos*tmp1;
340 matA[1][0] = -sin*matA[1][1];
341 matA[1][1] *= cos;
342
343 UINT32 row;
344 for (row = 0; row < 3; row++)
345 {
346 tmp0 = matR[0][row];
347 tmp1 = matR[1][row];
348 matR[0][row] = cos*tmp0-sin*tmp1;
349 matR[1][row] = sin*tmp0+cos*tmp1;
350 }
351
352 // Adjust left
353 y = matA[0][0];
354 z = matA[1][0];
355 invLength = Math::invSqrt(y*y+z*z);
356 sin = z*invLength;
357 cos = -y*invLength;
358
359 matA[0][0] = cos*matA[0][0]-sin*matA[1][0];
360 tmp0 = matA[0][1];
361 tmp1 = matA[1][1];
362 matA[0][1] = cos*tmp0-sin*tmp1;
363 matA[1][1] = sin*tmp0+cos*tmp1;
364 matA[0][2] = -sin*matA[1][2];
365 matA[1][2] *= cos;
366
367 UINT32 col;
368 for (col = 0; col < 3; col++)
369 {
370 tmp0 = matL[col][0];
371 tmp1 = matL[col][1];
372 matL[col][0] = cos*tmp0-sin*tmp1;
373 matL[col][1] = sin*tmp0+cos*tmp1;
374 }
375
376 // Adjust right
377 y = matA[0][1];
378 z = matA[0][2];
379 invLength = Math::invSqrt(y*y+z*z);
380 sin = z*invLength;
381 cos = -y*invLength;
382
383 matA[0][1] = cos*matA[0][1]-sin*matA[0][2];
384 tmp0 = matA[1][1];
385 tmp1 = matA[1][2];
386 matA[1][1] = cos*tmp0-sin*tmp1;
387 matA[1][2] = sin*tmp0+cos*tmp1;
388 matA[2][1] = -sin*matA[2][2];
389 matA[2][2] *= cos;
390
391 for (row = 0; row < 3; row++)
392 {
393 tmp0 = matR[1][row];
394 tmp1 = matR[2][row];
395 matR[1][row] = cos*tmp0-sin*tmp1;
396 matR[2][row] = sin*tmp0+cos*tmp1;
397 }
398
399 // Adjust left
400 y = matA[1][1];
401 z = matA[2][1];
402 invLength = Math::invSqrt(y*y+z*z);
403 sin = z*invLength;
404 cos = -y*invLength;
405
406 matA[1][1] = cos*matA[1][1]-sin*matA[2][1];
407 tmp0 = matA[1][2];
408 tmp1 = matA[2][2];
409 matA[1][2] = cos*tmp0-sin*tmp1;
410 matA[2][2] = sin*tmp0+cos*tmp1;
411
412 for (col = 0; col < 3; col++)
413 {
414 tmp0 = matL[col][1];
415 tmp1 = matL[col][2];
416 matL[col][1] = cos*tmp0-sin*tmp1;
417 matL[col][2] = sin*tmp0+cos*tmp1;
418 }
419 }
420
421 void Matrix3::singularValueDecomposition(Matrix3& matL, Vector3& matS, Matrix3& matR) const
422 {
423 UINT32 row, col;
424
425 Matrix3 mat = *this;
426 bidiagonalize(mat, matL, matR);
427
428 for (unsigned int i = 0; i < SVD_MAX_ITERS; i++)
429 {
430 float tmp, tmp0, tmp1;
431 float sin0, cos0, tan0;
432 float sin1, cos1, tan1;
433
434 bool test1 = (Math::abs(mat[0][1]) <= SVD_EPSILON*(Math::abs(mat[0][0])+Math::abs(mat[1][1])));
435 bool test2 = (Math::abs(mat[1][2]) <= SVD_EPSILON*(Math::abs(mat[1][1])+Math::abs(mat[2][2])));
436
437 if (test1)
438 {
439 if (test2)
440 {
441 matS[0] = mat[0][0];
442 matS[1] = mat[1][1];
443 matS[2] = mat[2][2];
444 break;
445 }
446 else
447 {
448 // 2x2 closed form factorization
449 tmp = (mat[1][1]*mat[1][1] - mat[2][2]*mat[2][2] + mat[1][2]*mat[1][2])/(mat[1][2]*mat[2][2]);
450 tan0 = 0.5f*(tmp+Math::sqrt(tmp*tmp + 4.0f));
451 cos0 = Math::invSqrt(1.0f+tan0*tan0);
452 sin0 = tan0*cos0;
453
454 for (col = 0; col < 3; col++)
455 {
456 tmp0 = matL[col][1];
457 tmp1 = matL[col][2];
458 matL[col][1] = cos0*tmp0-sin0*tmp1;
459 matL[col][2] = sin0*tmp0+cos0*tmp1;
460 }
461
462 tan1 = (mat[1][2]-mat[2][2]*tan0)/mat[1][1];
463 cos1 = Math::invSqrt(1.0f+tan1*tan1);
464 sin1 = -tan1*cos1;
465
466 for (row = 0; row < 3; row++)
467 {
468 tmp0 = matR[1][row];
469 tmp1 = matR[2][row];
470 matR[1][row] = cos1*tmp0-sin1*tmp1;
471 matR[2][row] = sin1*tmp0+cos1*tmp1;
472 }
473
474 matS[0] = mat[0][0];
475 matS[1] = cos0*cos1*mat[1][1] - sin1*(cos0*mat[1][2]-sin0*mat[2][2]);
476 matS[2] = sin0*sin1*mat[1][1] + cos1*(sin0*mat[1][2]+cos0*mat[2][2]);
477 break;
478 }
479 }
480 else
481 {
482 if (test2)
483 {
484 // 2x2 closed form factorization
485 tmp = (mat[0][0]*mat[0][0] + mat[1][1]*mat[1][1] - mat[0][1]*mat[0][1])/(mat[0][1]*mat[1][1]);
486 tan0 = 0.5f*(-tmp+Math::sqrt(tmp*tmp + 4.0f));
487 cos0 = Math::invSqrt(1.0f+tan0*tan0);
488 sin0 = tan0*cos0;
489
490 for (col = 0; col < 3; col++)
491 {
492 tmp0 = matL[col][0];
493 tmp1 = matL[col][1];
494 matL[col][0] = cos0*tmp0-sin0*tmp1;
495 matL[col][1] = sin0*tmp0+cos0*tmp1;
496 }
497
498 tan1 = (mat[0][1]-mat[1][1]*tan0)/mat[0][0];
499 cos1 = Math::invSqrt(1.0f+tan1*tan1);
500 sin1 = -tan1*cos1;
501
502 for (row = 0; row < 3; row++)
503 {
504 tmp0 = matR[0][row];
505 tmp1 = matR[1][row];
506 matR[0][row] = cos1*tmp0-sin1*tmp1;
507 matR[1][row] = sin1*tmp0+cos1*tmp1;
508 }
509
510 matS[0] = cos0*cos1*mat[0][0] - sin1*(cos0*mat[0][1]-sin0*mat[1][1]);
511 matS[1] = sin0*sin1*mat[0][0] + cos1*(sin0*mat[0][1]+cos0*mat[1][1]);
512 matS[2] = mat[2][2];
513 break;
514 }
515 else
516 {
517 golubKahanStep(mat, matL, matR);
518 }
519 }
520 }
521
522 // Positize diagonal
523 for (row = 0; row < 3; row++)
524 {
525 if ( matS[row] < 0.0 )
526 {
527 matS[row] = -matS[row];
528 for (col = 0; col < 3; col++)
529 matR[row][col] = -matR[row][col];
530 }
531 }
532 }
533
534 void Matrix3::orthonormalize()
535 {
536 // Compute q0
537 float invLength = Math::invSqrt(m[0][0]*m[0][0]+ m[1][0]*m[1][0] + m[2][0]*m[2][0]);
538
539 m[0][0] *= invLength;
540 m[1][0] *= invLength;
541 m[2][0] *= invLength;
542
543 // Compute q1
544 float dot0 = m[0][0]*m[0][1] + m[1][0]*m[1][1] + m[2][0]*m[2][1];
545
546 m[0][1] -= dot0*m[0][0];
547 m[1][1] -= dot0*m[1][0];
548 m[2][1] -= dot0*m[2][0];
549
550 invLength = Math::invSqrt(m[0][1]*m[0][1] + m[1][1]*m[1][1] + m[2][1]*m[2][1]);
551
552 m[0][1] *= invLength;
553 m[1][1] *= invLength;
554 m[2][1] *= invLength;
555
556 // Compute q2
557 float dot1 = m[0][1]*m[0][2] + m[1][1]*m[1][2] + m[2][1]*m[2][2];
558 dot0 = m[0][0]*m[0][2] + m[1][0]*m[1][2] + m[2][0]*m[2][2];
559
560 m[0][2] -= dot0*m[0][0] + dot1*m[0][1];
561 m[1][2] -= dot0*m[1][0] + dot1*m[1][1];
562 m[2][2] -= dot0*m[2][0] + dot1*m[2][1];
563
564 invLength = Math::invSqrt(m[0][2]*m[0][2] + m[1][2]*m[1][2] + m[2][2]*m[2][2]);
565
566 m[0][2] *= invLength;
567 m[1][2] *= invLength;
568 m[2][2] *= invLength;
569 }
570
571 void Matrix3::decomposition(Quaternion& rotation, Vector3& scale) const
572 {
573 Matrix3 matQ;
574 Vector3 vecU;
575 QDUDecomposition(matQ, scale, vecU);
576
577 rotation = Quaternion(matQ);
578 }
579
580 void Matrix3::QDUDecomposition(Matrix3& matQ, Vector3& vecD, Vector3& vecU) const
581 {
582 // Build orthogonal matrix Q
583 float invLength = Math::invSqrt(m[0][0]*m[0][0] + m[1][0]*m[1][0] + m[2][0]*m[2][0]);
584 matQ[0][0] = m[0][0]*invLength;
585 matQ[1][0] = m[1][0]*invLength;
586 matQ[2][0] = m[2][0]*invLength;
587
588 float dot = matQ[0][0]*m[0][1] + matQ[1][0]*m[1][1] + matQ[2][0]*m[2][1];
589 matQ[0][1] = m[0][1]-dot*matQ[0][0];
590 matQ[1][1] = m[1][1]-dot*matQ[1][0];
591 matQ[2][1] = m[2][1]-dot*matQ[2][0];
592
593 invLength = Math::invSqrt(matQ[0][1]*matQ[0][1] + matQ[1][1]*matQ[1][1] + matQ[2][1]*matQ[2][1]);
594 matQ[0][1] *= invLength;
595 matQ[1][1] *= invLength;
596 matQ[2][1] *= invLength;
597
598 dot = matQ[0][0]*m[0][2] + matQ[1][0]*m[1][2] + matQ[2][0]*m[2][2];
599 matQ[0][2] = m[0][2]-dot*matQ[0][0];
600 matQ[1][2] = m[1][2]-dot*matQ[1][0];
601 matQ[2][2] = m[2][2]-dot*matQ[2][0];
602
603 dot = matQ[0][1]*m[0][2] + matQ[1][1]*m[1][2] + matQ[2][1]*m[2][2];
604 matQ[0][2] -= dot*matQ[0][1];
605 matQ[1][2] -= dot*matQ[1][1];
606 matQ[2][2] -= dot*matQ[2][1];
607
608 invLength = Math::invSqrt(matQ[0][2]*matQ[0][2] + matQ[1][2]*matQ[1][2] + matQ[2][2]*matQ[2][2]);
609 matQ[0][2] *= invLength;
610 matQ[1][2] *= invLength;
611 matQ[2][2] *= invLength;
612
613 // Guarantee that orthogonal matrix has determinant 1 (no reflections)
614 float fDet = matQ[0][0]*matQ[1][1]*matQ[2][2] + matQ[0][1]*matQ[1][2]*matQ[2][0] +
615 matQ[0][2]*matQ[1][0]*matQ[2][1] - matQ[0][2]*matQ[1][1]*matQ[2][0] -
616 matQ[0][1]*matQ[1][0]*matQ[2][2] - matQ[0][0]*matQ[1][2]*matQ[2][1];
617
618 if (fDet < 0.0f)
619 {
620 for (UINT32 row = 0; row < 3; row++)
621 for (UINT32 col = 0; col < 3; col++)
622 matQ[row][col] = -matQ[row][col];
623 }
624
625 // Build "right" matrix R
626 Matrix3 matRight;
627 matRight[0][0] = matQ[0][0]*m[0][0] + matQ[1][0]*m[1][0] +
628 matQ[2][0]*m[2][0];
629 matRight[0][1] = matQ[0][0]*m[0][1] + matQ[1][0]*m[1][1] +
630 matQ[2][0]*m[2][1];
631 matRight[1][1] = matQ[0][1]*m[0][1] + matQ[1][1]*m[1][1] +
632 matQ[2][1]*m[2][1];
633 matRight[0][2] = matQ[0][0]*m[0][2] + matQ[1][0]*m[1][2] +
634 matQ[2][0]*m[2][2];
635 matRight[1][2] = matQ[0][1]*m[0][2] + matQ[1][1]*m[1][2] +
636 matQ[2][1]*m[2][2];
637 matRight[2][2] = matQ[0][2]*m[0][2] + matQ[1][2]*m[1][2] +
638 matQ[2][2]*m[2][2];
639
640 // The scaling component
641 vecD[0] = matRight[0][0];
642 vecD[1] = matRight[1][1];
643 vecD[2] = matRight[2][2];
644
645 // The shear component
646 float invD0 = 1.0f/vecD[0];
647 vecU[0] = matRight[0][1]*invD0;
648 vecU[1] = matRight[0][2]*invD0;
649 vecU[2] = matRight[1][2]/vecD[1];
650 }
651
652 void Matrix3::toAxisAngle(Vector3& axis, Radian& radians) const
653 {
654 float trace = m[0][0] + m[1][1] + m[2][2];
655 float cos = 0.5f*(trace-1.0f);
656 radians = Math::acos(cos); // In [0, PI]
657
658 if (radians > Radian(0.0f))
659 {
660 if (radians < Radian(Math::PI))
661 {
662 axis.x = m[2][1]-m[1][2];
663 axis.y = m[0][2]-m[2][0];
664 axis.z = m[1][0]-m[0][1];
665 axis.normalize();
666 }
667 else
668 {
669 // Angle is PI
670 float fHalfInverse;
671 if (m[0][0] >= m[1][1])
672 {
673 // r00 >= r11
674 if (m[0][0] >= m[2][2])
675 {
676 // r00 is maximum diagonal term
677 axis.x = 0.5f*Math::sqrt(m[0][0] - m[1][1] - m[2][2] + 1.0f);
678 fHalfInverse = 0.5f/axis.x;
679 axis.y = fHalfInverse*m[0][1];
680 axis.z = fHalfInverse*m[0][2];
681 }
682 else
683 {
684 // r22 is maximum diagonal term
685 axis.z = 0.5f*Math::sqrt(m[2][2] - m[0][0] - m[1][1] + 1.0f);
686 fHalfInverse = 0.5f/axis.z;
687 axis.x = fHalfInverse*m[0][2];
688 axis.y = fHalfInverse*m[1][2];
689 }
690 }
691 else
692 {
693 // r11 > r00
694 if ( m[1][1] >= m[2][2] )
695 {
696 // r11 is maximum diagonal term
697 axis.y = 0.5f*Math::sqrt(m[1][1] - m[0][0] - m[2][2] + 1.0f);
698 fHalfInverse = 0.5f/axis.y;
699 axis.x = fHalfInverse*m[0][1];
700 axis.z = fHalfInverse*m[1][2];
701 }
702 else
703 {
704 // r22 is maximum diagonal term
705 axis.z = 0.5f*Math::sqrt(m[2][2] - m[0][0] - m[1][1] + 1.0f);
706 fHalfInverse = 0.5f/axis.z;
707 axis.x = fHalfInverse*m[0][2];
708 axis.y = fHalfInverse*m[1][2];
709 }
710 }
711 }
712 }
713 else
714 {
715 // The angle is 0 and the matrix is the identity. Any axis will
716 // work, so just use the x-axis.
717 axis.x = 1.0f;
718 axis.y = 0.0f;
719 axis.z = 0.0f;
720 }
721 }
722
723 void Matrix3::fromAxisAngle(const Vector3& axis, const Radian& angle)
724 {
725 float cos = Math::cos(angle);
726 float sin = Math::sin(angle);
727 float oneMinusCos = 1.0f-cos;
728 float x2 = axis.x*axis.x;
729 float y2 = axis.y*axis.y;
730 float z2 = axis.z*axis.z;
731 float xym = axis.x*axis.y*oneMinusCos;
732 float xzm = axis.x*axis.z*oneMinusCos;
733 float yzm = axis.y*axis.z*oneMinusCos;
734 float xSin = axis.x*sin;
735 float ySin = axis.y*sin;
736 float zSin = axis.z*sin;
737
738 m[0][0] = x2*oneMinusCos+cos;
739 m[0][1] = xym-zSin;
740 m[0][2] = xzm+ySin;
741 m[1][0] = xym+zSin;
742 m[1][1] = y2*oneMinusCos+cos;
743 m[1][2] = yzm-xSin;
744 m[2][0] = xzm-ySin;
745 m[2][1] = yzm+xSin;
746 m[2][2] = z2*oneMinusCos+cos;
747 }
748
749 void Matrix3::toQuaternion(Quaternion& quat) const
750 {
751 quat.fromRotationMatrix(*this);
752 }
753
754 void Matrix3::fromQuaternion(const Quaternion& quat)
755 {
756 quat.toRotationMatrix(*this);
757 }
758
759 bool Matrix3::toEulerAngles(Radian& xAngle, Radian& yAngle, Radian& zAngle) const
760 {
761 float m21 = m[2][1];
762 if (m21 < 1)
763 {
764 if (m21 > -1)
765 {
766 xAngle = Radian(Math::asin(m21));
767 yAngle = Math::atan2(-m[2][0], m[2][2]);
768 zAngle = Math::atan2(-m[0][1], m[1][1]);
769
770 return true;
771 }
772 else
773 {
774 // Note: Not an unique solution.
775 xAngle = Radian(-Math::HALF_PI);
776 yAngle = Radian(0.0f);
777 zAngle = -Math::atan2(m[0][2], m[0][0]);
778
779 return false;
780 }
781 }
782 else
783 {
784 // Note: Not an unique solution.
785 xAngle = Radian(Math::HALF_PI);
786 yAngle = Radian(0.0f);
787 zAngle = Math::atan2(m[0][2], m[0][0]);
788
789 return false;
790 }
791 }
792
793 void Matrix3::fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle)
794 {
795 float cx = Math::cos(xAngle);
796 float sx = Math::sin(xAngle);
797
798 float cy = Math::cos(yAngle);
799 float sy = Math::sin(yAngle);
800
801 float cz = Math::cos(zAngle);
802 float sz = Math::sin(zAngle);
803
804 m[0][0] = cy * cz - sx * sy * sz;
805 m[0][1] = -cx * sz;
806 m[0][2] = cz * sy + cy * sx * sz;
807
808 m[1][0] = cz * sx * sy + cy * sz;
809 m[1][1] = cx * cz;
810 m[1][2] = -cy * cz * sx + sy * sz;
811
812 m[2][0] = -cx * sy;
813 m[2][1] = sx;
814 m[2][2] = cx * cy;
815 }
816
817 void Matrix3::fromEulerAngles(const Radian& xAngle, const Radian& yAngle, const Radian& zAngle, EulerAngleOrder order)
818 {
819 // Euler angle conversions
820 static constexpr const EulerAngleOrderData EA_LOOKUP[6] =
821 { { 0, 1, 2, 1.0f}, { 0, 2, 1, -1.0f}, { 1, 0, 2, -1.0f},
822 { 1, 2, 0, 1.0f}, { 2, 0, 1, 1.0f}, { 2, 1, 0, -1.0f} };
823
824 const EulerAngleOrderData& l = EA_LOOKUP[(int)order];
825
826 Matrix3 mats[3];
827 float cx = Math::cos(xAngle);
828 float sx = Math::sin(xAngle);
829 mats[0] = Matrix3(
830 1.0f, 0.0f, 0.0f,
831 0.0f, cx, -sx,
832 0.0f, sx, cx);
833
834 float cy = Math::cos(yAngle);
835 float sy = Math::sin(yAngle);
836 mats[1] = Matrix3(
837 cy, 0.0f, sy,
838 0.0f, 1.0f, 0.0f,
839 -sy, 0.0f, cy);
840
841 float cz = Math::cos(zAngle);
842 float sz = Math::sin(zAngle);
843 mats[2] = Matrix3(
844 cz, -sz, 0.0f,
845 sz, cz, 0.0f,
846 0.0f, 0.0f, 1.0f);
847
848 *this = mats[l.c]*(mats[l.b]*mats[l.a]);
849 }
850
851 void Matrix3::tridiagonal(float diag[3], float subDiag[3])
852 {
853 // Householder reduction T = Q^t M Q
854 // Input:
855 // mat, symmetric 3x3 matrix M
856 // Output:
857 // mat, orthogonal matrix Q
858 // diag, diagonal entries of T
859 // subd, subdiagonal entries of T (T is symmetric)
860
861 float fA = m[0][0];
862 float fB = m[0][1];
863 float fC = m[0][2];
864 float fD = m[1][1];
865 float fE = m[1][2];
866 float fF = m[2][2];
867
868 diag[0] = fA;
869 subDiag[2] = 0.0;
870 if (Math::abs(fC) >= EPSILON)
871 {
872 float length = Math::sqrt(fB*fB+fC*fC);
873 float invLength = 1.0f/length;
874 fB *= invLength;
875 fC *= invLength;
876 float fQ = 2.0f*fB*fE+fC*(fF-fD);
877 diag[1] = fD+fC*fQ;
878 diag[2] = fF-fC*fQ;
879 subDiag[0] = length;
880 subDiag[1] = fE-fB*fQ;
881 m[0][0] = 1.0;
882 m[0][1] = 0.0;
883 m[0][2] = 0.0;
884 m[1][0] = 0.0;
885 m[1][1] = fB;
886 m[1][2] = fC;
887 m[2][0] = 0.0;
888 m[2][1] = fC;
889 m[2][2] = -fB;
890 }
891 else
892 {
893 diag[1] = fD;
894 diag[2] = fF;
895 subDiag[0] = fB;
896 subDiag[1] = fE;
897 m[0][0] = 1.0;
898 m[0][1] = 0.0;
899 m[0][2] = 0.0;
900 m[1][0] = 0.0;
901 m[1][1] = 1.0;
902 m[1][2] = 0.0;
903 m[2][0] = 0.0;
904 m[2][1] = 0.0;
905 m[2][2] = 1.0;
906 }
907 }
908
909 bool Matrix3::QLAlgorithm(float diag[3], float subDiag[3])
910 {
911 // QL iteration with implicit shifting to reduce matrix from tridiagonal to diagonal
912
913 for (int i = 0; i < 3; i++)
914 {
915 const unsigned int maxIter = 32;
916 unsigned int iter;
917 for (iter = 0; iter < maxIter; iter++)
918 {
919 int j;
920 for (j = i; j <= 1; j++)
921 {
922 float sum = Math::abs(diag[j]) + Math::abs(diag[j+1]);
923
924 if (Math::abs(subDiag[j]) + sum == sum)
925 break;
926 }
927
928 if (j == i)
929 break;
930
931 float tmp0 = (diag[i+1]-diag[i])/(2.0f*subDiag[i]);
932 float tmp1 = Math::sqrt(tmp0*tmp0+1.0f);
933
934 if (tmp0 < 0.0f)
935 tmp0 = diag[j]-diag[i]+subDiag[i]/(tmp0-tmp1);
936 else
937 tmp0 = diag[j]-diag[i]+subDiag[i]/(tmp0+tmp1);
938
939 float sin = 1.0f;
940 float cos = 1.0f;
941 float tmp2 = 0.0f;
942 for (int k = j-1; k >= i; k--)
943 {
944 float tmp3 = sin*subDiag[k];
945 float tmp4 = cos*subDiag[k];
946
947 if (Math::abs(tmp3) >= Math::abs(tmp0))
948 {
949 cos = tmp0/tmp3;
950 tmp1 = Math::sqrt(cos*cos+1.0f);
951 subDiag[k+1] = tmp3*tmp1;
952 sin = 1.0f/tmp1;
953 cos *= sin;
954 }
955 else
956 {
957 sin = tmp3/tmp0;
958 tmp1 = Math::sqrt(sin*sin+1.0f);
959 subDiag[k+1] = tmp0*tmp1;
960 cos = 1.0f/tmp1;
961 sin *= cos;
962 }
963
964 tmp0 = diag[k+1]-tmp2;
965 tmp1 = (diag[k]-tmp0)*sin+2.0f*tmp4*cos;
966 tmp2 = sin*tmp1;
967 diag[k+1] = tmp0+tmp2;
968 tmp0 = cos*tmp1-tmp4;
969
970 for (int row = 0; row < 3; row++)
971 {
972 tmp3 = m[row][k+1];
973 m[row][k+1] = sin*m[row][k] + cos*tmp3;
974 m[row][k] = cos*m[row][k] - sin*tmp3;
975 }
976 }
977
978 diag[i] -= tmp2;
979 subDiag[i] = tmp0;
980 subDiag[j] = 0.0;
981 }
982
983 if (iter == maxIter)
984 {
985 // Should not get here under normal circumstances
986 return false;
987 }
988 }
989
990 return true;
991 }
992
993 void Matrix3::eigenSolveSymmetric(float eigenValues[3], Vector3 eigenVectors[3]) const
994 {
995 Matrix3 mat = *this;
996 float subDiag[3];
997 mat.tridiagonal(eigenValues, subDiag);
998 mat.QLAlgorithm(eigenValues, subDiag);
999
1000 for (UINT32 i = 0; i < 3; i++)
1001 {
1002 eigenVectors[i][0] = mat[0][i];
1003 eigenVectors[i][1] = mat[1][i];
1004 eigenVectors[i][2] = mat[2][i];
1005 }
1006
1007 // Make eigenvectors form a right--handed system
1008 Vector3 cross = eigenVectors[1].cross(eigenVectors[2]);
1009 float det = eigenVectors[0].dot(cross);
1010 if (det < 0.0f)
1011 {
1012 eigenVectors[2][0] = -eigenVectors[2][0];
1013 eigenVectors[2][1] = -eigenVectors[2][1];
1014 eigenVectors[2][2] = -eigenVectors[2][2];
1015 }
1016 }
1017}
1018