libmoldeo (Moldeo 1.0 Core)  1.0
libmoldeo es el conjunto de objetos y funciones, que permiten ejecutar las operaciones básicas de la plataforma Moldeo, y que compone su núcleo.
 Todo Clases Namespaces Archivos Funciones Variables 'typedefs' Enumeraciones Valores de enumeraciones Amigas 'defines' Grupos Páginas
moMathMatrix.cpp
Ir a la documentación de este archivo.
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 
f
f
Definition: jquery.js:71
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
const
moPort const
Definition: all_14.js:19