libmoldeo (Moldeo 1.0 Core)  1.0
libmoldeo is the group of objects and functions that executes the basic operations of Moldeo 1.0 Platform.
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
moMathMatrix.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 
3  moMathMatrix.h
4 
5  ****************************************************************************
6  * *
7  * This source is free software; you can redistribute it and/or modify *
8  * it under the terms of the GNU General Public License as published by *
9  * the Free Software Foundation; either version 2 of the License, or *
10  * (at your option) any later version. *
11  * *
12  * This code is distributed in the hope that it will be useful, but *
13  * WITHOUT ANY WARRANTY; without even the implied warranty of *
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
15  * General Public License for more details. *
16  * *
17  * A copy of the GNU General Public License is available on the World *
18  * Wide Web at <http://www.gnu.org/copyleft/gpl.html>. You can also *
19  * obtain it by writing to the Free Software Foundation, *
20  * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
21  * *
22  ****************************************************************************
23 
24  Copyright(C) 2006 Fabricio Costa
25 
26  Authors:
27  Fabricio Costa
28  Andrés Colubri
29 
30  Portions taken from
31  Wild Magic Source Code
32  David Eberly
33  http://www.geometrictools.com
34  Copyright (c) 1998-2007
35 
36 *******************************************************************************/
37 
38 #include "moMathMatrix.h"
39 
40 #include <moArray.h>
41 moDefineDynamicArray( moMatrix2fArray )
42 moDefineDynamicArray( moMatrix2dArray )
43 moDefineDynamicArray( moMatrix3fArray )
44 moDefineDynamicArray( moMatrix3dArray )
45 moDefineDynamicArray( moMatrix4fArray )
46 moDefineDynamicArray( moMatrix4dArray )
47 
48 // momoMatrix2 class ------------------------------------------------------------
49 /*
50 template <class Real>
51 moMatrix2<Real>::moMatrix2 (bool bZero)
52 {
53  if (bZero)
54  {
55  MakeZero();
56  }
57  else
58  {
59  MakeIdentity();
60  }
61 }
62 
63 template <class Real>
64 moMatrix2<Real>::moMatrix2 (const moMatrix2& rkM) : moAbstract(rkM)
65 {
66  m_bInitialized = rkM.m_bInitialized;
67  m_afEntry[0] = rkM.m_afEntry[0];
68  m_afEntry[1] = rkM.m_afEntry[1];
69  m_afEntry[2] = rkM.m_afEntry[2];
70  m_afEntry[3] = rkM.m_afEntry[3];
71 }
72 
73 template <class Real>
74 moMatrix2<Real>::moMatrix2 (Real fM00, Real fM01, Real fM10, Real fM11)
75 {
76  m_afEntry[0] = fM00;
77  m_afEntry[1] = fM01;
78  m_afEntry[2] = fM10;
79  m_afEntry[3] = fM11;
80 }
81 
82 template <class Real>
83 moMatrix2<Real>::moMatrix2 (const Real afEntry[4], bool bRowMajor)
84 {
85  if (bRowMajor)
86  {
87  m_afEntry[0] = afEntry[0];
88  m_afEntry[1] = afEntry[1];
89  m_afEntry[2] = afEntry[2];
90  m_afEntry[3] = afEntry[3];
91  }
92  else
93  {
94  m_afEntry[0] = afEntry[0];
95  m_afEntry[1] = afEntry[2];
96  m_afEntry[2] = afEntry[1];
97  m_afEntry[3] = afEntry[3];
98  }
99 }
100 
101 template <class Real>
102 moMatrix2<Real>::moMatrix2 (const moVector2<Real>& rkU, const moVector2<Real>& rkV,
103  bool bColumns)
104 {
105  if (bColumns)
106  {
107  m_afEntry[0] = rkU[0];
108  m_afEntry[1] = rkV[0];
109  m_afEntry[2] = rkU[1];
110  m_afEntry[3] = rkV[1];
111  }
112  else
113  {
114  m_afEntry[0] = rkU[0];
115  m_afEntry[1] = rkU[1];
116  m_afEntry[2] = rkV[0];
117  m_afEntry[3] = rkV[1];
118  }
119 }
120 
121 template <class Real>
122 moMatrix2<Real>::moMatrix2 (const moVector2<Real>* akV, bool bColumns)
123 {
124  if (bColumns)
125  {
126  m_afEntry[0] = akV[0][0];
127  m_afEntry[1] = akV[1][0];
128  m_afEntry[2] = akV[0][1];
129  m_afEntry[3] = akV[1][1];
130  }
131  else
132  {
133  m_afEntry[0] = akV[0][0];
134  m_afEntry[1] = akV[0][1];
135  m_afEntry[2] = akV[1][0];
136  m_afEntry[3] = akV[1][1];
137  }
138 }
139 
140 template <class Real>
141 moMatrix2<Real>::moMatrix2 (Real fM00, Real fM11)
142 {
143  MakeDiagonal(fM00,fM11);
144 }
145 
146 template <class Real>
147 moMatrix2<Real>::moMatrix2 (Real fAngle)
148 {
149  FromAngle(fAngle);
150 }
151 
152 template <class Real>
153 moMatrix2<Real>::moMatrix2 (const moVector2<Real>& rkU, const moVector2<Real>& rkV)
154 {
155  MakeTensorProduct(rkU,rkV);
156 }
157 
158 template <class Real>
159 void moMatrix2<Real>::MakeZero ()
160 {
161  m_afEntry[0] = (Real)0.0;
162  m_afEntry[1] = (Real)0.0;
163  m_afEntry[2] = (Real)0.0;
164  m_afEntry[3] = (Real)0.0;
165 }
166 
167 template <class Real>
168 void moMatrix2<Real>::MakeIdentity ()
169 {
170  m_afEntry[0] = (Real)1.0;
171  m_afEntry[1] = (Real)0.0;
172  m_afEntry[2] = (Real)0.0;
173  m_afEntry[3] = (Real)1.0;
174 }
175 
176 template <class Real>
177 void moMatrix2<Real>::MakeDiagonal (Real fM00, Real fM11)
178 {
179  m_afEntry[0] = fM00;
180  m_afEntry[1] = (Real)0.0;
181  m_afEntry[2] = (Real)0.0;
182  m_afEntry[3] = fM11;
183 }
184 
185 template <class Real>
186 void moMatrix2<Real>::FromAngle (Real fAngle)
187 {
188  m_afEntry[0] = moMath<Real>::Cos(fAngle);
189  m_afEntry[2] = moMath<Real>::Sin(fAngle);
190  m_afEntry[1] = -m_afEntry[2];
191  m_afEntry[3] = m_afEntry[0];
192 }
193 
194 template <class Real>
195 void moMatrix2<Real>::MakeTensorProduct (const moVector2<Real>& rkU,
196  const moVector2<Real>& rkV)
197 {
198  m_afEntry[0] = rkU[0]*rkV[0];
199  m_afEntry[1] = rkU[0]*rkV[1];
200  m_afEntry[2] = rkU[1]*rkV[0];
201  m_afEntry[3] = rkU[1]*rkV[1];
202 }
203 
204 template <class Real>
205 void moMatrix2<Real>::SetRow (int iRow, const moVector2<Real>& rkV)
206 {
207  int i0 = 2*iRow ,i1 = i0+1;
208  m_afEntry[i0] = rkV[0];
209  m_afEntry[i1] = rkV[1];
210 }
211 
212 template <class Real>
213 moVector2<Real> moMatrix2<Real>::GetRow (int iRow) const
214 {
215  int i0 = 2*iRow ,i1 = i0+1;
216  return moVector2<Real>(m_afEntry[i0],m_afEntry[i1]);
217 }
218 
219 template <class Real>
220 void moMatrix2<Real>::SetColumn (int iCol, const moVector2<Real>& rkV)
221 {
222  m_afEntry[iCol] = rkV[0];
223  m_afEntry[iCol+2] = rkV[1];
224 }
225 
226 template <class Real>
227 moVector2<Real> moMatrix2<Real>::GetColumn (int iCol) const
228 {
229  return moVector2<Real>(m_afEntry[iCol],m_afEntry[iCol+2]);
230 }
231 
232 template <class Real>
233 void moMatrix2<Real>::GetColumnMajor (Real* afCMajor) const
234 {
235  afCMajor[0] = m_afEntry[0];
236  afCMajor[1] = m_afEntry[2];
237  afCMajor[2] = m_afEntry[1];
238  afCMajor[3] = m_afEntry[3];
239 }
240 
241 template <class Real>
242 int moMatrix2<Real>::CompareArrays (const moMatrix2& rkM) const
243 {
244  return memcmp(m_afEntry,rkM.m_afEntry,4*sizeof(Real));
245 }
246 
247 template <class Real>
248 bool moMatrix2<Real>::operator== (const moMatrix2& rkM) const
249 {
250  return CompareArrays(rkM) == 0;
251 }
252 
253 template <class Real>
254 bool moMatrix2<Real>::operator!= (const moMatrix2& rkM) const
255 {
256  return CompareArrays(rkM) != 0;
257 }
258 
259 template <class Real>
260 bool moMatrix2<Real>::operator< (const moMatrix2& rkM) const
261 {
262  return CompareArrays(rkM) < 0;
263 }
264 
265 template <class Real>
266 bool moMatrix2<Real>::operator<= (const moMatrix2& rkM) const
267 {
268  return CompareArrays(rkM) <= 0;
269 }
270 
271 template <class Real>
272 bool moMatrix2<Real>::operator> (const moMatrix2& rkM) const
273 {
274  return CompareArrays(rkM) > 0;
275 }
276 
277 template <class Real>
278 bool moMatrix2<Real>::operator>= (const moMatrix2& rkM) const
279 {
280  return CompareArrays(rkM) >= 0;
281 }
282 
283 template <class Real>
284 moMatrix2<Real> moMatrix2<Real>::Transpose () const
285 {
286  return moMatrix2<Real>(
287  m_afEntry[0],
288  m_afEntry[2],
289  m_afEntry[1],
290  m_afEntry[3]);
291 }
292 
293 template <class Real>
294 moMatrix2<Real> moMatrix2<Real>::TransposeTimes (const moMatrix2& rkM) const
295 {
296  // P = A^T*B
297  return moMatrix2<Real>(
298  m_afEntry[0]*rkM.m_afEntry[0] + m_afEntry[2]*rkM.m_afEntry[2],
299  m_afEntry[0]*rkM.m_afEntry[1] + m_afEntry[2]*rkM.m_afEntry[3],
300  m_afEntry[1]*rkM.m_afEntry[0] + m_afEntry[3]*rkM.m_afEntry[2],
301  m_afEntry[1]*rkM.m_afEntry[1] + m_afEntry[3]*rkM.m_afEntry[3]);
302 }
303 
304 template <class Real>
305 moMatrix2<Real> moMatrix2<Real>::TimesTranspose (const moMatrix2& rkM) const
306 {
307  // P = A*B^T
308  return moMatrix2<Real>(
309  m_afEntry[0]*rkM.m_afEntry[0] + m_afEntry[1]*rkM.m_afEntry[1],
310  m_afEntry[0]*rkM.m_afEntry[2] + m_afEntry[1]*rkM.m_afEntry[3],
311  m_afEntry[2]*rkM.m_afEntry[0] + m_afEntry[3]*rkM.m_afEntry[1],
312  m_afEntry[2]*rkM.m_afEntry[2] + m_afEntry[3]*rkM.m_afEntry[3]);
313 }
314 
315 template <class Real>
316 moMatrix2<Real> moMatrix2<Real>::Inverse () const
317 {
318  moMatrix2<Real> kInverse;
319 
320  Real fDet = m_afEntry[0]*m_afEntry[3] - m_afEntry[1]*m_afEntry[2];
321  if (moMath<Real>::FAbs(fDet) > moMath<Real>::ZERO_TOLERANCE)
322  {
323  Real fInvDet = ((Real)1.0)/fDet;
324  kInverse.m_afEntry[0] = m_afEntry[3]*fInvDet;
325  kInverse.m_afEntry[1] = -m_afEntry[1]*fInvDet;
326  kInverse.m_afEntry[2] = -m_afEntry[2]*fInvDet;
327  kInverse.m_afEntry[3] = m_afEntry[0]*fInvDet;
328  }
329  else
330  {
331  kInverse.m_afEntry[0] = (Real)0.0;
332  kInverse.m_afEntry[1] = (Real)0.0;
333  kInverse.m_afEntry[2] = (Real)0.0;
334  kInverse.m_afEntry[3] = (Real)0.0;
335  }
336 
337  return kInverse;
338 }
339 
340 template <class Real>
341 moMatrix2<Real> moMatrix2<Real>::Adjoint () const
342 {
343  return moMatrix2<Real>(
344  m_afEntry[3],
345  -m_afEntry[1],
346  -m_afEntry[2],
347  m_afEntry[0]);
348 }
349 
350 template <class Real>
351 Real moMatrix2<Real>::Determinant () const
352 {
353  return m_afEntry[0]*m_afEntry[3] - m_afEntry[1]*m_afEntry[2];
354 }
355 
356 template <class Real>
357 Real moMatrix2<Real>::QForm (const moVector2<Real>& rkU,
358  const moVector2<Real>& rkV) const
359 {
360  return rkU.Dot((*this)*rkV);
361 }
362 
363 template <class Real>
364 void moMatrix2<Real>::ToAngle (Real& rfAngle) const
365 {
366  // assert: matrix is a rotation
367  rfAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
368 }
369 
370 template <class Real>
371 void moMatrix2<Real>::Orthonormalize ()
372 {
373  // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
374  // M = [m0|m1], then orthonormal output matrix is Q = [q0|q1],
375  //
376  // q0 = m0/|m0|
377  // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
378  //
379  // where |V| indicates length of vector V and A*B indicates dot
380  // product of vectors A and B.
381 
382  // compute q0
383  Real fInvLength = moMath<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
384  m_afEntry[2]*m_afEntry[2]);
385 
386  m_afEntry[0] *= fInvLength;
387  m_afEntry[2] *= fInvLength;
388 
389  // compute q1
390  Real fDot0 = m_afEntry[0]*m_afEntry[1] + m_afEntry[2]*m_afEntry[3];
391  m_afEntry[1] -= fDot0*m_afEntry[0];
392  m_afEntry[3] -= fDot0*m_afEntry[2];
393 
394  fInvLength = moMath<Real>::InvSqrt(m_afEntry[1]*m_afEntry[1] +
395  m_afEntry[3]*m_afEntry[3]);
396 
397  m_afEntry[1] *= fInvLength;
398  m_afEntry[3] *= fInvLength;
399 }
400 
401 template <class Real>
402 void moMatrix2<Real>::EigenDecomposition (moMatrix2& rkRot, moMatrix2& rkDiag) const
403 {
404  Real fTrace = m_afEntry[0] + m_afEntry[3];
405  Real fDiff = m_afEntry[0] - m_afEntry[3];
406  Real fDiscr = moMath<Real>::Sqrt(fDiff*fDiff +
407  ((Real)4.0)*m_afEntry[1]*m_afEntry[1]);
408  Real fEVal0 = ((Real)0.5)*(fTrace-fDiscr);
409  Real fEVal1 = ((Real)0.5)*(fTrace+fDiscr);
410  rkDiag.MakeDiagonal(fEVal0,fEVal1);
411 
412  Real fCos, fSin;
413  if (fDiff >= (Real)0.0)
414  {
415  fCos = m_afEntry[1];
416  fSin = fEVal0 - m_afEntry[0];
417  }
418  else
419  {
420  fCos = fEVal0 - m_afEntry[3];
421  fSin = m_afEntry[1];
422  }
423  Real fTmp = moMath<Real>::InvSqrt(fCos*fCos + fSin*fSin);
424  fCos *= fTmp;
425  fSin *= fTmp;
426 
427  rkRot.m_afEntry[0] = fCos;
428  rkRot.m_afEntry[1] = -fSin;
429  rkRot.m_afEntry[2] = fSin;
430  rkRot.m_afEntry[3] = fCos;
431 }
432 */
433 
434 template<> const moMatrix2<MOfloat> moMatrix2<MOfloat>::ZERO(
435  0.0f,0.0f,
436  0.0f,0.0f);
437 template<> const moMatrix2<MOfloat> moMatrix2<MOfloat>::IDENTITY(
438  1.0f,0.0f,
439  0.0f,1.0f);
440 
441 template<> const moMatrix2<MOdouble> moMatrix2<MOdouble>::ZERO(
442  0.0,0.0,
443  0.0,0.0);
444 template<> const moMatrix2<MOdouble> moMatrix2<MOdouble>::IDENTITY(
445  1.0,0.0,
446  0.0,1.0);
447 
448 // momoMatrix3 class ------------------------------------------------------------
449 /*
450 template <class Real>
451 moMatrix3<Real>::moMatrix3 (bool bZero)
452 {
453  if (bZero)
454  {
455  MakeZero();
456  }
457  else
458  {
459  MakeIdentity();
460  }
461 }
462 
463 template <class Real>
464 moMatrix3<Real>::moMatrix3 (const moMatrix3& rkM) : moAbstract(rkM)
465 {
466  m_afEntry[0] = rkM.m_afEntry[0];
467  m_afEntry[1] = rkM.m_afEntry[1];
468  m_afEntry[2] = rkM.m_afEntry[2];
469  m_afEntry[3] = rkM.m_afEntry[3];
470  m_afEntry[4] = rkM.m_afEntry[4];
471  m_afEntry[5] = rkM.m_afEntry[5];
472  m_afEntry[6] = rkM.m_afEntry[6];
473  m_afEntry[7] = rkM.m_afEntry[7];
474  m_afEntry[8] = rkM.m_afEntry[8];
475 }
476 
477 template <class Real>
478 moMatrix3<Real>::moMatrix3 (Real fM00, Real fM01, Real fM02, Real fM10, Real fM11,
479  Real fM12, Real fM20, Real fM21, Real fM22)
480 {
481  m_afEntry[0] = fM00;
482  m_afEntry[1] = fM01;
483  m_afEntry[2] = fM02;
484  m_afEntry[3] = fM10;
485  m_afEntry[4] = fM11;
486  m_afEntry[5] = fM12;
487  m_afEntry[6] = fM20;
488  m_afEntry[7] = fM21;
489  m_afEntry[8] = fM22;
490 }
491 
492 template <class Real>
493 moMatrix3<Real>::moMatrix3 (const Real afEntry[9], bool bRowMajor)
494 {
495  if (bRowMajor)
496  {
497  m_afEntry[0] = afEntry[0];
498  m_afEntry[1] = afEntry[1];
499  m_afEntry[2] = afEntry[2];
500  m_afEntry[3] = afEntry[3];
501  m_afEntry[4] = afEntry[4];
502  m_afEntry[5] = afEntry[5];
503  m_afEntry[6] = afEntry[6];
504  m_afEntry[7] = afEntry[7];
505  m_afEntry[8] = afEntry[8];
506  }
507  else
508  {
509  m_afEntry[0] = afEntry[0];
510  m_afEntry[1] = afEntry[3];
511  m_afEntry[2] = afEntry[6];
512  m_afEntry[3] = afEntry[1];
513  m_afEntry[4] = afEntry[4];
514  m_afEntry[5] = afEntry[7];
515  m_afEntry[6] = afEntry[2];
516  m_afEntry[7] = afEntry[5];
517  m_afEntry[8] = afEntry[8];
518  }
519 }
520 
521 template <class Real>
522 moMatrix3<Real>::moMatrix3 (const moVector3<Real>& rkU, const moVector3<Real>& rkV,
523  const moVector3<Real>& rkW, bool bColumns)
524 {
525  if (bColumns)
526  {
527  m_afEntry[0] = rkU[0];
528  m_afEntry[1] = rkV[0];
529  m_afEntry[2] = rkW[0];
530  m_afEntry[3] = rkU[1];
531  m_afEntry[4] = rkV[1];
532  m_afEntry[5] = rkW[1];
533  m_afEntry[6] = rkU[2];
534  m_afEntry[7] = rkV[2];
535  m_afEntry[8] = rkW[2];
536  }
537  else
538  {
539  m_afEntry[0] = rkU[0];
540  m_afEntry[1] = rkU[1];
541  m_afEntry[2] = rkU[2];
542  m_afEntry[3] = rkV[0];
543  m_afEntry[4] = rkV[1];
544  m_afEntry[5] = rkV[2];
545  m_afEntry[6] = rkW[0];
546  m_afEntry[7] = rkW[1];
547  m_afEntry[8] = rkW[2];
548  }
549 }
550 
551 template <class Real>
552 moMatrix3<Real>::moMatrix3 (const moVector3<Real>* akV, bool bColumns)
553 {
554  if (bColumns)
555  {
556  m_afEntry[0] = akV[0][0];
557  m_afEntry[1] = akV[1][0];
558  m_afEntry[2] = akV[2][0];
559  m_afEntry[3] = akV[0][1];
560  m_afEntry[4] = akV[1][1];
561  m_afEntry[5] = akV[2][1];
562  m_afEntry[6] = akV[0][2];
563  m_afEntry[7] = akV[1][2];
564  m_afEntry[8] = akV[2][2];
565  }
566  else
567  {
568  m_afEntry[0] = akV[0][0];
569  m_afEntry[1] = akV[0][1];
570  m_afEntry[2] = akV[0][2];
571  m_afEntry[3] = akV[1][0];
572  m_afEntry[4] = akV[1][1];
573  m_afEntry[5] = akV[1][2];
574  m_afEntry[6] = akV[2][0];
575  m_afEntry[7] = akV[2][1];
576  m_afEntry[8] = akV[2][2];
577  }
578 }
579 
580 template <class Real>
581 moMatrix3<Real>::moMatrix3 (Real fM00, Real fM11, Real fM22)
582 {
583  MakeDiagonal(fM00,fM11,fM22);
584 }
585 
586 template <class Real>
587 moMatrix3<Real>::moMatrix3 (const moVector3<Real>& rkAxis, Real fAngle)
588 {
589  FromAxisAngle(rkAxis,fAngle);
590 }
591 
592 template <class Real>
593 moMatrix3<Real>::moMatrix3 (const moVector3<Real>& rkU, const moVector3<Real>& rkV)
594 {
595  MakeTensorProduct(rkU,rkV);
596 }
597 
598 template <class Real>
599 moMatrix3<Real>& moMatrix3<Real>::MakeZero ()
600 {
601  m_afEntry[0] = (Real)0.0;
602  m_afEntry[1] = (Real)0.0;
603  m_afEntry[2] = (Real)0.0;
604  m_afEntry[3] = (Real)0.0;
605  m_afEntry[4] = (Real)0.0;
606  m_afEntry[5] = (Real)0.0;
607  m_afEntry[6] = (Real)0.0;
608  m_afEntry[7] = (Real)0.0;
609  m_afEntry[8] = (Real)0.0;
610  return *this;
611 }
612 
613 template <class Real>
614 moMatrix3<Real>& moMatrix3<Real>::MakeIdentity ()
615 {
616  m_afEntry[0] = (Real)1.0;
617  m_afEntry[1] = (Real)0.0;
618  m_afEntry[2] = (Real)0.0;
619  m_afEntry[3] = (Real)0.0;
620  m_afEntry[4] = (Real)1.0;
621  m_afEntry[5] = (Real)0.0;
622  m_afEntry[6] = (Real)0.0;
623  m_afEntry[7] = (Real)0.0;
624  m_afEntry[8] = (Real)1.0;
625  return *this;
626 }
627 
628 template <class Real>
629 moMatrix3<Real>& moMatrix3<Real>::MakeDiagonal (Real fM00, Real fM11, Real fM22)
630 {
631  m_afEntry[0] = fM00;
632  m_afEntry[1] = (Real)0.0;
633  m_afEntry[2] = (Real)0.0;
634  m_afEntry[3] = (Real)0.0;
635  m_afEntry[4] = fM11;
636  m_afEntry[5] = (Real)0.0;
637  m_afEntry[6] = (Real)0.0;
638  m_afEntry[7] = (Real)0.0;
639  m_afEntry[8] = fM22;
640  return *this;
641 }
642 
643 template <class Real>
644 moMatrix3<Real>& moMatrix3<Real>::FromAxisAngle (const moVector3<Real>& rkAxis,
645  Real fAngle)
646 {
647  Real fCos = moMath<Real>::Cos(fAngle);
648  Real fSin = moMath<Real>::Sin(fAngle);
649  Real fOneMinusCos = ((Real)1.0)-fCos;
650  Real fX2 = rkAxis[0]*rkAxis[0];
651  Real fY2 = rkAxis[1]*rkAxis[1];
652  Real fZ2 = rkAxis[2]*rkAxis[2];
653  Real fXYM = rkAxis[0]*rkAxis[1]*fOneMinusCos;
654  Real fXZM = rkAxis[0]*rkAxis[2]*fOneMinusCos;
655  Real fYZM = rkAxis[1]*rkAxis[2]*fOneMinusCos;
656  Real fXSin = rkAxis[0]*fSin;
657  Real fYSin = rkAxis[1]*fSin;
658  Real fZSin = rkAxis[2]*fSin;
659 
660  m_afEntry[0] = fX2*fOneMinusCos+fCos;
661  m_afEntry[1] = fXYM-fZSin;
662  m_afEntry[2] = fXZM+fYSin;
663  m_afEntry[3] = fXYM+fZSin;
664  m_afEntry[4] = fY2*fOneMinusCos+fCos;
665  m_afEntry[5] = fYZM-fXSin;
666  m_afEntry[6] = fXZM-fYSin;
667  m_afEntry[7] = fYZM+fXSin;
668  m_afEntry[8] = fZ2*fOneMinusCos+fCos;
669 
670  return *this;
671 }
672 
673 template <class Real>
674 moMatrix3<Real>& moMatrix3<Real>::MakeTensorProduct (const moVector3<Real>& rkU,
675  const moVector3<Real>& rkV)
676 {
677  m_afEntry[0] = rkU[0]*rkV[0];
678  m_afEntry[1] = rkU[0]*rkV[1];
679  m_afEntry[2] = rkU[0]*rkV[2];
680  m_afEntry[3] = rkU[1]*rkV[0];
681  m_afEntry[4] = rkU[1]*rkV[1];
682  m_afEntry[5] = rkU[1]*rkV[2];
683  m_afEntry[6] = rkU[2]*rkV[0];
684  m_afEntry[7] = rkU[2]*rkV[1];
685  m_afEntry[8] = rkU[2]*rkV[2];
686  return *this;
687 }
688 
689 template <class Real>
690 void moMatrix3<Real>::SetRow (int iRow, const moVector3<Real>& rkV)
691 {
692  int i0 = 3*iRow, i1 = i0+1, i2 = i1+1;
693  m_afEntry[i0] = rkV[0];
694  m_afEntry[i1] = rkV[1];
695  m_afEntry[i2] = rkV[2];
696 }
697 
698 template <class Real>
699 moVector3<Real> moMatrix3<Real>::GetRow (int iRow) const
700 {
701  int i0 = 3*iRow, i1 = i0+1, i2 = i1+1;
702  return moVector3<Real>(m_afEntry[i0],m_afEntry[i1],m_afEntry[i2]);
703 }
704 
705 template <class Real>
706 void moMatrix3<Real>::SetColumn (int iCol, const moVector3<Real>& rkV)
707 {
708  m_afEntry[iCol] = rkV[0];
709  m_afEntry[iCol+3] = rkV[1];
710  m_afEntry[iCol+6] = rkV[2];
711 }
712 
713 template <class Real>
714 moVector3<Real> moMatrix3<Real>::GetColumn (int iCol) const
715 {
716  return moVector3<Real>(m_afEntry[iCol],m_afEntry[iCol+3],m_afEntry[iCol+6]);
717 }
718 
719 template <class Real>
720 void moMatrix3<Real>::GetColumnMajor (Real* afCMajor) const
721 {
722  afCMajor[0] = m_afEntry[0];
723  afCMajor[1] = m_afEntry[3];
724  afCMajor[2] = m_afEntry[6];
725  afCMajor[3] = m_afEntry[1];
726  afCMajor[4] = m_afEntry[4];
727  afCMajor[5] = m_afEntry[7];
728  afCMajor[6] = m_afEntry[2];
729  afCMajor[7] = m_afEntry[5];
730  afCMajor[8] = m_afEntry[8];
731 }
732 
733 template <class Real>
734 int moMatrix3<Real>::CompareArrays (const moMatrix3& rkM) const
735 {
736  return memcmp(m_afEntry,rkM.m_afEntry,9*sizeof(Real));
737 }
738 
739 template <class Real>
740 bool moMatrix3<Real>::operator== (const moMatrix3& rkM) const
741 {
742  return CompareArrays(rkM) == 0;
743 }
744 
745 template <class Real>
746 bool moMatrix3<Real>::operator!= (const moMatrix3& rkM) const
747 {
748  return CompareArrays(rkM) != 0;
749 }
750 
751 template <class Real>
752 bool moMatrix3<Real>::operator< (const moMatrix3& rkM) const
753 {
754  return CompareArrays(rkM) < 0;
755 }
756 
757 template <class Real>
758 bool moMatrix3<Real>::operator<= (const moMatrix3& rkM) const
759 {
760  return CompareArrays(rkM) <= 0;
761 }
762 
763 template <class Real>
764 bool moMatrix3<Real>::operator> (const moMatrix3& rkM) const
765 {
766  return CompareArrays(rkM) > 0;
767 }
768 
769 template <class Real>
770 bool moMatrix3<Real>::operator>= (const moMatrix3& rkM) const
771 {
772  return CompareArrays(rkM) >= 0;
773 }
774 
775 template <class Real>
776 moMatrix3<Real> moMatrix3<Real>::Transpose () const
777 {
778  return moMatrix3<Real>(
779  m_afEntry[0],
780  m_afEntry[3],
781  m_afEntry[6],
782  m_afEntry[1],
783  m_afEntry[4],
784  m_afEntry[7],
785  m_afEntry[2],
786  m_afEntry[5],
787  m_afEntry[8]);
788 }
789 
790 template <class Real>
791 moMatrix3<Real> moMatrix3<Real>::TransposeTimes (const moMatrix3& rkM) const
792 {
793  // P = A^T*B
794  return moMatrix3<Real>(
795  m_afEntry[0]*rkM.m_afEntry[0] +
796  m_afEntry[3]*rkM.m_afEntry[3] +
797  m_afEntry[6]*rkM.m_afEntry[6],
798 
799  m_afEntry[0]*rkM.m_afEntry[1] +
800  m_afEntry[3]*rkM.m_afEntry[4] +
801  m_afEntry[6]*rkM.m_afEntry[7],
802 
803  m_afEntry[0]*rkM.m_afEntry[2] +
804  m_afEntry[3]*rkM.m_afEntry[5] +
805  m_afEntry[6]*rkM.m_afEntry[8],
806 
807  m_afEntry[1]*rkM.m_afEntry[0] +
808  m_afEntry[4]*rkM.m_afEntry[3] +
809  m_afEntry[7]*rkM.m_afEntry[6],
810 
811  m_afEntry[1]*rkM.m_afEntry[1] +
812  m_afEntry[4]*rkM.m_afEntry[4] +
813  m_afEntry[7]*rkM.m_afEntry[7],
814 
815  m_afEntry[1]*rkM.m_afEntry[2] +
816  m_afEntry[4]*rkM.m_afEntry[5] +
817  m_afEntry[7]*rkM.m_afEntry[8],
818 
819  m_afEntry[2]*rkM.m_afEntry[0] +
820  m_afEntry[5]*rkM.m_afEntry[3] +
821  m_afEntry[8]*rkM.m_afEntry[6],
822 
823  m_afEntry[2]*rkM.m_afEntry[1] +
824  m_afEntry[5]*rkM.m_afEntry[4] +
825  m_afEntry[8]*rkM.m_afEntry[7],
826 
827  m_afEntry[2]*rkM.m_afEntry[2] +
828  m_afEntry[5]*rkM.m_afEntry[5] +
829  m_afEntry[8]*rkM.m_afEntry[8]);
830 }
831 
832 template <class Real>
833 moMatrix3<Real> moMatrix3<Real>::TimesTranspose (const moMatrix3& rkM) const
834 {
835  // P = A*B^T
836  return moMatrix3<Real>(
837  m_afEntry[0]*rkM.m_afEntry[0] +
838  m_afEntry[1]*rkM.m_afEntry[1] +
839  m_afEntry[2]*rkM.m_afEntry[2],
840 
841  m_afEntry[0]*rkM.m_afEntry[3] +
842  m_afEntry[1]*rkM.m_afEntry[4] +
843  m_afEntry[2]*rkM.m_afEntry[5],
844 
845  m_afEntry[0]*rkM.m_afEntry[6] +
846  m_afEntry[1]*rkM.m_afEntry[7] +
847  m_afEntry[2]*rkM.m_afEntry[8],
848 
849  m_afEntry[3]*rkM.m_afEntry[0] +
850  m_afEntry[4]*rkM.m_afEntry[1] +
851  m_afEntry[5]*rkM.m_afEntry[2],
852 
853  m_afEntry[3]*rkM.m_afEntry[3] +
854  m_afEntry[4]*rkM.m_afEntry[4] +
855  m_afEntry[5]*rkM.m_afEntry[5],
856 
857  m_afEntry[3]*rkM.m_afEntry[6] +
858  m_afEntry[4]*rkM.m_afEntry[7] +
859  m_afEntry[5]*rkM.m_afEntry[8],
860 
861  m_afEntry[6]*rkM.m_afEntry[0] +
862  m_afEntry[7]*rkM.m_afEntry[1] +
863  m_afEntry[8]*rkM.m_afEntry[2],
864 
865  m_afEntry[6]*rkM.m_afEntry[3] +
866  m_afEntry[7]*rkM.m_afEntry[4] +
867  m_afEntry[8]*rkM.m_afEntry[5],
868 
869  m_afEntry[6]*rkM.m_afEntry[6] +
870  m_afEntry[7]*rkM.m_afEntry[7] +
871  m_afEntry[8]*rkM.m_afEntry[8]);
872 }
873 
874 template <class Real>
875 moMatrix3<Real> moMatrix3<Real>::Inverse () const
876 {
877  // Invert a 3x3 using cofactors. This is faster than using a generic
878  // Gaussian elimination because of the loop overhead of such a method.
879 
880  moMatrix3 kInverse;
881 
882  kInverse.m_afEntry[0] =
883  m_afEntry[4]*m_afEntry[8] - m_afEntry[5]*m_afEntry[7];
884  kInverse.m_afEntry[1] =
885  m_afEntry[2]*m_afEntry[7] - m_afEntry[1]*m_afEntry[8];
886  kInverse.m_afEntry[2] =
887  m_afEntry[1]*m_afEntry[5] - m_afEntry[2]*m_afEntry[4];
888  kInverse.m_afEntry[3] =
889  m_afEntry[5]*m_afEntry[6] - m_afEntry[3]*m_afEntry[8];
890  kInverse.m_afEntry[4] =
891  m_afEntry[0]*m_afEntry[8] - m_afEntry[2]*m_afEntry[6];
892  kInverse.m_afEntry[5] =
893  m_afEntry[2]*m_afEntry[3] - m_afEntry[0]*m_afEntry[5];
894  kInverse.m_afEntry[6] =
895  m_afEntry[3]*m_afEntry[7] - m_afEntry[4]*m_afEntry[6];
896  kInverse.m_afEntry[7] =
897  m_afEntry[1]*m_afEntry[6] - m_afEntry[0]*m_afEntry[7];
898  kInverse.m_afEntry[8] =
899  m_afEntry[0]*m_afEntry[4] - m_afEntry[1]*m_afEntry[3];
900 
901  Real fDet =
902  m_afEntry[0]*kInverse.m_afEntry[0] +
903  m_afEntry[1]*kInverse.m_afEntry[3] +
904  m_afEntry[2]*kInverse.m_afEntry[6];
905 
906  if (moMath<Real>::FAbs(fDet) <= moMath<Real>::ZERO_TOLERANCE)
907  {
908  return ZERO;
909  }
910 
911  Real fInvDet = ((Real)1.0)/fDet;
912  kInverse.m_afEntry[0] *= fInvDet;
913  kInverse.m_afEntry[1] *= fInvDet;
914  kInverse.m_afEntry[2] *= fInvDet;
915  kInverse.m_afEntry[3] *= fInvDet;
916  kInverse.m_afEntry[4] *= fInvDet;
917  kInverse.m_afEntry[5] *= fInvDet;
918  kInverse.m_afEntry[6] *= fInvDet;
919  kInverse.m_afEntry[7] *= fInvDet;
920  kInverse.m_afEntry[8] *= fInvDet;
921  return kInverse;
922 }
923 
924 template <class Real>
925 moMatrix3<Real> moMatrix3<Real>::Adjoint () const
926 {
927  return moMatrix3<Real>(
928  m_afEntry[4]*m_afEntry[8] - m_afEntry[5]*m_afEntry[7],
929  m_afEntry[2]*m_afEntry[7] - m_afEntry[1]*m_afEntry[8],
930  m_afEntry[1]*m_afEntry[5] - m_afEntry[2]*m_afEntry[4],
931  m_afEntry[5]*m_afEntry[6] - m_afEntry[3]*m_afEntry[8],
932  m_afEntry[0]*m_afEntry[8] - m_afEntry[2]*m_afEntry[6],
933  m_afEntry[2]*m_afEntry[3] - m_afEntry[0]*m_afEntry[5],
934  m_afEntry[3]*m_afEntry[7] - m_afEntry[4]*m_afEntry[6],
935  m_afEntry[1]*m_afEntry[6] - m_afEntry[0]*m_afEntry[7],
936  m_afEntry[0]*m_afEntry[4] - m_afEntry[1]*m_afEntry[3]);
937 }
938 
939 template <class Real>
940 Real moMatrix3<Real>::Determinant () const
941 {
942  Real fCo00 = m_afEntry[4]*m_afEntry[8] - m_afEntry[5]*m_afEntry[7];
943  Real fCo10 = m_afEntry[5]*m_afEntry[6] - m_afEntry[3]*m_afEntry[8];
944  Real fCo20 = m_afEntry[3]*m_afEntry[7] - m_afEntry[4]*m_afEntry[6];
945  Real fDet = m_afEntry[0]*fCo00 + m_afEntry[1]*fCo10 + m_afEntry[2]*fCo20;
946  return fDet;
947 }
948 
949 template <class Real>
950 Real moMatrix3<Real>::QForm (const moVector3<Real>& rkU, const moVector3<Real>& rkV)
951  const
952 {
953  return rkU.Dot((*this)*rkV);
954 }
955 
956 template <class Real>
957 moMatrix3<Real> moMatrix3<Real>::TimesDiagonal (const moVector3<Real>& rkDiag) const
958 {
959  return moMatrix3<Real>(
960  m_afEntry[0]*rkDiag[0],m_afEntry[1]*rkDiag[1],m_afEntry[2]*rkDiag[2],
961  m_afEntry[3]*rkDiag[0],m_afEntry[4]*rkDiag[1],m_afEntry[5]*rkDiag[2],
962  m_afEntry[6]*rkDiag[0],m_afEntry[7]*rkDiag[1],m_afEntry[8]*rkDiag[2]);
963 }
964 
965 template <class Real>
966 moMatrix3<Real> moMatrix3<Real>::DiagonalTimes (const moVector3<Real>& rkDiag) const
967 {
968  return moMatrix3<Real>(
969  rkDiag[0]*m_afEntry[0],rkDiag[0]*m_afEntry[1],rkDiag[0]*m_afEntry[2],
970  rkDiag[1]*m_afEntry[3],rkDiag[1]*m_afEntry[4],rkDiag[1]*m_afEntry[5],
971  rkDiag[2]*m_afEntry[6],rkDiag[2]*m_afEntry[7],rkDiag[2]*m_afEntry[8]);
972 }
973 
974 template <class Real>
975 void moMatrix3<Real>::ToAxisAngle (moVector3<Real>& rkAxis, Real& rfAngle) const
976 {
977  // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
978  // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
979  // I is the identity and
980  //
981  // +- -+
982  // P = | 0 -z +y |
983  // | +z 0 -x |
984  // | -y +x 0 |
985  // +- -+
986  //
987  // If A > 0, R represents a counterclockwise rotation about the axis in
988  // the sense of looking from the tip of the axis vector towards the
989  // origin. Some algebra will show that
990  //
991  // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P
992  //
993  // In the event that A = pi, R-R^t = 0 which prevents us from extracting
994  // the axis through P. Instead note that R = I+2*P^2 when A = pi, so
995  // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and
996  // z^2-1. We can solve these for axis (x,y,z). Because the angle is pi,
997  // it does not matter which sign you choose on the square roots.
998 
999  Real fTrace = m_afEntry[0] + m_afEntry[4] + m_afEntry[8];
1000  Real fCos = ((Real)0.5)*(fTrace-(Real)1.0);
1001  rfAngle = moMath<Real>::ACos(fCos); // in [0,PI]
1002 
1003  if (rfAngle > (Real)0.0)
1004  {
1005  if (rfAngle < moMath<Real>::PI)
1006  {
1007  rkAxis[0] = m_afEntry[7]-m_afEntry[5];
1008  rkAxis[1] = m_afEntry[2]-m_afEntry[6];
1009  rkAxis[2] = m_afEntry[3]-m_afEntry[1];
1010  rkAxis.Normalize();
1011  }
1012  else
1013  {
1014  // angle is PI
1015  Real fHalfInverse;
1016  if (m_afEntry[0] >= m_afEntry[4])
1017  {
1018  // r00 >= r11
1019  if (m_afEntry[0] >= m_afEntry[8])
1020  {
1021  // r00 is maximum diagonal term
1022  rkAxis[0] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[0] -
1023  m_afEntry[4] - m_afEntry[8] + (Real)1.0);
1024  fHalfInverse = ((Real)0.5)/rkAxis[0];
1025  rkAxis[1] = fHalfInverse*m_afEntry[1];
1026  rkAxis[2] = fHalfInverse*m_afEntry[2];
1027  }
1028  else
1029  {
1030  // r22 is maximum diagonal term
1031  rkAxis[2] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[8] -
1032  m_afEntry[0] - m_afEntry[4] + (Real)1.0);
1033  fHalfInverse = ((Real)0.5)/rkAxis[2];
1034  rkAxis[0] = fHalfInverse*m_afEntry[2];
1035  rkAxis[1] = fHalfInverse*m_afEntry[5];
1036  }
1037  }
1038  else
1039  {
1040  // r11 > r00
1041  if (m_afEntry[4] >= m_afEntry[8])
1042  {
1043  // r11 is maximum diagonal term
1044  rkAxis[1] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[4] -
1045  m_afEntry[0] - m_afEntry[8] + (Real)1.0);
1046  fHalfInverse = ((Real)0.5)/rkAxis[1];
1047  rkAxis[0] = fHalfInverse*m_afEntry[1];
1048  rkAxis[2] = fHalfInverse*m_afEntry[5];
1049  }
1050  else
1051  {
1052  // r22 is maximum diagonal term
1053  rkAxis[2] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[8] -
1054  m_afEntry[0] - m_afEntry[4] + (Real)1.0);
1055  fHalfInverse = ((Real)0.5)/rkAxis[2];
1056  rkAxis[0] = fHalfInverse*m_afEntry[2];
1057  rkAxis[1] = fHalfInverse*m_afEntry[5];
1058  }
1059  }
1060  }
1061  }
1062  else
1063  {
1064  // The angle is 0 and the matrix is the identity. Any axis will
1065  // work, so just use the x-axis.
1066  rkAxis[0] = (Real)1.0;
1067  rkAxis[1] = (Real)0.0;
1068  rkAxis[2] = (Real)0.0;
1069  }
1070 }
1071 
1072 template <class Real>
1073 void moMatrix3<Real>::Orthonormalize ()
1074 {
1075  // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
1076  // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
1077  //
1078  // q0 = m0/|m0|
1079  // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
1080  // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
1081  //
1082  // where |V| indicates length of vector V and A*B indicates dot
1083  // product of vectors A and B.
1084 
1085  // compute q0
1086  Real fInvLength = moMath<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
1087  m_afEntry[3]*m_afEntry[3] + m_afEntry[6]*m_afEntry[6]);
1088 
1089  m_afEntry[0] *= fInvLength;
1090  m_afEntry[3] *= fInvLength;
1091  m_afEntry[6] *= fInvLength;
1092 
1093  // compute q1
1094  Real fDot0 = m_afEntry[0]*m_afEntry[1] + m_afEntry[3]*m_afEntry[4] +
1095  m_afEntry[6]*m_afEntry[7];
1096 
1097  m_afEntry[1] -= fDot0*m_afEntry[0];
1098  m_afEntry[4] -= fDot0*m_afEntry[3];
1099  m_afEntry[7] -= fDot0*m_afEntry[6];
1100 
1101  fInvLength = moMath<Real>::InvSqrt(m_afEntry[1]*m_afEntry[1] +
1102  m_afEntry[4]*m_afEntry[4] + m_afEntry[7]*m_afEntry[7]);
1103 
1104  m_afEntry[1] *= fInvLength;
1105  m_afEntry[4] *= fInvLength;
1106  m_afEntry[7] *= fInvLength;
1107 
1108  // compute q2
1109  Real fDot1 = m_afEntry[1]*m_afEntry[2] + m_afEntry[4]*m_afEntry[5] +
1110  m_afEntry[7]*m_afEntry[8];
1111 
1112  fDot0 = m_afEntry[0]*m_afEntry[2] + m_afEntry[3]*m_afEntry[5] +
1113  m_afEntry[6]*m_afEntry[8];
1114 
1115  m_afEntry[2] -= fDot0*m_afEntry[0] + fDot1*m_afEntry[1];
1116  m_afEntry[5] -= fDot0*m_afEntry[3] + fDot1*m_afEntry[4];
1117  m_afEntry[8] -= fDot0*m_afEntry[6] + fDot1*m_afEntry[7];
1118 
1119  fInvLength = moMath<Real>::InvSqrt(m_afEntry[2]*m_afEntry[2] +
1120  m_afEntry[5]*m_afEntry[5] + m_afEntry[8]*m_afEntry[8]);
1121 
1122  m_afEntry[2] *= fInvLength;
1123  m_afEntry[5] *= fInvLength;
1124  m_afEntry[8] *= fInvLength;
1125 }
1126 
1127 template <class Real>
1128 void moMatrix3<Real>::EigenDecomposition (moMatrix3& rkRot, moMatrix3& rkDiag) const
1129 {
1130  // Factor M = R*D*R^T. The columns of R are the eigenvectors. The
1131  // diagonal entries of D are the corresponding eigenvalues.
1132  Real afDiag[3], afSubd[2];
1133  rkRot = *this;
1134  bool bReflection = rkRot.Tridiagonalize(afDiag,afSubd);
1135  bool bConverged = rkRot.QLAlgorithm(afDiag,afSubd);
1136  if (!bConverged) return;
1137 
1138  // (insertion) sort eigenvalues in increasing order, d0 <= d1 <= d2
1139  int i;
1140  Real fSave;
1141 
1142  if (afDiag[1] < afDiag[0])
1143  {
1144  // swap d0 and d1
1145  fSave = afDiag[0];
1146  afDiag[0] = afDiag[1];
1147  afDiag[1] = fSave;
1148 
1149  // swap V0 and V1
1150  for (i = 0; i < 3; i++)
1151  {
1152  fSave = rkRot[i][0];
1153  rkRot[i][0] = rkRot[i][1];
1154  rkRot[i][1] = fSave;
1155  }
1156  bReflection = !bReflection;
1157  }
1158 
1159  if (afDiag[2] < afDiag[1])
1160  {
1161  // swap d1 and d2
1162  fSave = afDiag[1];
1163  afDiag[1] = afDiag[2];
1164  afDiag[2] = fSave;
1165 
1166  // swap V1 and V2
1167  for (i = 0; i < 3; i++)
1168  {
1169  fSave = rkRot[i][1];
1170  rkRot[i][1] = rkRot[i][2];
1171  rkRot[i][2] = fSave;
1172  }
1173  bReflection = !bReflection;
1174  }
1175 
1176  if (afDiag[1] < afDiag[0])
1177  {
1178  // swap d0 and d1
1179  fSave = afDiag[0];
1180  afDiag[0] = afDiag[1];
1181  afDiag[1] = fSave;
1182 
1183  // swap V0 and V1
1184  for (i = 0; i < 3; i++)
1185  {
1186  fSave = rkRot[i][0];
1187  rkRot[i][0] = rkRot[i][1];
1188  rkRot[i][1] = fSave;
1189  }
1190  bReflection = !bReflection;
1191  }
1192 
1193  rkDiag.MakeDiagonal(afDiag[0],afDiag[1],afDiag[2]);
1194 
1195  if (bReflection)
1196  {
1197  // The orthogonal transformation that diagonalizes M is a reflection.
1198  // Make the eigenvectors a right--handed system by changing sign on
1199  // the last column.
1200  rkRot[0][2] = -rkRot[0][2];
1201  rkRot[1][2] = -rkRot[1][2];
1202  rkRot[2][2] = -rkRot[2][2];
1203  }
1204 }
1205 
1206 template <class Real>
1207 moMatrix3<Real>& moMatrix3<Real>::FromEulerAnglesXYZ (Real fXAngle, Real fYAngle,
1208  Real fZAngle)
1209 {
1210  Real fCos, fSin;
1211 
1212  fCos = moMath<Real>::Cos(fXAngle);
1213  fSin = moMath<Real>::Sin(fXAngle);
1214  moMatrix3 kXMat(
1215  (Real)1.0,(Real)0.0,(Real)0.0,
1216  (Real)0.0,fCos,-fSin,
1217  (Real)0.0,fSin,fCos);
1218 
1219  fCos = moMath<Real>::Cos(fYAngle);
1220  fSin = moMath<Real>::Sin(fYAngle);
1221  moMatrix3 kYMat(
1222  fCos,(Real)0.0,fSin,
1223  (Real)0.0,(Real)1.0,(Real)0.0,
1224  -fSin,(Real)0.0,fCos);
1225 
1226  fCos = moMath<Real>::Cos(fZAngle);
1227  fSin = moMath<Real>::Sin(fZAngle);
1228  moMatrix3 kZMat(
1229  fCos,-fSin,(Real)0.0,
1230  fSin,fCos,(Real)0.0,
1231  (Real)0.0,(Real)0.0,(Real)1.0);
1232 
1233  *this = kXMat*(kYMat*kZMat);
1234  return *this;
1235 }
1236 
1237 template <class Real>
1238 moMatrix3<Real>& moMatrix3<Real>::FromEulerAnglesXZY (Real fXAngle, Real fZAngle,
1239  Real fYAngle)
1240 {
1241  Real fCos, fSin;
1242 
1243  fCos = moMath<Real>::Cos(fXAngle);
1244  fSin = moMath<Real>::Sin(fXAngle);
1245  moMatrix3 kXMat(
1246  (Real)1.0,(Real)0.0,(Real)0.0,
1247  (Real)0.0,fCos,-fSin,
1248  (Real)0.0,fSin,fCos);
1249 
1250  fCos = moMath<Real>::Cos(fZAngle);
1251  fSin = moMath<Real>::Sin(fZAngle);
1252  moMatrix3 kZMat(
1253  fCos,-fSin,(Real)0.0,
1254  fSin,fCos,(Real)0.0,
1255  (Real)0.0,(Real)0.0,(Real)1.0);
1256 
1257  fCos = moMath<Real>::Cos(fYAngle);
1258  fSin = moMath<Real>::Sin(fYAngle);
1259  moMatrix3 kYMat(
1260  fCos,(Real)0.0,fSin,
1261  (Real)0.0,(Real)1.0,(Real)0.0,
1262  -fSin,(Real)0.0,fCos);
1263 
1264  *this = kXMat*(kZMat*kYMat);
1265  return *this;
1266 }
1267 
1268 template <class Real>
1269 moMatrix3<Real>& moMatrix3<Real>::FromEulerAnglesYXZ (Real fYAngle, Real fXAngle,
1270  Real fZAngle)
1271 {
1272  Real fCos, fSin;
1273 
1274  fCos = moMath<Real>::Cos(fYAngle);
1275  fSin = moMath<Real>::Sin(fYAngle);
1276  moMatrix3 kYMat(
1277  fCos,(Real)0.0,fSin,
1278  (Real)0.0,(Real)1.0,(Real)0.0,
1279  -fSin,(Real)0.0,fCos);
1280 
1281  fCos = moMath<Real>::Cos(fXAngle);
1282  fSin = moMath<Real>::Sin(fXAngle);
1283  moMatrix3 kXMat(
1284  (Real)1.0,(Real)0.0,(Real)0.0,
1285  (Real)0.0,fCos,-fSin,
1286  (Real)0.0,fSin,fCos);
1287 
1288  fCos = moMath<Real>::Cos(fZAngle);
1289  fSin = moMath<Real>::Sin(fZAngle);
1290  moMatrix3 kZMat(
1291  fCos,-fSin,(Real)0.0,
1292  fSin,fCos,(Real)0.0,
1293  (Real)0.0,(Real)0.0,(Real)1.0);
1294 
1295  *this = kYMat*(kXMat*kZMat);
1296  return *this;
1297 }
1298 
1299 template <class Real>
1300 moMatrix3<Real>& moMatrix3<Real>::FromEulerAnglesYZX (Real fYAngle, Real fZAngle,
1301  Real fXAngle)
1302 {
1303  Real fCos, fSin;
1304 
1305  fCos = moMath<Real>::Cos(fYAngle);
1306  fSin = moMath<Real>::Sin(fYAngle);
1307  moMatrix3 kYMat(
1308  fCos,(Real)0.0,fSin,
1309  (Real)0.0,(Real)1.0,(Real)0.0,
1310  -fSin,(Real)0.0,fCos);
1311 
1312  fCos = moMath<Real>::Cos(fZAngle);
1313  fSin = moMath<Real>::Sin(fZAngle);
1314  moMatrix3 kZMat(
1315  fCos,-fSin,(Real)0.0,
1316  fSin,fCos,(Real)0.0,
1317  (Real)0.0,(Real)0.0,(Real)1.0);
1318 
1319  fCos = moMath<Real>::Cos(fXAngle);
1320  fSin = moMath<Real>::Sin(fXAngle);
1321  moMatrix3 kXMat(
1322  (Real)1.0,(Real)0.0,(Real)0.0,
1323  (Real)0.0,fCos,-fSin,
1324  (Real)0.0,fSin,fCos);
1325 
1326  *this = kYMat*(kZMat*kXMat);
1327  return *this;
1328 }
1329 
1330 template <class Real>
1331 moMatrix3<Real>& moMatrix3<Real>::FromEulerAnglesZXY (Real fZAngle, Real fXAngle,
1332  Real fYAngle)
1333 {
1334  Real fCos, fSin;
1335 
1336  fCos = moMath<Real>::Cos(fZAngle);
1337  fSin = moMath<Real>::Sin(fZAngle);
1338  moMatrix3 kZMat(
1339  fCos,-fSin,(Real)0.0,
1340  fSin,fCos,(Real)0.0,
1341  (Real)0.0,(Real)0.0,(Real)1.0);
1342 
1343  fCos = moMath<Real>::Cos(fXAngle);
1344  fSin = moMath<Real>::Sin(fXAngle);
1345  moMatrix3 kXMat(
1346  (Real)1.0,(Real)0.0,(Real)0.0,
1347  (Real)0.0,fCos,-fSin,
1348  (Real)0.0,fSin,fCos);
1349 
1350  fCos = moMath<Real>::Cos(fYAngle);
1351  fSin = moMath<Real>::Sin(fYAngle);
1352  moMatrix3 kYMat(
1353  fCos,(Real)0.0,fSin,
1354  (Real)0.0,(Real)1.0,(Real)0.0,
1355  -fSin,(Real)0.0,fCos);
1356 
1357  *this = kZMat*(kXMat*kYMat);
1358  return *this;
1359 }
1360 
1361 template <class Real>
1362 moMatrix3<Real>& moMatrix3<Real>::FromEulerAnglesZYX (Real fZAngle, Real fYAngle,
1363  Real fXAngle)
1364 {
1365  Real fCos, fSin;
1366 
1367  fCos = moMath<Real>::Cos(fZAngle);
1368  fSin = moMath<Real>::Sin(fZAngle);
1369  moMatrix3 kZMat(
1370  fCos,-fSin,(Real)0.0,
1371  fSin,fCos,(Real)0.0,
1372  (Real)0.0,(Real)0.0,(Real)1.0);
1373 
1374  fCos = moMath<Real>::Cos(fYAngle);
1375  fSin = moMath<Real>::Sin(fYAngle);
1376  moMatrix3 kYMat(
1377  fCos,(Real)0.0,fSin,
1378  (Real)0.0,(Real)1.0,(Real)0.0,
1379  -fSin,(Real)0.0,fCos);
1380 
1381  fCos = moMath<Real>::Cos(fXAngle);
1382  fSin = moMath<Real>::Sin(fXAngle);
1383  moMatrix3 kXMat(
1384  (Real)1.0,(Real)0.0,(Real)0.0,
1385  (Real)0.0,fCos,-fSin,
1386  (Real)0.0,fSin,fCos);
1387 
1388  *this = kZMat*(kYMat*kXMat);
1389  return *this;
1390 }
1391 
1392 template <class Real>
1393 bool moMatrix3<Real>::ToEulerAnglesXYZ (Real& rfXAngle, Real& rfYAngle,
1394  Real& rfZAngle) const
1395 {
1396  // rot = cy*cz -cy*sz sy
1397  // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
1398  // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
1399 
1400  if (m_afEntry[2] < (Real)1.0)
1401  {
1402  if (m_afEntry[2] > -(Real)1.0)
1403  {
1404  rfXAngle = moMath<Real>::ATan2(-m_afEntry[5],m_afEntry[8]);
1405  rfYAngle = (Real)asin((double)m_afEntry[2]);
1406  rfZAngle = moMath<Real>::ATan2(-m_afEntry[1],m_afEntry[0]);
1407  return true;
1408  }
1409  else
1410  {
1411  // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
1412  rfXAngle = -moMath<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
1413  rfYAngle = -moMath<Real>::HALF_PI;
1414  rfZAngle = (Real)0.0;
1415  return false;
1416  }
1417  }
1418  else
1419  {
1420  // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
1421  rfXAngle = moMath<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
1422  rfYAngle = moMath<Real>::HALF_PI;
1423  rfZAngle = (Real)0.0;
1424  return false;
1425  }
1426 }
1427 
1428 template <class Real>
1429 bool moMatrix3<Real>::ToEulerAnglesXZY (Real& rfXAngle, Real& rfZAngle,
1430  Real& rfYAngle) const
1431 {
1432  // rot = cy*cz -sz cz*sy
1433  // sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz
1434  // -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz
1435 
1436  if (m_afEntry[1] < (Real)1.0)
1437  {
1438  if (m_afEntry[1] > -(Real)1.0)
1439  {
1440  rfXAngle = moMath<Real>::ATan2(m_afEntry[7],m_afEntry[4]);
1441  rfZAngle = (Real)asin(-(double)m_afEntry[1]);
1442  rfYAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
1443  return true;
1444  }
1445  else
1446  {
1447  // WARNING. Not unique. XA - YA = atan2(r20,r22)
1448  rfXAngle = moMath<Real>::ATan2(m_afEntry[6],m_afEntry[8]);
1449  rfZAngle = moMath<Real>::HALF_PI;
1450  rfYAngle = (Real)0.0;
1451  return false;
1452  }
1453  }
1454  else
1455  {
1456  // WARNING. Not unique. XA + YA = atan2(-r20,r22)
1457  rfXAngle = moMath<Real>::ATan2(-m_afEntry[6],m_afEntry[8]);
1458  rfZAngle = -moMath<Real>::HALF_PI;
1459  rfYAngle = (Real)0.0;
1460  return false;
1461  }
1462 }
1463 
1464 template <class Real>
1465 bool moMatrix3<Real>::ToEulerAnglesYXZ (Real& rfYAngle, Real& rfXAngle,
1466  Real& rfZAngle) const
1467 {
1468  // rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy
1469  // cx*sz cx*cz -sx
1470  // -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy
1471 
1472  if (m_afEntry[5] < (Real)1.0)
1473  {
1474  if (m_afEntry[5] > -(Real)1.0)
1475  {
1476  rfYAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[8]);
1477  rfXAngle = (Real)asin(-(double)m_afEntry[5]);
1478  rfZAngle = moMath<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
1479  return true;
1480  }
1481  else
1482  {
1483  // WARNING. Not unique. YA - ZA = atan2(r01,r00)
1484  rfYAngle = moMath<Real>::ATan2(m_afEntry[1],m_afEntry[0]);
1485  rfXAngle = moMath<Real>::HALF_PI;
1486  rfZAngle = (Real)0.0;
1487  return false;
1488  }
1489  }
1490  else
1491  {
1492  // WARNING. Not unique. YA + ZA = atan2(-r01,r00)
1493  rfYAngle = moMath<Real>::ATan2(-m_afEntry[1],m_afEntry[0]);
1494  rfXAngle = -moMath<Real>::HALF_PI;
1495  rfZAngle = (Real)0.0;
1496  return false;
1497  }
1498 }
1499 
1500 template <class Real>
1501 bool moMatrix3<Real>::ToEulerAnglesYZX (Real& rfYAngle, Real& rfZAngle,
1502  Real& rfXAngle) const
1503 {
1504  // rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz
1505  // sz cx*cz -cz*sx
1506  // -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz
1507 
1508  if (m_afEntry[3] < (Real)1.0)
1509  {
1510  if (m_afEntry[3] > -(Real)1.0)
1511  {
1512  rfYAngle = moMath<Real>::ATan2(-m_afEntry[6],m_afEntry[0]);
1513  rfZAngle = (Real)asin((double)m_afEntry[3]);
1514  rfXAngle = moMath<Real>::ATan2(-m_afEntry[5],m_afEntry[4]);
1515  return true;
1516  }
1517  else
1518  {
1519  // WARNING. Not unique. YA - XA = -atan2(r21,r22);
1520  rfYAngle = -moMath<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
1521  rfZAngle = -moMath<Real>::HALF_PI;
1522  rfXAngle = (Real)0.0;
1523  return false;
1524  }
1525  }
1526  else
1527  {
1528  // WARNING. Not unique. YA + XA = atan2(r21,r22)
1529  rfYAngle = moMath<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
1530  rfZAngle = moMath<Real>::HALF_PI;
1531  rfXAngle = (Real)0.0;
1532  return false;
1533  }
1534 }
1535 
1536 template <class Real>
1537 bool moMatrix3<Real>::ToEulerAnglesZXY (Real& rfZAngle, Real& rfXAngle,
1538  Real& rfYAngle) const
1539 {
1540  // rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz
1541  // cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz
1542  // -cx*sy sx cx*cy
1543 
1544  if (m_afEntry[7] < (Real)1.0)
1545  {
1546  if (m_afEntry[7] > -(Real)1.0)
1547  {
1548  rfZAngle = moMath<Real>::ATan2(-m_afEntry[1],m_afEntry[4]);
1549  rfXAngle = (Real)asin((double)m_afEntry[7]);
1550  rfYAngle = moMath<Real>::ATan2(-m_afEntry[6],m_afEntry[8]);
1551  return true;
1552  }
1553  else
1554  {
1555  // WARNING. Not unique. ZA - YA = -atan(r02,r00)
1556  rfZAngle = -moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
1557  rfXAngle = -moMath<Real>::HALF_PI;
1558  rfYAngle = (Real)0.0;
1559  return false;
1560  }
1561  }
1562  else
1563  {
1564  // WARNING. Not unique. ZA + YA = atan2(r02,r00)
1565  rfZAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
1566  rfXAngle = moMath<Real>::HALF_PI;
1567  rfYAngle = (Real)0.0;
1568  return false;
1569  }
1570 }
1571 
1572 template <class Real>
1573 bool moMatrix3<Real>::ToEulerAnglesZYX (Real& rfZAngle, Real& rfYAngle,
1574  Real& rfXAngle) const
1575 {
1576  // rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz
1577  // cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
1578  // -sy cy*sx cx*cy
1579 
1580  if (m_afEntry[6] < (Real)1.0)
1581  {
1582  if (m_afEntry[6] > -(Real)1.0)
1583  {
1584  rfZAngle = moMath<Real>::ATan2(m_afEntry[3],m_afEntry[0]);
1585  rfYAngle = (Real)asin(-(double)m_afEntry[6]);
1586  rfXAngle = moMath<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
1587  return true;
1588  }
1589  else
1590  {
1591  // WARNING. Not unique. ZA - XA = -atan2(r01,r02)
1592  rfZAngle = -moMath<Real>::ATan2(m_afEntry[1],m_afEntry[2]);
1593  rfYAngle = moMath<Real>::HALF_PI;
1594  rfXAngle = (Real)0.0;
1595  return false;
1596  }
1597  }
1598  else
1599  {
1600  // WARNING. Not unique. ZA + XA = atan2(-r01,-r02)
1601  rfZAngle = moMath<Real>::ATan2(-m_afEntry[1],-m_afEntry[2]);
1602  rfYAngle = -moMath<Real>::HALF_PI;
1603  rfXAngle = (Real)0.0;
1604  return false;
1605  }
1606 }
1607 
1608 template <class Real>
1609 moMatrix3<Real>& moMatrix3<Real>::Slerp (Real fT, const moMatrix3& rkR0,
1610  const moMatrix3& rkR1)
1611 {
1612  moVector3<Real> kAxis;
1613  Real fAngle;
1614  moMatrix3 kProd = rkR0.TransposeTimes(rkR1);
1615  kProd.ToAxisAngle(kAxis,fAngle);
1616  FromAxisAngle(kAxis,fT*fAngle);
1617  return *this;
1618 }
1619 
1620 template <class Real>
1621 bool moMatrix3<Real>::Tridiagonalize (Real afDiag[3], Real afSubd[2])
1622 {
1623  // Householder reduction T = Q^t M Q
1624  // Input:
1625  // mat, symmetric 3x3 matrix M
1626  // Output:
1627  // mat, orthogonal matrix Q (a reflection)
1628  // diag, diagonal entries of T
1629  // subd, subdiagonal entries of T (T is symmetric)
1630 
1631  Real fM00 = m_afEntry[0];
1632  Real fM01 = m_afEntry[1];
1633  Real fM02 = m_afEntry[2];
1634  Real fM11 = m_afEntry[4];
1635  Real fM12 = m_afEntry[5];
1636  Real fM22 = m_afEntry[8];
1637 
1638  afDiag[0] = fM00;
1639  if (moMath<Real>::FAbs(fM02) >= moMath<Real>::ZERO_TOLERANCE)
1640  {
1641  afSubd[0] = moMath<Real>::Sqrt(fM01*fM01+fM02*fM02);
1642  Real fInvLength = ((Real)1.0)/afSubd[0];
1643  fM01 *= fInvLength;
1644  fM02 *= fInvLength;
1645  Real fTmp = ((Real)2.0)*fM01*fM12+fM02*(fM22-fM11);
1646  afDiag[1] = fM11+fM02*fTmp;
1647  afDiag[2] = fM22-fM02*fTmp;
1648  afSubd[1] = fM12-fM01*fTmp;
1649 
1650  m_afEntry[0] = (Real)1.0;
1651  m_afEntry[1] = (Real)0.0;
1652  m_afEntry[2] = (Real)0.0;
1653  m_afEntry[3] = (Real)0.0;
1654  m_afEntry[4] = fM01;
1655  m_afEntry[5] = fM02;
1656  m_afEntry[6] = (Real)0.0;
1657  m_afEntry[7] = fM02;
1658  m_afEntry[8] = -fM01;
1659  return true;
1660  }
1661  else
1662  {
1663  afDiag[1] = fM11;
1664  afDiag[2] = fM22;
1665  afSubd[0] = fM01;
1666  afSubd[1] = fM12;
1667 
1668  m_afEntry[0] = (Real)1.0;
1669  m_afEntry[1] = (Real)0.0;
1670  m_afEntry[2] = (Real)0.0;
1671  m_afEntry[3] = (Real)0.0;
1672  m_afEntry[4] = (Real)1.0;
1673  m_afEntry[5] = (Real)0.0;
1674  m_afEntry[6] = (Real)0.0;
1675  m_afEntry[7] = (Real)0.0;
1676  m_afEntry[8] = (Real)1.0;
1677  return false;
1678  }
1679 }
1680 
1681 template <class Real>
1682 bool moMatrix3<Real>::QLAlgorithm (Real afDiag[3], Real afSubd[2])
1683 {
1684  // This is an implementation of the symmetric QR algorithm from the book
1685  // "Matrix Computations" by Gene H. Golub and Charles F. Van Loan, second
1686  // edition. The algorithm is 8.2.3. The implementation has a slight
1687  // variation to actually make it a QL algorithm, and it traps the case
1688  // when either of the subdiagonal terms s0 or s1 is zero and reduces the
1689  // 2-by-2 subblock directly.
1690 
1691  const int iMax = 32;
1692  for (int i = 0; i < iMax; i++)
1693  {
1694  Real fSum, fDiff, fDiscr, fEValue0, fEValue1, fCos, fSin, fTmp;
1695  int iRow;
1696 
1697  fSum = moMath<Real>::FAbs(afDiag[0]) + moMath<Real>::FAbs(afDiag[1]);
1698  if (moMath<Real>::FAbs(afSubd[0]) + fSum == fSum)
1699  {
1700  // The matrix is effectively
1701  // +- -+
1702  // M = | d0 0 0 |
1703  // | 0 d1 s1 |
1704  // | 0 s1 d2 |
1705  // +- -+
1706 
1707  // Compute the eigenvalues as roots of a quadratic equation.
1708  fSum = afDiag[1] + afDiag[2];
1709  fDiff = afDiag[1] - afDiag[2];
1710  fDiscr = moMath<Real>::Sqrt(fDiff*fDiff +
1711  ((Real)4.0)*afSubd[1]*afSubd[1]);
1712  fEValue0 = ((Real)0.5)*(fSum - fDiscr);
1713  fEValue1 = ((Real)0.5)*(fSum + fDiscr);
1714 
1715  // Compute the Givens rotation.
1716  if (fDiff >= (Real)0.0)
1717  {
1718  fCos = afSubd[1];
1719  fSin = afDiag[1] - fEValue0;
1720  }
1721  else
1722  {
1723  fCos = afDiag[2] - fEValue0;
1724  fSin = afSubd[1];
1725  }
1726  fTmp = moMath<Real>::InvSqrt(fCos*fCos + fSin*fSin);
1727  fCos *= fTmp;
1728  fSin *= fTmp;
1729 
1730  // Postmultiply the current orthogonal matrix with the Givens
1731  // rotation.
1732  for (iRow = 0; iRow < 3; iRow++)
1733  {
1734  fTmp = m_afEntry[2+3*iRow];
1735  m_afEntry[2+3*iRow] = fSin*m_afEntry[1+3*iRow] + fCos*fTmp;
1736  m_afEntry[1+3*iRow] = fCos*m_afEntry[1+3*iRow] - fSin*fTmp;
1737  }
1738 
1739  // Update the tridiagonal matrix.
1740  afDiag[1] = fEValue0;
1741  afDiag[2] = fEValue1;
1742  afSubd[0] = (Real)0.0;
1743  afSubd[1] = (Real)0.0;
1744  return true;
1745  }
1746 
1747  fSum = moMath<Real>::FAbs(afDiag[1]) + moMath<Real>::FAbs(afDiag[2]);
1748  if (moMath<Real>::FAbs(afSubd[1]) + fSum == fSum)
1749  {
1750  // The matrix is effectively
1751  // +- -+
1752  // M = | d0 s0 0 |
1753  // | s0 d1 0 |
1754  // | 0 0 d2 |
1755  // +- -+
1756 
1757  // Compute the eigenvalues as roots of a quadratic equation.
1758  fSum = afDiag[0] + afDiag[1];
1759  fDiff = afDiag[0] - afDiag[1];
1760  fDiscr = moMath<Real>::Sqrt(fDiff*fDiff +
1761  ((Real)4.0)*afSubd[0]*afSubd[0]);
1762  fEValue0 = ((Real)0.5)*(fSum - fDiscr);
1763  fEValue1 = ((Real)0.5)*(fSum + fDiscr);
1764 
1765  // Compute the Givens rotation.
1766  if (fDiff >= (Real)0.0)
1767  {
1768  fCos = afSubd[0];
1769  fSin = afDiag[0] - fEValue0;
1770  }
1771  else
1772  {
1773  fCos = afDiag[1] - fEValue0;
1774  fSin = afSubd[0];
1775  }
1776  fTmp = moMath<Real>::InvSqrt(fCos*fCos + fSin*fSin);
1777  fCos *= fTmp;
1778  fSin *= fTmp;
1779 
1780  // Postmultiply the current orthogonal matrix with the Givens
1781  // rotation.
1782  for (iRow = 0; iRow < 3; iRow++)
1783  {
1784  fTmp = m_afEntry[1+3*iRow];
1785  m_afEntry[1+3*iRow] = fSin*m_afEntry[0+3*iRow] + fCos*fTmp;
1786  m_afEntry[0+3*iRow] = fCos*m_afEntry[0+3*iRow] - fSin*fTmp;
1787  }
1788 
1789  // Update the tridiagonal matrix.
1790  afDiag[0] = fEValue0;
1791  afDiag[1] = fEValue1;
1792  afSubd[0] = (Real)0.0;
1793  afSubd[1] = (Real)0.0;
1794  return true;
1795  }
1796 
1797  // The matrix is
1798  // +- -+
1799  // M = | d0 s0 0 |
1800  // | s0 d1 s1 |
1801  // | 0 s1 d2 |
1802  // +- -+
1803 
1804  // Set up the parameters for the first pass of the QL step. The
1805  // value for A is the difference between diagonal term D[2] and the
1806  // implicit shift suggested by Wilkinson.
1807  Real fRatio = (afDiag[1]-afDiag[0])/(((Real)2.0f)*afSubd[0]);
1808  Real fRoot = moMath<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
1809  Real fB = afSubd[1];
1810  Real fA = afDiag[2] - afDiag[0];
1811  if (fRatio >= (Real)0.0)
1812  {
1813  fA += afSubd[0]/(fRatio + fRoot);
1814  }
1815  else
1816  {
1817  fA += afSubd[0]/(fRatio - fRoot);
1818  }
1819 
1820  // Compute the Givens rotation for the first pass.
1821  if (moMath<Real>::FAbs(fB) >= moMath<Real>::FAbs(fA))
1822  {
1823  fRatio = fA/fB;
1824  fSin = moMath<Real>::InvSqrt((Real)1.0 + fRatio*fRatio);
1825  fCos = fRatio*fSin;
1826  }
1827  else
1828  {
1829  fRatio = fB/fA;
1830  fCos = moMath<Real>::InvSqrt((Real)1.0 + fRatio*fRatio);
1831  fSin = fRatio*fCos;
1832  }
1833 
1834  // Postmultiply the current orthogonal matrix with the Givens
1835  // rotation.
1836  for (iRow = 0; iRow < 3; iRow++)
1837  {
1838  fTmp = m_afEntry[2+3*iRow];
1839  m_afEntry[2+3*iRow] = fSin*m_afEntry[1+3*iRow]+fCos*fTmp;
1840  m_afEntry[1+3*iRow] = fCos*m_afEntry[1+3*iRow]-fSin*fTmp;
1841  }
1842 
1843  // Set up the parameters for the second pass of the QL step. The
1844  // values tmp0 and tmp1 are required to fully update the tridiagonal
1845  // matrix at the end of the second pass.
1846  Real fTmp0 = (afDiag[1] - afDiag[2])*fSin +
1847  ((Real)2.0)*afSubd[1]*fCos;
1848  Real fTmp1 = fCos*afSubd[0];
1849  fB = fSin*afSubd[0];
1850  fA = fCos*fTmp0 - afSubd[1];
1851  fTmp0 *= fSin;
1852 
1853  // Compute the Givens rotation for the second pass. The subdiagonal
1854  // term S[1] in the tridiagonal matrix is updated at this time.
1855  if (moMath<Real>::FAbs(fB) >= moMath<Real>::FAbs(fA))
1856  {
1857  fRatio = fA/fB;
1858  fRoot = moMath<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
1859  afSubd[1] = fB*fRoot;
1860  fSin = ((Real)1.0)/fRoot;
1861  fCos = fRatio*fSin;
1862  }
1863  else
1864  {
1865  fRatio = fB/fA;
1866  fRoot = moMath<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
1867  afSubd[1] = fA*fRoot;
1868  fCos = ((Real)1.0)/fRoot;
1869  fSin = fRatio*fCos;
1870  }
1871 
1872  // Postmultiply the current orthogonal matrix with the Givens
1873  // rotation.
1874  for (iRow = 0; iRow < 3; iRow++)
1875  {
1876  fTmp = m_afEntry[1+3*iRow];
1877  m_afEntry[1+3*iRow] = fSin*m_afEntry[0+3*iRow]+fCos*fTmp;
1878  m_afEntry[0+3*iRow] = fCos*m_afEntry[0+3*iRow]-fSin*fTmp;
1879  }
1880 
1881  // Complete the update of the tridiagonal matrix.
1882  Real fTmp2 = afDiag[1] - fTmp0;
1883  afDiag[2] += fTmp0;
1884  fTmp0 = (afDiag[0] - fTmp2)*fSin + ((Real)2.0)*fTmp1*fCos;
1885  afSubd[0] = fCos*fTmp0 - fTmp1;
1886  fTmp0 *= fSin;
1887  afDiag[1] = fTmp2 + fTmp0;
1888  afDiag[0] -= fTmp0;
1889  }
1890  return false;
1891 }
1892 
1893 template <class Real>
1894 void moMatrix3<Real>::Bidiagonalize (moMatrix3& rkA, moMatrix3& rkL, moMatrix3& rkR)
1895 {
1896  Real afV[3], afW[3];
1897  Real fLength, fSign, fT1, fInvT1, fT2;
1898  bool bIdentity;
1899 
1900  // map first column to (*,0,0)
1901  fLength = moMath<Real>::Sqrt(rkA[0][0]*rkA[0][0] + rkA[1][0]*rkA[1][0] +
1902  rkA[2][0]*rkA[2][0]);
1903  if (fLength > (Real)0.0)
1904  {
1905  fSign = (rkA[0][0] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
1906  fT1 = rkA[0][0] + fSign*fLength;
1907  fInvT1 = ((Real)1.0)/fT1;
1908  afV[1] = rkA[1][0]*fInvT1;
1909  afV[2] = rkA[2][0]*fInvT1;
1910 
1911  fT2 = -((Real)2.0)/(((Real)1.0)+afV[1]*afV[1]+afV[2]*afV[2]);
1912  afW[0] = fT2*(rkA[0][0]+rkA[1][0]*afV[1]+rkA[2][0]*afV[2]);
1913  afW[1] = fT2*(rkA[0][1]+rkA[1][1]*afV[1]+rkA[2][1]*afV[2]);
1914  afW[2] = fT2*(rkA[0][2]+rkA[1][2]*afV[1]+rkA[2][2]*afV[2]);
1915  rkA[0][0] += afW[0];
1916  rkA[0][1] += afW[1];
1917  rkA[0][2] += afW[2];
1918  rkA[1][1] += afV[1]*afW[1];
1919  rkA[1][2] += afV[1]*afW[2];
1920  rkA[2][1] += afV[2]*afW[1];
1921  rkA[2][2] += afV[2]*afW[2];
1922 
1923  rkL[0][0] = ((Real)1.0)+fT2;
1924  rkL[0][1] = fT2*afV[1];
1925  rkL[1][0] = rkL[0][1];
1926  rkL[0][2] = fT2*afV[2];
1927  rkL[2][0] = rkL[0][2];
1928  rkL[1][1] = ((Real)1.0)+fT2*afV[1]*afV[1];
1929  rkL[1][2] = fT2*afV[1]*afV[2];
1930  rkL[2][1] = rkL[1][2];
1931  rkL[2][2] = ((Real)1.0)+fT2*afV[2]*afV[2];
1932  bIdentity = false;
1933  }
1934  else
1935  {
1936  rkL = moMatrix3::IDENTITY;
1937  bIdentity = true;
1938  }
1939 
1940  // map first row to (*,*,0)
1941  fLength = moMath<Real>::Sqrt(rkA[0][1]*rkA[0][1]+rkA[0][2]*rkA[0][2]);
1942  if (fLength > (Real)0.0)
1943  {
1944  fSign = (rkA[0][1] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
1945  fT1 = rkA[0][1] + fSign*fLength;
1946  afV[2] = rkA[0][2]/fT1;
1947 
1948  fT2 = -((Real)2.0)/(((Real)1.0)+afV[2]*afV[2]);
1949  afW[0] = fT2*(rkA[0][1]+rkA[0][2]*afV[2]);
1950  afW[1] = fT2*(rkA[1][1]+rkA[1][2]*afV[2]);
1951  afW[2] = fT2*(rkA[2][1]+rkA[2][2]*afV[2]);
1952  rkA[0][1] += afW[0];
1953  rkA[1][1] += afW[1];
1954  rkA[1][2] += afW[1]*afV[2];
1955  rkA[2][1] += afW[2];
1956  rkA[2][2] += afW[2]*afV[2];
1957 
1958  rkR[0][0] = (Real)1.0;
1959  rkR[0][1] = (Real)0.0;
1960  rkR[1][0] = (Real)0.0;
1961  rkR[0][2] = (Real)0.0;
1962  rkR[2][0] = (Real)0.0;
1963  rkR[1][1] = ((Real)1.0)+fT2;
1964  rkR[1][2] = fT2*afV[2];
1965  rkR[2][1] = rkR[1][2];
1966  rkR[2][2] = ((Real)1.0)+fT2*afV[2]*afV[2];
1967  }
1968  else
1969  {
1970  rkR = moMatrix3::IDENTITY;
1971  }
1972 
1973  // map second column to (*,*,0)
1974  fLength = moMath<Real>::Sqrt(rkA[1][1]*rkA[1][1]+rkA[2][1]*rkA[2][1]);
1975  if (fLength > (Real)0.0)
1976  {
1977  fSign = (rkA[1][1] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
1978  fT1 = rkA[1][1] + fSign*fLength;
1979  afV[2] = rkA[2][1]/fT1;
1980 
1981  fT2 = -((Real)2.0)/(((Real)1.0)+afV[2]*afV[2]);
1982  afW[1] = fT2*(rkA[1][1]+rkA[2][1]*afV[2]);
1983  afW[2] = fT2*(rkA[1][2]+rkA[2][2]*afV[2]);
1984  rkA[1][1] += afW[1];
1985  rkA[1][2] += afW[2];
1986  rkA[2][2] += afV[2]*afW[2];
1987 
1988  Real fA = ((Real)1.0)+fT2;
1989  Real fB = fT2*afV[2];
1990  Real fC = ((Real)1.0)+fB*afV[2];
1991 
1992  if (bIdentity)
1993  {
1994  rkL[0][0] = (Real)1.0;
1995  rkL[0][1] = (Real)0.0;
1996  rkL[1][0] = (Real)0.0;
1997  rkL[0][2] = (Real)0.0;
1998  rkL[2][0] = (Real)0.0;
1999  rkL[1][1] = fA;
2000  rkL[1][2] = fB;
2001  rkL[2][1] = fB;
2002  rkL[2][2] = fC;
2003  }
2004  else
2005  {
2006  for (int iRow = 0; iRow < 3; iRow++)
2007  {
2008  Real fTmp0 = rkL[iRow][1];
2009  Real fTmp1 = rkL[iRow][2];
2010  rkL[iRow][1] = fA*fTmp0+fB*fTmp1;
2011  rkL[iRow][2] = fB*fTmp0+fC*fTmp1;
2012  }
2013  }
2014  }
2015 }
2016 
2017 template <class Real>
2018 void moMatrix3<Real>::GolubKahanStep (moMatrix3& rkA, moMatrix3& rkL, moMatrix3& rkR)
2019 {
2020  Real fT11 = rkA[0][1]*rkA[0][1]+rkA[1][1]*rkA[1][1];
2021  Real fT22 = rkA[1][2]*rkA[1][2]+rkA[2][2]*rkA[2][2];
2022  Real fT12 = rkA[1][1]*rkA[1][2];
2023  Real fTrace = fT11+fT22;
2024  Real fDiff = fT11-fT22;
2025  Real fDiscr = moMath<Real>::Sqrt(fDiff*fDiff+((Real)4.0)*fT12*fT12);
2026  Real fRoot1 = ((Real)0.5)*(fTrace+fDiscr);
2027  Real fRoot2 = ((Real)0.5)*(fTrace-fDiscr);
2028 
2029  // adjust right
2030  Real fY = rkA[0][0] - (moMath<Real>::FAbs(fRoot1-fT22) <=
2031  moMath<Real>::FAbs(fRoot2-fT22) ? fRoot1 : fRoot2);
2032  Real fZ = rkA[0][1];
2033  Real fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2034  Real fSin = fZ*fInvLength;
2035  Real fCos = -fY*fInvLength;
2036 
2037  Real fTmp0 = rkA[0][0];
2038  Real fTmp1 = rkA[0][1];
2039  rkA[0][0] = fCos*fTmp0-fSin*fTmp1;
2040  rkA[0][1] = fSin*fTmp0+fCos*fTmp1;
2041  rkA[1][0] = -fSin*rkA[1][1];
2042  rkA[1][1] *= fCos;
2043 
2044  int iRow;
2045  for (iRow = 0; iRow < 3; iRow++)
2046  {
2047  fTmp0 = rkR[0][iRow];
2048  fTmp1 = rkR[1][iRow];
2049  rkR[0][iRow] = fCos*fTmp0-fSin*fTmp1;
2050  rkR[1][iRow] = fSin*fTmp0+fCos*fTmp1;
2051  }
2052 
2053  // adjust left
2054  fY = rkA[0][0];
2055  fZ = rkA[1][0];
2056  fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2057  fSin = fZ*fInvLength;
2058  fCos = -fY*fInvLength;
2059 
2060  rkA[0][0] = fCos*rkA[0][0]-fSin*rkA[1][0];
2061  fTmp0 = rkA[0][1];
2062  fTmp1 = rkA[1][1];
2063  rkA[0][1] = fCos*fTmp0-fSin*fTmp1;
2064  rkA[1][1] = fSin*fTmp0+fCos*fTmp1;
2065  rkA[0][2] = -fSin*rkA[1][2];
2066  rkA[1][2] *= fCos;
2067 
2068  int iCol;
2069  for (iCol = 0; iCol < 3; iCol++)
2070  {
2071  fTmp0 = rkL[iCol][0];
2072  fTmp1 = rkL[iCol][1];
2073  rkL[iCol][0] = fCos*fTmp0-fSin*fTmp1;
2074  rkL[iCol][1] = fSin*fTmp0+fCos*fTmp1;
2075  }
2076 
2077  // adjust right
2078  fY = rkA[0][1];
2079  fZ = rkA[0][2];
2080  fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2081  fSin = fZ*fInvLength;
2082  fCos = -fY*fInvLength;
2083 
2084  rkA[0][1] = fCos*rkA[0][1]-fSin*rkA[0][2];
2085  fTmp0 = rkA[1][1];
2086  fTmp1 = rkA[1][2];
2087  rkA[1][1] = fCos*fTmp0-fSin*fTmp1;
2088  rkA[1][2] = fSin*fTmp0+fCos*fTmp1;
2089  rkA[2][1] = -fSin*rkA[2][2];
2090  rkA[2][2] *= fCos;
2091 
2092  for (iRow = 0; iRow < 3; iRow++)
2093  {
2094  fTmp0 = rkR[1][iRow];
2095  fTmp1 = rkR[2][iRow];
2096  rkR[1][iRow] = fCos*fTmp0-fSin*fTmp1;
2097  rkR[2][iRow] = fSin*fTmp0+fCos*fTmp1;
2098  }
2099 
2100  // adjust left
2101  fY = rkA[1][1];
2102  fZ = rkA[2][1];
2103  fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2104  fSin = fZ*fInvLength;
2105  fCos = -fY*fInvLength;
2106 
2107  rkA[1][1] = fCos*rkA[1][1]-fSin*rkA[2][1];
2108  fTmp0 = rkA[1][2];
2109  fTmp1 = rkA[2][2];
2110  rkA[1][2] = fCos*fTmp0-fSin*fTmp1;
2111  rkA[2][2] = fSin*fTmp0+fCos*fTmp1;
2112 
2113  for (iCol = 0; iCol < 3; iCol++)
2114  {
2115  fTmp0 = rkL[iCol][1];
2116  fTmp1 = rkL[iCol][2];
2117  rkL[iCol][1] = fCos*fTmp0-fSin*fTmp1;
2118  rkL[iCol][2] = fSin*fTmp0+fCos*fTmp1;
2119  }
2120 }
2121 
2122 template <class Real>
2123 void moMatrix3<Real>::SingularValueDecomposition (moMatrix3& rkL, moMatrix3& rkD,
2124  moMatrix3& rkRTranspose) const
2125 {
2126  int iRow, iCol;
2127 
2128  moMatrix3 kA = *this;
2129  Bidiagonalize(kA,rkL,rkRTranspose);
2130  rkD.MakeZero();
2131 
2132  const int iMax = 32;
2133  const Real fEpsilon = (Real)1e-04;
2134  for (int i = 0; i < iMax; i++)
2135  {
2136  Real fTmp, fTmp0, fTmp1;
2137  Real fSin0, fCos0, fTan0;
2138  Real fSin1, fCos1, fTan1;
2139 
2140  bool bTest1 = (moMath<Real>::FAbs(kA[0][1]) <=
2141  fEpsilon*(moMath<Real>::FAbs(kA[0][0]) +
2142  moMath<Real>::FAbs(kA[1][1])));
2143  bool bTest2 = (moMath<Real>::FAbs(kA[1][2]) <=
2144  fEpsilon*(moMath<Real>::FAbs(kA[1][1]) +
2145  moMath<Real>::FAbs(kA[2][2])));
2146  if (bTest1)
2147  {
2148  if (bTest2)
2149  {
2150  rkD[0][0] = kA[0][0];
2151  rkD[1][1] = kA[1][1];
2152  rkD[2][2] = kA[2][2];
2153  break;
2154  }
2155  else
2156  {
2157  // 2x2 closed form factorization
2158  fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] +
2159  kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]);
2160  fTan0 = ((Real)0.5)*(fTmp + moMath<Real>::Sqrt(fTmp*fTmp +
2161  ((Real)4.0)));
2162  fCos0 = moMath<Real>::InvSqrt(((Real)1.0)+fTan0*fTan0);
2163  fSin0 = fTan0*fCos0;
2164 
2165  for (iCol = 0; iCol < 3; iCol++)
2166  {
2167  fTmp0 = rkL[iCol][1];
2168  fTmp1 = rkL[iCol][2];
2169  rkL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1;
2170  rkL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1;
2171  }
2172 
2173  fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1];
2174  fCos1 = moMath<Real>::InvSqrt(((Real)1.0)+fTan1*fTan1);
2175  fSin1 = -fTan1*fCos1;
2176 
2177  for (iRow = 0; iRow < 3; iRow++)
2178  {
2179  fTmp0 = rkRTranspose[1][iRow];
2180  fTmp1 = rkRTranspose[2][iRow];
2181  rkRTranspose[1][iRow] = fCos1*fTmp0-fSin1*fTmp1;
2182  rkRTranspose[2][iRow] = fSin1*fTmp0+fCos1*fTmp1;
2183  }
2184 
2185  rkD[0][0] = kA[0][0];
2186  rkD[1][1] = fCos0*fCos1*kA[1][1] -
2187  fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]);
2188  rkD[2][2] = fSin0*fSin1*kA[1][1] +
2189  fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]);
2190  break;
2191  }
2192  }
2193  else
2194  {
2195  if (bTest2)
2196  {
2197  // 2x2 closed form factorization
2198  fTmp = (kA[0][0]*kA[0][0] + kA[1][1]*kA[1][1] -
2199  kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]);
2200  fTan0 = ((Real)0.5)*(-fTmp + moMath<Real>::Sqrt(fTmp*fTmp +
2201  ((Real)4.0)));
2202  fCos0 = moMath<Real>::InvSqrt(((Real)1.0)+fTan0*fTan0);
2203  fSin0 = fTan0*fCos0;
2204 
2205  for (iCol = 0; iCol < 3; iCol++)
2206  {
2207  fTmp0 = rkL[iCol][0];
2208  fTmp1 = rkL[iCol][1];
2209  rkL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1;
2210  rkL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1;
2211  }
2212 
2213  fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0];
2214  fCos1 = moMath<Real>::InvSqrt(((Real)1.0)+fTan1*fTan1);
2215  fSin1 = -fTan1*fCos1;
2216 
2217  for (iRow = 0; iRow < 3; iRow++)
2218  {
2219  fTmp0 = rkRTranspose[0][iRow];
2220  fTmp1 = rkRTranspose[1][iRow];
2221  rkRTranspose[0][iRow] = fCos1*fTmp0-fSin1*fTmp1;
2222  rkRTranspose[1][iRow] = fSin1*fTmp0+fCos1*fTmp1;
2223  }
2224 
2225  rkD[0][0] = fCos0*fCos1*kA[0][0] -
2226  fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]);
2227  rkD[1][1] = fSin0*fSin1*kA[0][0] +
2228  fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]);
2229  rkD[2][2] = kA[2][2];
2230  break;
2231  }
2232  else
2233  {
2234  GolubKahanStep(kA,rkL,rkRTranspose);
2235  }
2236  }
2237  }
2238 
2239  // Make the diagonal entries positive.
2240  for (iRow = 0; iRow < 3; iRow++)
2241  {
2242  if (rkD[iRow][iRow] < (Real)0.0)
2243  {
2244  rkD[iRow][iRow] = -rkD[iRow][iRow];
2245  for (iCol = 0; iCol < 3; iCol++)
2246  {
2247  rkRTranspose[iRow][iCol] = -rkRTranspose[iRow][iCol];
2248  }
2249  }
2250  }
2251 }
2252 
2253 template <class Real>
2254 void moMatrix3<Real>::SingularValueComposition (const moMatrix3& rkL,
2255  const moMatrix3& rkD, const moMatrix3& rkRTranspose)
2256 {
2257  *this = rkL*(rkD*rkRTranspose);
2258 }
2259 
2260 template <class Real>
2261 void moMatrix3<Real>::PolarDecomposition (moMatrix3& rkQ, moMatrix3& rkS)
2262 {
2263  // Decompose M = L*D*R^T.
2264  moMatrix3 kL, kD, kRTranspose;
2265  SingularValueDecomposition(kL,kD,kRTranspose);
2266 
2267  // Compute Q = L*R^T.
2268  rkQ = kL*kRTranspose;
2269 
2270  // Compute S = R*D*R^T.
2271  rkS = kRTranspose.TransposeTimes(kD*kRTranspose);
2272 
2273  // Numerical round-off errors can cause S not to be symmetric in the
2274  // sense that S[i][j] and S[j][i] are slightly different for i != j.
2275  // Correct this by averaging S and Transpose(S).
2276  rkS[0][1] = ((Real)0.5)*(rkS[0][1] + rkS[1][0]);
2277  rkS[1][0] = rkS[0][1];
2278  rkS[0][2] = ((Real)0.5)*(rkS[0][2] + rkS[2][0]);
2279  rkS[2][0] = rkS[0][2];
2280  rkS[1][2] = ((Real)0.5)*(rkS[1][2] + rkS[2][1]);
2281  rkS[2][1] = rkS[1][2];
2282 }
2283 
2284 template <class Real>
2285 void moMatrix3<Real>::QDUDecomposition (moMatrix3& rkQ, moMatrix3& rkD,
2286  moMatrix3& rkU) const
2287 {
2288  // Factor M = QR = QDU where Q is orthogonal (rotation), D is diagonal
2289  // (scaling), and U is upper triangular with ones on its diagonal
2290  // (shear). Algorithm uses Gram-Schmidt orthogonalization (the QR
2291  // algorithm).
2292  //
2293  // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
2294  //
2295  // q0 = m0/|m0|
2296  // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
2297  // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
2298  //
2299  // where |V| indicates length of vector V and A*B indicates dot
2300  // product of vectors A and B. The matrix R has entries
2301  //
2302  // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2
2303  // r10 = 0 r11 = q1*m1 r12 = q1*m2
2304  // r20 = 0 r21 = 0 r22 = q2*m2
2305  //
2306  // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
2307  // u02 = r02/r00, and u12 = r12/r11.
2308 
2309  // build orthogonal matrix Q
2310  Real fInvLength = moMath<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
2311  m_afEntry[3]*m_afEntry[3] + m_afEntry[6]*m_afEntry[6]);
2312  rkQ[0][0] = m_afEntry[0]*fInvLength;
2313  rkQ[1][0] = m_afEntry[3]*fInvLength;
2314  rkQ[2][0] = m_afEntry[6]*fInvLength;
2315 
2316  Real fDot = rkQ[0][0]*m_afEntry[1] + rkQ[1][0]*m_afEntry[4] +
2317  rkQ[2][0]*m_afEntry[7];
2318  rkQ[0][1] = m_afEntry[1]-fDot*rkQ[0][0];
2319  rkQ[1][1] = m_afEntry[4]-fDot*rkQ[1][0];
2320  rkQ[2][1] = m_afEntry[7]-fDot*rkQ[2][0];
2321  fInvLength = moMath<Real>::InvSqrt(rkQ[0][1]*rkQ[0][1] +
2322  rkQ[1][1]*rkQ[1][1] + rkQ[2][1]*rkQ[2][1]);
2323  rkQ[0][1] *= fInvLength;
2324  rkQ[1][1] *= fInvLength;
2325  rkQ[2][1] *= fInvLength;
2326 
2327  fDot = rkQ[0][0]*m_afEntry[2] + rkQ[1][0]*m_afEntry[5] +
2328  rkQ[2][0]*m_afEntry[8];
2329  rkQ[0][2] = m_afEntry[2]-fDot*rkQ[0][0];
2330  rkQ[1][2] = m_afEntry[5]-fDot*rkQ[1][0];
2331  rkQ[2][2] = m_afEntry[8]-fDot*rkQ[2][0];
2332  fDot = rkQ[0][1]*m_afEntry[2] + rkQ[1][1]*m_afEntry[5] +
2333  rkQ[2][1]*m_afEntry[8];
2334  rkQ[0][2] -= fDot*rkQ[0][1];
2335  rkQ[1][2] -= fDot*rkQ[1][1];
2336  rkQ[2][2] -= fDot*rkQ[2][1];
2337  fInvLength = moMath<Real>::InvSqrt(rkQ[0][2]*rkQ[0][2] +
2338  rkQ[1][2]*rkQ[1][2] + rkQ[2][2]*rkQ[2][2]);
2339  rkQ[0][2] *= fInvLength;
2340  rkQ[1][2] *= fInvLength;
2341  rkQ[2][2] *= fInvLength;
2342 
2343  // guarantee that orthogonal matrix has determinant 1 (no reflections)
2344  Real fDet = rkQ[0][0]*rkQ[1][1]*rkQ[2][2] + rkQ[0][1]*rkQ[1][2]*rkQ[2][0]
2345  + rkQ[0][2]*rkQ[1][0]*rkQ[2][1] - rkQ[0][2]*rkQ[1][1]*rkQ[2][0]
2346  - rkQ[0][1]*rkQ[1][0]*rkQ[2][2] - rkQ[0][0]*rkQ[1][2]*rkQ[2][1];
2347 
2348  if (fDet < (Real)0.0)
2349  {
2350  for (int iRow = 0; iRow < 3; iRow++)
2351  {
2352  for (int iCol = 0; iCol < 3; iCol++)
2353  {
2354  rkQ[iRow][iCol] = -rkQ[iRow][iCol];
2355  }
2356  }
2357  }
2358 
2359  // build "right" matrix R
2360  moMatrix3 kR;
2361  kR[0][0] = rkQ[0][0]*m_afEntry[0] + rkQ[1][0]*m_afEntry[3] +
2362  rkQ[2][0]*m_afEntry[6];
2363  kR[0][1] = rkQ[0][0]*m_afEntry[1] + rkQ[1][0]*m_afEntry[4] +
2364  rkQ[2][0]*m_afEntry[7];
2365  kR[1][1] = rkQ[0][1]*m_afEntry[1] + rkQ[1][1]*m_afEntry[4] +
2366  rkQ[2][1]*m_afEntry[7];
2367  kR[0][2] = rkQ[0][0]*m_afEntry[2] + rkQ[1][0]*m_afEntry[5] +
2368  rkQ[2][0]*m_afEntry[8];
2369  kR[1][2] = rkQ[0][1]*m_afEntry[2] + rkQ[1][1]*m_afEntry[5] +
2370  rkQ[2][1]*m_afEntry[8];
2371  kR[2][2] = rkQ[0][2]*m_afEntry[2] + rkQ[1][2]*m_afEntry[5] +
2372  rkQ[2][2]*m_afEntry[8];
2373 
2374  // the scaling component
2375  rkD.MakeDiagonal(kR[0][0],kR[1][1],kR[2][2]);
2376 
2377  // the shear component
2378  Real fInvD0 = ((Real)1.0)/rkD[0][0];
2379  rkU[0][0] = (Real)1.0;
2380  rkU[0][1] = kR[0][1]*fInvD0;
2381  rkU[0][2] = kR[0][2]*fInvD0;
2382  rkU[1][0] = (Real)0.0;
2383  rkU[1][1] = (Real)1.0;
2384  rkU[1][2] = kR[1][2]/rkD[1][1];
2385  rkU[2][0] = (Real)0.0;
2386  rkU[2][1] = (Real)0.0;
2387  rkU[2][2] = (Real)1.0;
2388 }
2389 */
2390 
2391 template<> const moMatrix3<MOfloat> moMatrix3<MOfloat>::ZERO(
2392  0.0f,0.0f,0.0f,
2393  0.0f,0.0f,0.0f,
2394  0.0f,0.0f,0.0f);
2395 template<> const moMatrix3<MOfloat> moMatrix3<MOfloat>::IDENTITY(
2396  1.0f,0.0f,0.0f,
2397  0.0f,1.0f,0.0f,
2398  0.0f,0.0f,1.0f);
2399 
2400 template<> const moMatrix3<MOdouble> moMatrix3<MOdouble>::ZERO(
2401  0.0,0.0,0.0,
2402  0.0,0.0,0.0,
2403  0.0,0.0,0.0);
2404 template<> const moMatrix3<MOdouble> moMatrix3<MOdouble>::IDENTITY(
2405  1.0,0.0,0.0,
2406  0.0,1.0,0.0,
2407  0.0,0.0,1.0);
2408 
2409 // momoMatrix4 class ------------------------------------------------------------
2410 
2411 
2412 template<> const moMatrix4<MOfloat> moMatrix4<MOfloat>::ZERO(
2413  0.0f,0.0f,0.0f,0.0f,
2414  0.0f,0.0f,0.0f,0.0f,
2415  0.0f,0.0f,0.0f,0.0f,
2416  0.0f,0.0f,0.0f,0.0f);
2417 template<> const moMatrix4<MOfloat> moMatrix4<MOfloat>::IDENTITY(
2418  1.0f,0.0f,0.0f,0.0f,
2419  0.0f,1.0f,0.0f,0.0f,
2420  0.0f,0.0f,1.0f,0.0f,
2421  0.0f,0.0f,0.0f,1.0f);
2422 
2423 template<> const moMatrix4<MOdouble> moMatrix4<MOdouble>::ZERO(
2424  0.0,0.0,0.0,0.0,
2425  0.0,0.0,0.0,0.0,
2426  0.0,0.0,0.0,0.0,
2427  0.0,0.0,0.0,0.0);
2428 template<> const moMatrix4<MOdouble> moMatrix4<MOdouble>::IDENTITY(
2429  1.0,0.0,0.0,0.0,
2430  0.0,1.0,0.0,0.0,
2431  0.0,0.0,1.0,0.0,
2432  0.0,0.0,0.0,1.0);
2433 
MOfloat
#define MOfloat
Definition: moTypes.h:403
moDefineDynamicArray
#define moDefineDynamicArray(name)
Definition: moArray.cpp:222
MOdouble
#define MOdouble
Definition: moTypes.h:404
moMatrix3
moMatrix3(const moMatrix3 &rkM)
Definition: moMathMatrix.h:669