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.h
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 // Matrix operations are applied on the left. For example, given a matrix M
39 // and a vector V, matrix-times-vector is M*V. That is, V is treated as a
40 // column vector. Some graphics APIs use V*M where V is treated as a row
41 // vector. In this context the "M" matrix is really a transpose of the M as
42 // represented in Wild Magic. Similarly, to apply two matrix operations M0
43 // and M1, in that order, you compute M1*M0 so that the transform of a vector
44 // is (M1*M0)*V = M1*(M0*V). Some graphics APIs use M0*M1, but again these
45 // matrices are the transpose of those as represented in Wild Magic. You
46 // must therefore be careful about how you interface the transformation code
47 // with graphics APIS.
48 //
49 // For memory organization it might seem natural to chose Real[N][N] for the
50 // matrix storage, but this can be a problem on a platform/console that
51 // chooses to store the data in column-major rather than row-major format.
52 // To avoid potential portability problems, the matrix is stored as Real[N*N]
53 // and organized in row-major order. That is, the entry of the matrix in row
54 // r (0 <= r < N) and column c (0 <= c < N) is stored at index i = c+N*r
55 // (0 <= i < N*N).
56 
57 // Rotation matrices are of the form
58 // R = cos(t) -sin(t)
59 // sin(t) cos(t)
60 // where t > 0 indicates a counterclockwise rotation in the xy-plane.
61 
62 // The (x,y,z) coordinate system is assumed to be right-handed. Coordinate
63 // axis rotation matrices are of the form
64 // RX = 1 0 0
65 // 0 cos(t) -sin(t)
66 // 0 sin(t) cos(t)
67 // where t > 0 indicates a counterclockwise rotation in the yz-plane
68 // RY = cos(t) 0 sin(t)
69 // 0 1 0
70 // -sin(t) 0 cos(t)
71 // where t > 0 indicates a counterclockwise rotation in the zx-plane
72 // RZ = cos(t) -sin(t) 0
73 // sin(t) cos(t) 0
74 // 0 0 1
75 // where t > 0 indicates a counterclockwise rotation in the xy-plane.
76 
77 #ifndef __MO_MATH_MATRIX_H__
78 #define __MO_MATH_MATRIX_H__
79 
80 #include "moMathVector.h"
81 #include "moMathVector3.h"
82 #include "moMathVector4.h"
83 
84 // moMatrix2 class ------------------------------------------------------------
85 
86 template <class Real>
87 class LIBMOLDEO_API moMatrix2 : public moAbstract
88 {
89 public:
90 
91 moMatrix2 (bool bZero = true )
92 {
93  if (bZero)
94  {
95  MakeZero();
96  }
97  else
98  {
99  MakeIdentity();
100  }
101 }
102 
103 moMatrix2 (const moMatrix2& rkM) : moAbstract()
104 {
105  m_bInitialized = rkM.m_bInitialized;
106  m_afEntry[0] = rkM.m_afEntry[0];
107  m_afEntry[1] = rkM.m_afEntry[1];
108  m_afEntry[2] = rkM.m_afEntry[2];
109  m_afEntry[3] = rkM.m_afEntry[3];
110 }
111 
112 moMatrix2 (Real fM00, Real fM01, Real fM10, Real fM11)
113 {
114  m_afEntry[0] = fM00;
115  m_afEntry[1] = fM01;
116  m_afEntry[2] = fM10;
117  m_afEntry[3] = fM11;
118 }
119 
120 moMatrix2 (const Real afEntry[4], bool bRowMajor)
121 {
122  if (bRowMajor)
123  {
124  m_afEntry[0] = afEntry[0];
125  m_afEntry[1] = afEntry[1];
126  m_afEntry[2] = afEntry[2];
127  m_afEntry[3] = afEntry[3];
128  }
129  else
130  {
131  m_afEntry[0] = afEntry[0];
132  m_afEntry[1] = afEntry[2];
133  m_afEntry[2] = afEntry[1];
134  m_afEntry[3] = afEntry[3];
135  }
136 }
137 
138 moMatrix2 (const moVector2<Real>& rkU, const moVector2<Real>& rkV,
139  bool bColumns)
140 {
141  if (bColumns)
142  {
143  m_afEntry[0] = rkU[0];
144  m_afEntry[1] = rkV[0];
145  m_afEntry[2] = rkU[1];
146  m_afEntry[3] = rkV[1];
147  }
148  else
149  {
150  m_afEntry[0] = rkU[0];
151  m_afEntry[1] = rkU[1];
152  m_afEntry[2] = rkV[0];
153  m_afEntry[3] = rkV[1];
154  }
155 }
156 
157 moMatrix2 (const moVector2<Real>* akV, bool bColumns)
158 {
159  if (bColumns)
160  {
161  m_afEntry[0] = akV[0][0];
162  m_afEntry[1] = akV[1][0];
163  m_afEntry[2] = akV[0][1];
164  m_afEntry[3] = akV[1][1];
165  }
166  else
167  {
168  m_afEntry[0] = akV[0][0];
169  m_afEntry[1] = akV[0][1];
170  m_afEntry[2] = akV[1][0];
171  m_afEntry[3] = akV[1][1];
172  }
173 }
174 
175 moMatrix2 (Real fM00, Real fM11)
176 {
177  MakeDiagonal(fM00,fM11);
178 }
179 
180 moMatrix2 (Real fAngle)
181 {
182  FromAngle(fAngle);
183 }
184 
185 
186 moMatrix2 (const moVector2<Real>& rkU, const moVector2<Real>& rkV)
187 {
188  MakeTensorProduct(rkU,rkV);
189 }
190 
191 
192 void MakeZero ()
193 {
194  m_afEntry[0] = (Real)0.0;
195  m_afEntry[1] = (Real)0.0;
196  m_afEntry[2] = (Real)0.0;
197  m_afEntry[3] = (Real)0.0;
198 }
199 
200 
201 void MakeIdentity ()
202 {
203  m_afEntry[0] = (Real)1.0;
204  m_afEntry[1] = (Real)0.0;
205  m_afEntry[2] = (Real)0.0;
206  m_afEntry[3] = (Real)1.0;
207 }
208 
209 
210 void MakeDiagonal (Real fM00, Real fM11)
211 {
212  m_afEntry[0] = fM00;
213  m_afEntry[1] = (Real)0.0;
214  m_afEntry[2] = (Real)0.0;
215  m_afEntry[3] = fM11;
216 }
217 
218 
219 void FromAngle (Real fAngle)
220 {
221  m_afEntry[0] = moMath<Real>::Cos(fAngle);
222  m_afEntry[2] = moMath<Real>::Sin(fAngle);
223  m_afEntry[1] = -m_afEntry[2];
224  m_afEntry[3] = m_afEntry[0];
225 }
226 
227 
228 void MakeTensorProduct (const moVector2<Real>& rkU,
229  const moVector2<Real>& rkV)
230 {
231  m_afEntry[0] = rkU[0]*rkV[0];
232  m_afEntry[1] = rkU[0]*rkV[1];
233  m_afEntry[2] = rkU[1]*rkV[0];
234  m_afEntry[3] = rkU[1]*rkV[1];
235 }
236 
237 
238 void SetRow (int iRow, const moVector2<Real>& rkV)
239 {
240  int i0 = 2*iRow ,i1 = i0+1;
241  m_afEntry[i0] = rkV[0];
242  m_afEntry[i1] = rkV[1];
243 }
244 
245 
246 moVector2<Real> GetRow (int iRow) const
247 {
248  int i0 = 2*iRow ,i1 = i0+1;
249  return moVector2<Real>(m_afEntry[i0],m_afEntry[i1]);
250 }
251 
252 
253 void SetColumn (int iCol, const moVector2<Real>& rkV)
254 {
255  m_afEntry[iCol] = rkV[0];
256  m_afEntry[iCol+2] = rkV[1];
257 }
258 
259 
260 moVector2<Real> GetColumn (int iCol) const
261 {
262  return moVector2<Real>(m_afEntry[iCol],m_afEntry[iCol+2]);
263 }
264 
265 
266 void GetColumnMajor (Real* afCMajor) const
267 {
268  afCMajor[0] = m_afEntry[0];
269  afCMajor[1] = m_afEntry[2];
270  afCMajor[2] = m_afEntry[1];
271  afCMajor[3] = m_afEntry[3];
272 }
273 
274 
275 
276 
277 bool operator== (const moMatrix2& rkM) const
278 {
279  return CompareArrays(rkM) == 0;
280 }
281 
282 
283 bool operator!= (const moMatrix2& rkM) const
284 {
285  return CompareArrays(rkM) != 0;
286 }
287 
288 
289 bool operator< (const moMatrix2& rkM) const
290 {
291  return CompareArrays(rkM) < 0;
292 }
293 
294 
295 bool operator<= (const moMatrix2& rkM) const
296 {
297  return CompareArrays(rkM) <= 0;
298 }
299 
300 
301 bool operator> (const moMatrix2& rkM) const
302 {
303  return CompareArrays(rkM) > 0;
304 }
305 
306 
307 bool operator>= (const moMatrix2& rkM) const
308 {
309  return CompareArrays(rkM) >= 0;
310 }
311 
312 
313 moMatrix2<Real> Transpose () const
314 {
315  return moMatrix2<Real>(
316  m_afEntry[0],
317  m_afEntry[2],
318  m_afEntry[1],
319  m_afEntry[3]);
320 }
321 
322 
323 moMatrix2<Real> TransposeTimes (const moMatrix2& rkM) const
324 {
325  // P = A^T*B
326  return moMatrix2<Real>(
327  m_afEntry[0]*rkM.m_afEntry[0] + m_afEntry[2]*rkM.m_afEntry[2],
328  m_afEntry[0]*rkM.m_afEntry[1] + m_afEntry[2]*rkM.m_afEntry[3],
329  m_afEntry[1]*rkM.m_afEntry[0] + m_afEntry[3]*rkM.m_afEntry[2],
330  m_afEntry[1]*rkM.m_afEntry[1] + m_afEntry[3]*rkM.m_afEntry[3]);
331 }
332 
333 moMatrix2<Real> TimesTranspose (const moMatrix2& rkM) const
334 {
335  // P = A*B^T
336  return moMatrix2<Real>(
337  m_afEntry[0]*rkM.m_afEntry[0] + m_afEntry[1]*rkM.m_afEntry[1],
338  m_afEntry[0]*rkM.m_afEntry[2] + m_afEntry[1]*rkM.m_afEntry[3],
339  m_afEntry[2]*rkM.m_afEntry[0] + m_afEntry[3]*rkM.m_afEntry[1],
340  m_afEntry[2]*rkM.m_afEntry[2] + m_afEntry[3]*rkM.m_afEntry[3]);
341 }
342 
343 moMatrix2<Real> Inverse () const
344 {
345 
346  moMatrix2 kInverse;
347 
348  Real fDet = m_afEntry[0]*m_afEntry[3] - m_afEntry[1]*m_afEntry[2];
349  if (moMath<Real>::FAbs(fDet) > moMath<Real>::ZERO_TOLERANCE)
350  {
351  Real fInvDet = ((Real)1.0)/fDet;
352  kInverse.m_afEntry[0] = m_afEntry[3]*fInvDet;
353  kInverse.m_afEntry[1] = -m_afEntry[1]*fInvDet;
354  kInverse.m_afEntry[2] = -m_afEntry[2]*fInvDet;
355  kInverse.m_afEntry[3] = m_afEntry[0]*fInvDet;
356  }
357  else
358  {
359  kInverse.m_afEntry[0] = (Real)0.0;
360  kInverse.m_afEntry[1] = (Real)0.0;
361  kInverse.m_afEntry[2] = (Real)0.0;
362  kInverse.m_afEntry[3] = (Real)0.0;
363  }
364 
365  return kInverse;
366 }
367 
368 
369 moMatrix2<Real> Adjoint () const
370 {
371  return moMatrix2<Real>(
372  m_afEntry[3],
373  -m_afEntry[1],
374  -m_afEntry[2],
375  m_afEntry[0]);
376 }
377 
378 Real Determinant () const
379 {
380  return m_afEntry[0]*m_afEntry[3] - m_afEntry[1]*m_afEntry[2];
381 }
382 
383 
384 Real QForm (const moVector2<Real>& rkU,
385  const moVector2<Real>& rkV) const
386 {
387  return rkU.Dot((*this)*rkV);
388 }
389 
390 void ToAngle (Real& rfAngle) const
391 {
392  // assert: matrix is a rotation
393  rfAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
394 }
395 
396 
397 void Orthonormalize ()
398 {
399  // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
400  // M = [m0|m1], then orthonormal output matrix is Q = [q0|q1],
401  //
402  // q0 = m0/|m0|
403  // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
404  //
405  // where |V| indicates length of vector V and A*B indicates dot
406  // product of vectors A and B.
407 
408  // compute q0
409  Real fInvLength = moMath<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
410  m_afEntry[2]*m_afEntry[2]);
411 
412  m_afEntry[0] *= fInvLength;
413  m_afEntry[2] *= fInvLength;
414 
415  // compute q1
416  Real fDot0 = m_afEntry[0]*m_afEntry[1] + m_afEntry[2]*m_afEntry[3];
417  m_afEntry[1] -= fDot0*m_afEntry[0];
418  m_afEntry[3] -= fDot0*m_afEntry[2];
419 
420  fInvLength = moMath<Real>::InvSqrt(m_afEntry[1]*m_afEntry[1] +
421  m_afEntry[3]*m_afEntry[3]);
422 
423  m_afEntry[1] *= fInvLength;
424  m_afEntry[3] *= fInvLength;
425 }
426 
427 
428 void EigenDecomposition (moMatrix2& rkRot, moMatrix2& rkDiag) const
429 {
430  Real fTrace = m_afEntry[0] + m_afEntry[3];
431  Real fDiff = m_afEntry[0] - m_afEntry[3];
432  Real fDiscr = moMath<Real>::Sqrt(fDiff*fDiff +
433  ((Real)4.0)*m_afEntry[1]*m_afEntry[1]);
434  Real fEVal0 = ((Real)0.5)*(fTrace-fDiscr);
435  Real fEVal1 = ((Real)0.5)*(fTrace+fDiscr);
436  rkDiag.MakeDiagonal(fEVal0,fEVal1);
437 
438  Real fCos, fSin;
439  if (fDiff >= (Real)0.0)
440  {
441  fCos = m_afEntry[1];
442  fSin = fEVal0 - m_afEntry[0];
443  }
444  else
445  {
446  fCos = fEVal0 - m_afEntry[3];
447  fSin = m_afEntry[1];
448  }
449  Real fTmp = moMath<Real>::InvSqrt(fCos*fCos + fSin*fSin);
450  fCos *= fTmp;
451  fSin *= fTmp;
452 
453  rkRot.m_afEntry[0] = fCos;
454  rkRot.m_afEntry[1] = -fSin;
455  rkRot.m_afEntry[2] = fSin;
456  rkRot.m_afEntry[3] = fCos;
457 }
458 
459  // assignment
460  inline moMatrix2& operator= (const moMatrix2& rkM)
461  {
462  m_afEntry[0] = rkM.m_afEntry[0];
463  m_afEntry[1] = rkM.m_afEntry[1];
464  m_afEntry[2] = rkM.m_afEntry[2];
465  m_afEntry[3] = rkM.m_afEntry[3];
466  return *this;
467  }
468 
469  // arithmetic operations
470  inline moMatrix2 operator+ (const moMatrix2& rkM) const
471  {
472  return moMatrix2<Real>(
473  m_afEntry[0] + rkM.m_afEntry[0],
474  m_afEntry[1] + rkM.m_afEntry[1],
475  m_afEntry[2] + rkM.m_afEntry[2],
476  m_afEntry[3] + rkM.m_afEntry[3]);
477  }
478  inline moMatrix2 operator- (const moMatrix2& rkM) const
479  {
480  return moMatrix2<Real>(
481  m_afEntry[0] - rkM.m_afEntry[0],
482  m_afEntry[1] - rkM.m_afEntry[1],
483  m_afEntry[2] - rkM.m_afEntry[2],
484  m_afEntry[3] - rkM.m_afEntry[3]);
485  }
486  inline moMatrix2 operator* (const moMatrix2& rkM) const
487  {
488  return moMatrix2<Real>(
489  m_afEntry[0]*rkM.m_afEntry[0] + m_afEntry[1]*rkM.m_afEntry[2],
490  m_afEntry[0]*rkM.m_afEntry[1] + m_afEntry[1]*rkM.m_afEntry[3],
491  m_afEntry[2]*rkM.m_afEntry[0] + m_afEntry[3]*rkM.m_afEntry[2],
492  m_afEntry[2]*rkM.m_afEntry[1] + m_afEntry[3]*rkM.m_afEntry[3]);
493  }
494  inline moMatrix2 operator* (Real fScalar) const
495  {
496  return moMatrix2<Real>(
497  fScalar*m_afEntry[0],
498  fScalar*m_afEntry[1],
499  fScalar*m_afEntry[2],
500  fScalar*m_afEntry[3]);
501  }
502  inline moMatrix2 operator/ (Real fScalar) const
503  {
504  if (fScalar != (Real)0.0)
505  {
506  Real fInvScalar = ((Real)1.0)/fScalar;
507  return moMatrix2<Real>(
508  fInvScalar*m_afEntry[0],
509  fInvScalar*m_afEntry[1],
510  fInvScalar*m_afEntry[2],
511  fInvScalar*m_afEntry[3]);
512  }
513 
514  return moMatrix2<Real>(
515  moMath<Real>::MAX_REAL,
516  moMath<Real>::MAX_REAL,
517  moMath<Real>::MAX_REAL,
518  moMath<Real>::MAX_REAL);
519  }
520  inline moMatrix2 operator- () const
521  {
522  return moMatrix2<Real>(
523  -m_afEntry[0],
524  -m_afEntry[1],
525  -m_afEntry[2],
526  -m_afEntry[3]);
527  }
528 
529  // arithmetic updates
530  inline moMatrix2& operator+= (const moMatrix2& rkM)
531  {
532  m_afEntry[0] += rkM.m_afEntry[0];
533  m_afEntry[1] += rkM.m_afEntry[1];
534  m_afEntry[2] += rkM.m_afEntry[2];
535  m_afEntry[3] += rkM.m_afEntry[3];
536  return *this;
537  }
538  inline moMatrix2& operator-= (const moMatrix2& rkM)
539  {
540  m_afEntry[0] -= rkM.m_afEntry[0];
541  m_afEntry[1] -= rkM.m_afEntry[1];
542  m_afEntry[2] -= rkM.m_afEntry[2];
543  m_afEntry[3] -= rkM.m_afEntry[3];
544  return *this;
545  }
546  inline moMatrix2& operator*= (Real fScalar)
547  {
548  m_afEntry[0] *= fScalar;
549  m_afEntry[1] *= fScalar;
550  m_afEntry[2] *= fScalar;
551  m_afEntry[3] *= fScalar;
552  return *this;
553  }
554  inline moMatrix2& operator/= (Real fScalar)
555  {
556  if (fScalar != (Real)0.0)
557  {
558  Real fInvScalar = ((Real)1.0)/fScalar;
559  m_afEntry[0] *= fInvScalar;
560  m_afEntry[1] *= fInvScalar;
561  m_afEntry[2] *= fInvScalar;
562  m_afEntry[3] *= fInvScalar;
563  }
564  else
565  {
566  m_afEntry[0] = moMath<Real>::MAX_REAL;
567  m_afEntry[1] = moMath<Real>::MAX_REAL;
568  m_afEntry[2] = moMath<Real>::MAX_REAL;
569  m_afEntry[3] = moMath<Real>::MAX_REAL;
570  }
571 
572  return *this;
573  }
574 
575  // matrix times vector
576  inline moVector2<Real> operator* (const moVector2<Real>& rkV) const // M * v
577  {
578  return moVector2<Real>(
579  m_afEntry[0]*rkV[0] + m_afEntry[1]*rkV[1],
580  m_afEntry[2]*rkV[0] + m_afEntry[3]*rkV[1]);
581  }
582 
583 
584  // The matrix must be a rotation for these functions to be valid. The
585  // last function uses Gram-Schmidt orthonormalization applied to the
586  // columns of the rotation matrix. The angle must be in radians, not
587  // degrees.
588 
589  // The matrix must be symmetric. Factor M = R * D * R^T where
590  // R = [u0|u1] is a rotation matrix with columns u0 and u1 and
591  // D = diag(d0,d1) is a diagonal matrix whose diagonal entries are d0 and
592  // d1. The eigenvector u[i] corresponds to eigenvector d[i]. The
593  // eigenvalues are ordered as d0 <= d1.
594 
595  static const moMatrix2 ZERO;
596  static const moMatrix2 IDENTITY;
597 
598 private:
599  // support for comparisons
600 int CompareArrays (const moMatrix2& rkM) const
601 {
602  return memcmp(m_afEntry,rkM.m_afEntry,4*sizeof(Real));
603 }
604 
605 
606  // matrix stored in row-major order
607  Real m_afEntry[4];
608 };
609 
610 // c * M
611 template <class Real>
612 inline moMatrix2<Real> operator* (Real fScalar, const moMatrix2<Real>& rkM)
613 {
614  return rkM*fScalar;
615 }
616 
617 // v^T * M
618 template <class Real>
619 inline moVector2<Real> operator* (const moVector2<Real>& rkV,
620  const moMatrix2<Real>& rkM)
621 {
622  return moVector2<Real>(
623  rkV[0]*rkM[0][0] + rkV[1]*rkM[1][0],
624  rkV[0]*rkM[0][1] + rkV[1]*rkM[1][1]);
625 }
626 
627 #ifndef MO_MACOSX
628 #ifndef MO_RASPBIAN
629 template class LIBMOLDEO_API moMatrix2<MOfloat>;
630 #endif
631 #endif
632 typedef moMatrix2<MOfloat> moMatrix2f;
633 
634 #ifndef MO_MACOSX
635 #ifndef MO_RASPBIAN
636 template class LIBMOLDEO_API moMatrix2<MOdouble>;
637 #endif
638 #endif
639 typedef moMatrix2<MOdouble> moMatrix2d;
640 
641 moDeclareExportedDynamicArray( moMatrix2f, moMatrix2fArray );
642 moDeclareExportedDynamicArray( moMatrix2d, moMatrix2dArray )
643 
644 
645 // moMatrix3 class ------------------------------------------------------------
646 
647 
648 
649 template <class Real>
650 class LIBMOLDEO_API moMatrix3 : public moAbstract
651 {
652  public:
653 
654 
655 
656  moMatrix3 (bool bZero = true)
657  {
658  if (bZero)
659  {
660  MakeZero();
661  }
662  else
663  {
664  MakeIdentity();
665  }
666  }
667 
668 
669  moMatrix3 (const moMatrix3& rkM) : moAbstract()
670  {
671  m_afEntry[0] = rkM.m_afEntry[0];
672  m_afEntry[1] = rkM.m_afEntry[1];
673  m_afEntry[2] = rkM.m_afEntry[2];
674  m_afEntry[3] = rkM.m_afEntry[3];
675  m_afEntry[4] = rkM.m_afEntry[4];
676  m_afEntry[5] = rkM.m_afEntry[5];
677  m_afEntry[6] = rkM.m_afEntry[6];
678  m_afEntry[7] = rkM.m_afEntry[7];
679  m_afEntry[8] = rkM.m_afEntry[8];
680  }
681 
682 
683  moMatrix3 (Real fM00, Real fM01, Real fM02, Real fM10, Real fM11,
684  Real fM12, Real fM20, Real fM21, Real fM22)
685  {
686  m_afEntry[0] = fM00;
687  m_afEntry[1] = fM01;
688  m_afEntry[2] = fM02;
689  m_afEntry[3] = fM10;
690  m_afEntry[4] = fM11;
691  m_afEntry[5] = fM12;
692  m_afEntry[6] = fM20;
693  m_afEntry[7] = fM21;
694  m_afEntry[8] = fM22;
695  }
696 
697 
698  moMatrix3 (const Real afEntry[9], bool bRowMajor)
699  {
700  if (bRowMajor)
701  {
702  m_afEntry[0] = afEntry[0];
703  m_afEntry[1] = afEntry[1];
704  m_afEntry[2] = afEntry[2];
705  m_afEntry[3] = afEntry[3];
706  m_afEntry[4] = afEntry[4];
707  m_afEntry[5] = afEntry[5];
708  m_afEntry[6] = afEntry[6];
709  m_afEntry[7] = afEntry[7];
710  m_afEntry[8] = afEntry[8];
711  }
712  else
713  {
714  m_afEntry[0] = afEntry[0];
715  m_afEntry[1] = afEntry[3];
716  m_afEntry[2] = afEntry[6];
717  m_afEntry[3] = afEntry[1];
718  m_afEntry[4] = afEntry[4];
719  m_afEntry[5] = afEntry[7];
720  m_afEntry[6] = afEntry[2];
721  m_afEntry[7] = afEntry[5];
722  m_afEntry[8] = afEntry[8];
723  }
724  }
725 
726 
727  moMatrix3 (const moVector3<Real>& rkU, const moVector3<Real>& rkV,
728  const moVector3<Real>& rkW, bool bColumns)
729  {
730  if (bColumns)
731  {
732  m_afEntry[0] = rkU[0];
733  m_afEntry[1] = rkV[0];
734  m_afEntry[2] = rkW[0];
735  m_afEntry[3] = rkU[1];
736  m_afEntry[4] = rkV[1];
737  m_afEntry[5] = rkW[1];
738  m_afEntry[6] = rkU[2];
739  m_afEntry[7] = rkV[2];
740  m_afEntry[8] = rkW[2];
741  }
742  else
743  {
744  m_afEntry[0] = rkU[0];
745  m_afEntry[1] = rkU[1];
746  m_afEntry[2] = rkU[2];
747  m_afEntry[3] = rkV[0];
748  m_afEntry[4] = rkV[1];
749  m_afEntry[5] = rkV[2];
750  m_afEntry[6] = rkW[0];
751  m_afEntry[7] = rkW[1];
752  m_afEntry[8] = rkW[2];
753  }
754  }
755 
756 
757  moMatrix3 (const moVector3<Real>* akV, bool bColumns)
758  {
759  if (bColumns)
760  {
761  m_afEntry[0] = akV[0][0];
762  m_afEntry[1] = akV[1][0];
763  m_afEntry[2] = akV[2][0];
764  m_afEntry[3] = akV[0][1];
765  m_afEntry[4] = akV[1][1];
766  m_afEntry[5] = akV[2][1];
767  m_afEntry[6] = akV[0][2];
768  m_afEntry[7] = akV[1][2];
769  m_afEntry[8] = akV[2][2];
770  }
771  else
772  {
773  m_afEntry[0] = akV[0][0];
774  m_afEntry[1] = akV[0][1];
775  m_afEntry[2] = akV[0][2];
776  m_afEntry[3] = akV[1][0];
777  m_afEntry[4] = akV[1][1];
778  m_afEntry[5] = akV[1][2];
779  m_afEntry[6] = akV[2][0];
780  m_afEntry[7] = akV[2][1];
781  m_afEntry[8] = akV[2][2];
782  }
783  }
784 
785 
786  moMatrix3 (Real fM00, Real fM11, Real fM22)
787  {
788  MakeDiagonal(fM00,fM11,fM22);
789  }
790 
791 
792  moMatrix3 (const moVector3<Real>& rkAxis, Real fAngle)
793  {
794  FromAxisAngle(rkAxis,fAngle);
795  }
796 
797 
798  moMatrix3 (const moVector3<Real>& rkU, const moVector3<Real>& rkV)
799  {
800  MakeTensorProduct(rkU,rkV);
801  }
802 
803 
804  moMatrix3<Real>& MakeZero ()
805  {
806  m_afEntry[0] = (Real)0.0;
807  m_afEntry[1] = (Real)0.0;
808  m_afEntry[2] = (Real)0.0;
809  m_afEntry[3] = (Real)0.0;
810  m_afEntry[4] = (Real)0.0;
811  m_afEntry[5] = (Real)0.0;
812  m_afEntry[6] = (Real)0.0;
813  m_afEntry[7] = (Real)0.0;
814  m_afEntry[8] = (Real)0.0;
815  return *this;
816  }
817 
818 
819  moMatrix3<Real>& MakeIdentity ()
820  {
821  m_afEntry[0] = (Real)1.0;
822  m_afEntry[1] = (Real)0.0;
823  m_afEntry[2] = (Real)0.0;
824  m_afEntry[3] = (Real)0.0;
825  m_afEntry[4] = (Real)1.0;
826  m_afEntry[5] = (Real)0.0;
827  m_afEntry[6] = (Real)0.0;
828  m_afEntry[7] = (Real)0.0;
829  m_afEntry[8] = (Real)1.0;
830  return *this;
831  }
832 
833 
834  moMatrix3<Real>& MakeDiagonal (Real fM00, Real fM11, Real fM22)
835  {
836  m_afEntry[0] = fM00;
837  m_afEntry[1] = (Real)0.0;
838  m_afEntry[2] = (Real)0.0;
839  m_afEntry[3] = (Real)0.0;
840  m_afEntry[4] = fM11;
841  m_afEntry[5] = (Real)0.0;
842  m_afEntry[6] = (Real)0.0;
843  m_afEntry[7] = (Real)0.0;
844  m_afEntry[8] = fM22;
845  return *this;
846  }
847 
848 
849  moMatrix3<Real>& FromAxisAngle (const moVector3<Real>& rkAxis,
850  Real fAngle)
851  {
852  Real fCos = moMath<Real>::Cos(fAngle);
853  Real fSin = moMath<Real>::Sin(fAngle);
854  Real fOneMinusCos = ((Real)1.0)-fCos;
855  Real fX2 = rkAxis[0]*rkAxis[0];
856  Real fY2 = rkAxis[1]*rkAxis[1];
857  Real fZ2 = rkAxis[2]*rkAxis[2];
858  Real fXYM = rkAxis[0]*rkAxis[1]*fOneMinusCos;
859  Real fXZM = rkAxis[0]*rkAxis[2]*fOneMinusCos;
860  Real fYZM = rkAxis[1]*rkAxis[2]*fOneMinusCos;
861  Real fXSin = rkAxis[0]*fSin;
862  Real fYSin = rkAxis[1]*fSin;
863  Real fZSin = rkAxis[2]*fSin;
864 
865  m_afEntry[0] = fX2*fOneMinusCos+fCos;
866  m_afEntry[1] = fXYM-fZSin;
867  m_afEntry[2] = fXZM+fYSin;
868  m_afEntry[3] = fXYM+fZSin;
869  m_afEntry[4] = fY2*fOneMinusCos+fCos;
870  m_afEntry[5] = fYZM-fXSin;
871  m_afEntry[6] = fXZM-fYSin;
872  m_afEntry[7] = fYZM+fXSin;
873  m_afEntry[8] = fZ2*fOneMinusCos+fCos;
874 
875  return *this;
876  }
877 
878 
879  moMatrix3<Real>& MakeTensorProduct (const moVector3<Real>& rkU,
880  const moVector3<Real>& rkV)
881  {
882  m_afEntry[0] = rkU[0]*rkV[0];
883  m_afEntry[1] = rkU[0]*rkV[1];
884  m_afEntry[2] = rkU[0]*rkV[2];
885  m_afEntry[3] = rkU[1]*rkV[0];
886  m_afEntry[4] = rkU[1]*rkV[1];
887  m_afEntry[5] = rkU[1]*rkV[2];
888  m_afEntry[6] = rkU[2]*rkV[0];
889  m_afEntry[7] = rkU[2]*rkV[1];
890  m_afEntry[8] = rkU[2]*rkV[2];
891  return *this;
892  }
893 
894 
895  void SetRow (int iRow, const moVector3<Real>& rkV)
896  {
897  int i0 = 3*iRow, i1 = i0+1, i2 = i1+1;
898  m_afEntry[i0] = rkV[0];
899  m_afEntry[i1] = rkV[1];
900  m_afEntry[i2] = rkV[2];
901  }
902 
903 
904  moVector3<Real> GetRow (int iRow) const
905  {
906  int i0 = 3*iRow, i1 = i0+1, i2 = i1+1;
907  return moVector3<Real>(m_afEntry[i0],m_afEntry[i1],m_afEntry[i2]);
908  }
909 
910 
911  void SetColumn (int iCol, const moVector3<Real>& rkV)
912  {
913  m_afEntry[iCol] = rkV[0];
914  m_afEntry[iCol+3] = rkV[1];
915  m_afEntry[iCol+6] = rkV[2];
916  }
917 
918 
919  moVector3<Real> GetColumn (int iCol) const
920  {
921  return moVector3<Real>(m_afEntry[iCol],m_afEntry[iCol+3],m_afEntry[iCol+6]);
922  }
923 
924 
925  void GetColumnMajor (Real* afCMajor) const
926  {
927  afCMajor[0] = m_afEntry[0];
928  afCMajor[1] = m_afEntry[3];
929  afCMajor[2] = m_afEntry[6];
930  afCMajor[3] = m_afEntry[1];
931  afCMajor[4] = m_afEntry[4];
932  afCMajor[5] = m_afEntry[7];
933  afCMajor[6] = m_afEntry[2];
934  afCMajor[7] = m_afEntry[5];
935  afCMajor[8] = m_afEntry[8];
936  }
937 
938 
939 
940 
941 
942  bool operator== (const moMatrix3& rkM) const
943  {
944  return CompareArrays(rkM) == 0;
945  }
946 
947 
948  bool operator!= (const moMatrix3& rkM) const
949  {
950  return CompareArrays(rkM) != 0;
951  }
952 
953 
954  bool operator< (const moMatrix3& rkM) const
955  {
956  return CompareArrays(rkM) < 0;
957  }
958 
959 
960  bool operator<= (const moMatrix3& rkM) const
961  {
962  return CompareArrays(rkM) <= 0;
963  }
964 
965 
966  bool operator> (const moMatrix3& rkM) const
967  {
968  return CompareArrays(rkM) > 0;
969  }
970 
971 
972  bool operator>= (const moMatrix3& rkM) const
973  {
974  return CompareArrays(rkM) >= 0;
975  }
976 
977 
978  moMatrix3<Real> Transpose () const
979  {
980  return moMatrix3<Real>(
981  m_afEntry[0],
982  m_afEntry[3],
983  m_afEntry[6],
984  m_afEntry[1],
985  m_afEntry[4],
986  m_afEntry[7],
987  m_afEntry[2],
988  m_afEntry[5],
989  m_afEntry[8]);
990  }
991 
992 
993  moMatrix3<Real> TransposeTimes (const moMatrix3& rkM) const
994  {
995  // P = A^T*B
996  return moMatrix3<Real>(
997  m_afEntry[0]*rkM.m_afEntry[0] +
998  m_afEntry[3]*rkM.m_afEntry[3] +
999  m_afEntry[6]*rkM.m_afEntry[6],
1000 
1001  m_afEntry[0]*rkM.m_afEntry[1] +
1002  m_afEntry[3]*rkM.m_afEntry[4] +
1003  m_afEntry[6]*rkM.m_afEntry[7],
1004 
1005  m_afEntry[0]*rkM.m_afEntry[2] +
1006  m_afEntry[3]*rkM.m_afEntry[5] +
1007  m_afEntry[6]*rkM.m_afEntry[8],
1008 
1009  m_afEntry[1]*rkM.m_afEntry[0] +
1010  m_afEntry[4]*rkM.m_afEntry[3] +
1011  m_afEntry[7]*rkM.m_afEntry[6],
1012 
1013  m_afEntry[1]*rkM.m_afEntry[1] +
1014  m_afEntry[4]*rkM.m_afEntry[4] +
1015  m_afEntry[7]*rkM.m_afEntry[7],
1016 
1017  m_afEntry[1]*rkM.m_afEntry[2] +
1018  m_afEntry[4]*rkM.m_afEntry[5] +
1019  m_afEntry[7]*rkM.m_afEntry[8],
1020 
1021  m_afEntry[2]*rkM.m_afEntry[0] +
1022  m_afEntry[5]*rkM.m_afEntry[3] +
1023  m_afEntry[8]*rkM.m_afEntry[6],
1024 
1025  m_afEntry[2]*rkM.m_afEntry[1] +
1026  m_afEntry[5]*rkM.m_afEntry[4] +
1027  m_afEntry[8]*rkM.m_afEntry[7],
1028 
1029  m_afEntry[2]*rkM.m_afEntry[2] +
1030  m_afEntry[5]*rkM.m_afEntry[5] +
1031  m_afEntry[8]*rkM.m_afEntry[8]);
1032  }
1033 
1034 
1035  moMatrix3<Real> TimesTranspose (const moMatrix3& rkM) const
1036  {
1037  // P = A*B^T
1038  return moMatrix3<Real>(
1039  m_afEntry[0]*rkM.m_afEntry[0] +
1040  m_afEntry[1]*rkM.m_afEntry[1] +
1041  m_afEntry[2]*rkM.m_afEntry[2],
1042 
1043  m_afEntry[0]*rkM.m_afEntry[3] +
1044  m_afEntry[1]*rkM.m_afEntry[4] +
1045  m_afEntry[2]*rkM.m_afEntry[5],
1046 
1047  m_afEntry[0]*rkM.m_afEntry[6] +
1048  m_afEntry[1]*rkM.m_afEntry[7] +
1049  m_afEntry[2]*rkM.m_afEntry[8],
1050 
1051  m_afEntry[3]*rkM.m_afEntry[0] +
1052  m_afEntry[4]*rkM.m_afEntry[1] +
1053  m_afEntry[5]*rkM.m_afEntry[2],
1054 
1055  m_afEntry[3]*rkM.m_afEntry[3] +
1056  m_afEntry[4]*rkM.m_afEntry[4] +
1057  m_afEntry[5]*rkM.m_afEntry[5],
1058 
1059  m_afEntry[3]*rkM.m_afEntry[6] +
1060  m_afEntry[4]*rkM.m_afEntry[7] +
1061  m_afEntry[5]*rkM.m_afEntry[8],
1062 
1063  m_afEntry[6]*rkM.m_afEntry[0] +
1064  m_afEntry[7]*rkM.m_afEntry[1] +
1065  m_afEntry[8]*rkM.m_afEntry[2],
1066 
1067  m_afEntry[6]*rkM.m_afEntry[3] +
1068  m_afEntry[7]*rkM.m_afEntry[4] +
1069  m_afEntry[8]*rkM.m_afEntry[5],
1070 
1071  m_afEntry[6]*rkM.m_afEntry[6] +
1072  m_afEntry[7]*rkM.m_afEntry[7] +
1073  m_afEntry[8]*rkM.m_afEntry[8]);
1074  }
1075 
1076 
1077  moMatrix3<Real> Inverse () const
1078  {
1079  // Invert a 3x3 using cofactors. This is faster than using a generic
1080  // Gaussian elimination because of the loop overhead of such a method.
1081 
1082 
1083  moMatrix3 kInverse;
1084 
1085  kInverse.m_afEntry[0] =
1086  m_afEntry[4]*m_afEntry[8] - m_afEntry[5]*m_afEntry[7];
1087  kInverse.m_afEntry[1] =
1088  m_afEntry[2]*m_afEntry[7] - m_afEntry[1]*m_afEntry[8];
1089  kInverse.m_afEntry[2] =
1090  m_afEntry[1]*m_afEntry[5] - m_afEntry[2]*m_afEntry[4];
1091  kInverse.m_afEntry[3] =
1092  m_afEntry[5]*m_afEntry[6] - m_afEntry[3]*m_afEntry[8];
1093  kInverse.m_afEntry[4] =
1094  m_afEntry[0]*m_afEntry[8] - m_afEntry[2]*m_afEntry[6];
1095  kInverse.m_afEntry[5] =
1096  m_afEntry[2]*m_afEntry[3] - m_afEntry[0]*m_afEntry[5];
1097  kInverse.m_afEntry[6] =
1098  m_afEntry[3]*m_afEntry[7] - m_afEntry[4]*m_afEntry[6];
1099  kInverse.m_afEntry[7] =
1100  m_afEntry[1]*m_afEntry[6] - m_afEntry[0]*m_afEntry[7];
1101  kInverse.m_afEntry[8] =
1102  m_afEntry[0]*m_afEntry[4] - m_afEntry[1]*m_afEntry[3];
1103 
1104  Real fDet =
1105  m_afEntry[0]*kInverse.m_afEntry[0] +
1106  m_afEntry[1]*kInverse.m_afEntry[3] +
1107  m_afEntry[2]*kInverse.m_afEntry[6];
1108 
1109  if (moMath<Real>::FAbs(fDet) <= moMath<Real>::ZERO_TOLERANCE)
1110  {
1111  return ZERO;
1112  }
1113 
1114  Real fInvDet = ((Real)1.0)/fDet;
1115  kInverse.m_afEntry[0] *= fInvDet;
1116  kInverse.m_afEntry[1] *= fInvDet;
1117  kInverse.m_afEntry[2] *= fInvDet;
1118  kInverse.m_afEntry[3] *= fInvDet;
1119  kInverse.m_afEntry[4] *= fInvDet;
1120  kInverse.m_afEntry[5] *= fInvDet;
1121  kInverse.m_afEntry[6] *= fInvDet;
1122  kInverse.m_afEntry[7] *= fInvDet;
1123  kInverse.m_afEntry[8] *= fInvDet;
1124  return kInverse;
1125  }
1126 
1127 
1128  moMatrix3<Real> Adjoint () const
1129  {
1130  return moMatrix3<Real>(
1131  m_afEntry[4]*m_afEntry[8] - m_afEntry[5]*m_afEntry[7],
1132  m_afEntry[2]*m_afEntry[7] - m_afEntry[1]*m_afEntry[8],
1133  m_afEntry[1]*m_afEntry[5] - m_afEntry[2]*m_afEntry[4],
1134  m_afEntry[5]*m_afEntry[6] - m_afEntry[3]*m_afEntry[8],
1135  m_afEntry[0]*m_afEntry[8] - m_afEntry[2]*m_afEntry[6],
1136  m_afEntry[2]*m_afEntry[3] - m_afEntry[0]*m_afEntry[5],
1137  m_afEntry[3]*m_afEntry[7] - m_afEntry[4]*m_afEntry[6],
1138  m_afEntry[1]*m_afEntry[6] - m_afEntry[0]*m_afEntry[7],
1139  m_afEntry[0]*m_afEntry[4] - m_afEntry[1]*m_afEntry[3]);
1140  }
1141 
1142 
1143  Real Determinant () const
1144  {
1145  Real fCo00 = m_afEntry[4]*m_afEntry[8] - m_afEntry[5]*m_afEntry[7];
1146  Real fCo10 = m_afEntry[5]*m_afEntry[6] - m_afEntry[3]*m_afEntry[8];
1147  Real fCo20 = m_afEntry[3]*m_afEntry[7] - m_afEntry[4]*m_afEntry[6];
1148  Real fDet = m_afEntry[0]*fCo00 + m_afEntry[1]*fCo10 + m_afEntry[2]*fCo20;
1149  return fDet;
1150  }
1151 
1152 
1153  Real QForm (const moVector3<Real>& rkU, const moVector3<Real>& rkV)
1154  const
1155  {
1156  return rkU.Dot((*this)*rkV);
1157  }
1158 
1159 
1160  moMatrix3<Real> TimesDiagonal (const moVector3<Real>& rkDiag) const
1161  {
1162  return moMatrix3<Real>(
1163  m_afEntry[0]*rkDiag[0],m_afEntry[1]*rkDiag[1],m_afEntry[2]*rkDiag[2],
1164  m_afEntry[3]*rkDiag[0],m_afEntry[4]*rkDiag[1],m_afEntry[5]*rkDiag[2],
1165  m_afEntry[6]*rkDiag[0],m_afEntry[7]*rkDiag[1],m_afEntry[8]*rkDiag[2]);
1166  }
1167 
1168 
1169  moMatrix3<Real> DiagonalTimes (const moVector3<Real>& rkDiag) const
1170  {
1171  return moMatrix3<Real>(
1172  rkDiag[0]*m_afEntry[0],rkDiag[0]*m_afEntry[1],rkDiag[0]*m_afEntry[2],
1173  rkDiag[1]*m_afEntry[3],rkDiag[1]*m_afEntry[4],rkDiag[1]*m_afEntry[5],
1174  rkDiag[2]*m_afEntry[6],rkDiag[2]*m_afEntry[7],rkDiag[2]*m_afEntry[8]);
1175  }
1176 
1177 
1178  void ToAxisAngle (moVector3<Real>& rkAxis, Real& rfAngle) const
1179  {
1180  // Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
1181  // The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 where
1182  // I is the identity and
1183  //
1184  // +- -+
1185  // P = | 0 -z +y |
1186  // | +z 0 -x |
1187  // | -y +x 0 |
1188  // +- -+
1189  //
1190  // If A > 0, R represents a counterclockwise rotation about the axis in
1191  // the sense of looking from the tip of the axis vector towards the
1192  // origin. Some algebra will show that
1193  //
1194  // cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P
1195  //
1196  // In the event that A = pi, R-R^t = 0 which prevents us from extracting
1197  // the axis through P. Instead note that R = I+2*P^2 when A = pi, so
1198  // P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and
1199  // z^2-1. We can solve these for axis (x,y,z). Because the angle is pi,
1200  // it does not matter which sign you choose on the square roots.
1201 
1202  Real fTrace = m_afEntry[0] + m_afEntry[4] + m_afEntry[8];
1203  Real fCos = ((Real)0.5)*(fTrace-(Real)1.0);
1204  rfAngle = moMath<Real>::ACos(fCos); // in [0,PI]
1205 
1206  if (rfAngle > (Real)0.0)
1207  {
1208  if (rfAngle < moMath<Real>::PI)
1209  {
1210  rkAxis[0] = m_afEntry[7]-m_afEntry[5];
1211  rkAxis[1] = m_afEntry[2]-m_afEntry[6];
1212  rkAxis[2] = m_afEntry[3]-m_afEntry[1];
1213  rkAxis.Normalize();
1214  }
1215  else
1216  {
1217  // angle is PI
1218  Real fHalfInverse;
1219  if (m_afEntry[0] >= m_afEntry[4])
1220  {
1221  // r00 >= r11
1222  if (m_afEntry[0] >= m_afEntry[8])
1223  {
1224  // r00 is maximum diagonal term
1225  rkAxis[0] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[0] -
1226  m_afEntry[4] - m_afEntry[8] + (Real)1.0);
1227  fHalfInverse = ((Real)0.5)/rkAxis[0];
1228  rkAxis[1] = fHalfInverse*m_afEntry[1];
1229  rkAxis[2] = fHalfInverse*m_afEntry[2];
1230  }
1231  else
1232  {
1233  // r22 is maximum diagonal term
1234  rkAxis[2] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[8] -
1235  m_afEntry[0] - m_afEntry[4] + (Real)1.0);
1236  fHalfInverse = ((Real)0.5)/rkAxis[2];
1237  rkAxis[0] = fHalfInverse*m_afEntry[2];
1238  rkAxis[1] = fHalfInverse*m_afEntry[5];
1239  }
1240  }
1241  else
1242  {
1243  // r11 > r00
1244  if (m_afEntry[4] >= m_afEntry[8])
1245  {
1246  // r11 is maximum diagonal term
1247  rkAxis[1] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[4] -
1248  m_afEntry[0] - m_afEntry[8] + (Real)1.0);
1249  fHalfInverse = ((Real)0.5)/rkAxis[1];
1250  rkAxis[0] = fHalfInverse*m_afEntry[1];
1251  rkAxis[2] = fHalfInverse*m_afEntry[5];
1252  }
1253  else
1254  {
1255  // r22 is maximum diagonal term
1256  rkAxis[2] = ((Real)0.5)*moMath<Real>::Sqrt(m_afEntry[8] -
1257  m_afEntry[0] - m_afEntry[4] + (Real)1.0);
1258  fHalfInverse = ((Real)0.5)/rkAxis[2];
1259  rkAxis[0] = fHalfInverse*m_afEntry[2];
1260  rkAxis[1] = fHalfInverse*m_afEntry[5];
1261  }
1262  }
1263  }
1264  }
1265  else
1266  {
1267  // The angle is 0 and the matrix is the identity. Any axis will
1268  // work, so just use the x-axis.
1269  rkAxis[0] = (Real)1.0;
1270  rkAxis[1] = (Real)0.0;
1271  rkAxis[2] = (Real)0.0;
1272  }
1273  }
1274 
1275 
1276  void Orthonormalize ()
1277  {
1278  // Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
1279  // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
1280  //
1281  // q0 = m0/|m0|
1282  // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
1283  // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
1284  //
1285  // where |V| indicates length of vector V and A*B indicates dot
1286  // product of vectors A and B.
1287 
1288  // compute q0
1289  Real fInvLength = moMath<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
1290  m_afEntry[3]*m_afEntry[3] + m_afEntry[6]*m_afEntry[6]);
1291 
1292  m_afEntry[0] *= fInvLength;
1293  m_afEntry[3] *= fInvLength;
1294  m_afEntry[6] *= fInvLength;
1295 
1296  // compute q1
1297  Real fDot0 = m_afEntry[0]*m_afEntry[1] + m_afEntry[3]*m_afEntry[4] +
1298  m_afEntry[6]*m_afEntry[7];
1299 
1300  m_afEntry[1] -= fDot0*m_afEntry[0];
1301  m_afEntry[4] -= fDot0*m_afEntry[3];
1302  m_afEntry[7] -= fDot0*m_afEntry[6];
1303 
1304  fInvLength = moMath<Real>::InvSqrt(m_afEntry[1]*m_afEntry[1] +
1305  m_afEntry[4]*m_afEntry[4] + m_afEntry[7]*m_afEntry[7]);
1306 
1307  m_afEntry[1] *= fInvLength;
1308  m_afEntry[4] *= fInvLength;
1309  m_afEntry[7] *= fInvLength;
1310 
1311  // compute q2
1312  Real fDot1 = m_afEntry[1]*m_afEntry[2] + m_afEntry[4]*m_afEntry[5] +
1313  m_afEntry[7]*m_afEntry[8];
1314 
1315  fDot0 = m_afEntry[0]*m_afEntry[2] + m_afEntry[3]*m_afEntry[5] +
1316  m_afEntry[6]*m_afEntry[8];
1317 
1318  m_afEntry[2] -= fDot0*m_afEntry[0] + fDot1*m_afEntry[1];
1319  m_afEntry[5] -= fDot0*m_afEntry[3] + fDot1*m_afEntry[4];
1320  m_afEntry[8] -= fDot0*m_afEntry[6] + fDot1*m_afEntry[7];
1321 
1322  fInvLength = moMath<Real>::InvSqrt(m_afEntry[2]*m_afEntry[2] +
1323  m_afEntry[5]*m_afEntry[5] + m_afEntry[8]*m_afEntry[8]);
1324 
1325  m_afEntry[2] *= fInvLength;
1326  m_afEntry[5] *= fInvLength;
1327  m_afEntry[8] *= fInvLength;
1328  }
1329 
1330 
1331  void EigenDecomposition (moMatrix3& rkRot, moMatrix3& rkDiag) const
1332  {
1333  // Factor M = R*D*R^T. The columns of R are the eigenvectors. The
1334  // diagonal entries of D are the corresponding eigenvalues.
1335  Real afDiag[3], afSubd[2];
1336  rkRot = *this;
1337  bool bReflection = rkRot.Tridiagonalize(afDiag,afSubd);
1338  bool bConverged = rkRot.QLAlgorithm(afDiag,afSubd);
1339  if (!bConverged) return;
1340 
1341  // (insertion) sort eigenvalues in increasing order, d0 <= d1 <= d2
1342  int i;
1343  Real fSave;
1344 
1345  if (afDiag[1] < afDiag[0])
1346  {
1347  // swap d0 and d1
1348  fSave = afDiag[0];
1349  afDiag[0] = afDiag[1];
1350  afDiag[1] = fSave;
1351 
1352  // swap V0 and V1
1353  for (i = 0; i < 3; i++)
1354  {
1355  fSave = rkRot[i][0];
1356  rkRot[i][0] = rkRot[i][1];
1357  rkRot[i][1] = fSave;
1358  }
1359  bReflection = !bReflection;
1360  }
1361 
1362  if (afDiag[2] < afDiag[1])
1363  {
1364  // swap d1 and d2
1365  fSave = afDiag[1];
1366  afDiag[1] = afDiag[2];
1367  afDiag[2] = fSave;
1368 
1369  // swap V1 and V2
1370  for (i = 0; i < 3; i++)
1371  {
1372  fSave = rkRot[i][1];
1373  rkRot[i][1] = rkRot[i][2];
1374  rkRot[i][2] = fSave;
1375  }
1376  bReflection = !bReflection;
1377  }
1378 
1379  if (afDiag[1] < afDiag[0])
1380  {
1381  // swap d0 and d1
1382  fSave = afDiag[0];
1383  afDiag[0] = afDiag[1];
1384  afDiag[1] = fSave;
1385 
1386  // swap V0 and V1
1387  for (i = 0; i < 3; i++)
1388  {
1389  fSave = rkRot[i][0];
1390  rkRot[i][0] = rkRot[i][1];
1391  rkRot[i][1] = fSave;
1392  }
1393  bReflection = !bReflection;
1394  }
1395 
1396  rkDiag.MakeDiagonal(afDiag[0],afDiag[1],afDiag[2]);
1397 
1398  if (bReflection)
1399  {
1400  // The orthogonal transformation that diagonalizes M is a reflection.
1401  // Make the eigenvectors a right--handed system by changing sign on
1402  // the last column.
1403  rkRot[0][2] = -rkRot[0][2];
1404  rkRot[1][2] = -rkRot[1][2];
1405  rkRot[2][2] = -rkRot[2][2];
1406  }
1407  }
1408 
1409 
1410  moMatrix3<Real>& FromEulerAnglesXYZ (Real fXAngle, Real fYAngle,
1411  Real fZAngle)
1412  {
1413  Real fCos, fSin;
1414 
1415  fCos = moMath<Real>::Cos(fXAngle);
1416  fSin = moMath<Real>::Sin(fXAngle);
1417  moMatrix3 kXMat(
1418  (Real)1.0,(Real)0.0,(Real)0.0,
1419  (Real)0.0,fCos,-fSin,
1420  (Real)0.0,fSin,fCos);
1421 
1422  fCos = moMath<Real>::Cos(fYAngle);
1423  fSin = moMath<Real>::Sin(fYAngle);
1424  moMatrix3 kYMat(
1425  fCos,(Real)0.0,fSin,
1426  (Real)0.0,(Real)1.0,(Real)0.0,
1427  -fSin,(Real)0.0,fCos);
1428 
1429  fCos = moMath<Real>::Cos(fZAngle);
1430  fSin = moMath<Real>::Sin(fZAngle);
1431  moMatrix3 kZMat(
1432  fCos,-fSin,(Real)0.0,
1433  fSin,fCos,(Real)0.0,
1434  (Real)0.0,(Real)0.0,(Real)1.0);
1435 
1436  *this = kXMat*(kYMat*kZMat);
1437  return *this;
1438  }
1439 
1440 
1441  moMatrix3<Real>& FromEulerAnglesXZY (Real fXAngle, Real fZAngle,
1442  Real fYAngle)
1443  {
1444  Real fCos, fSin;
1445 
1446  fCos = moMath<Real>::Cos(fXAngle);
1447  fSin = moMath<Real>::Sin(fXAngle);
1448  moMatrix3 kXMat(
1449  (Real)1.0,(Real)0.0,(Real)0.0,
1450  (Real)0.0,fCos,-fSin,
1451  (Real)0.0,fSin,fCos);
1452 
1453  fCos = moMath<Real>::Cos(fZAngle);
1454  fSin = moMath<Real>::Sin(fZAngle);
1455  moMatrix3 kZMat(
1456  fCos,-fSin,(Real)0.0,
1457  fSin,fCos,(Real)0.0,
1458  (Real)0.0,(Real)0.0,(Real)1.0);
1459 
1460  fCos = moMath<Real>::Cos(fYAngle);
1461  fSin = moMath<Real>::Sin(fYAngle);
1462  moMatrix3 kYMat(
1463  fCos,(Real)0.0,fSin,
1464  (Real)0.0,(Real)1.0,(Real)0.0,
1465  -fSin,(Real)0.0,fCos);
1466 
1467  *this = kXMat*(kZMat*kYMat);
1468  return *this;
1469  }
1470 
1471 
1472  moMatrix3<Real>& FromEulerAnglesYXZ (Real fYAngle, Real fXAngle,
1473  Real fZAngle)
1474  {
1475  Real fCos, fSin;
1476 
1477  fCos = moMath<Real>::Cos(fYAngle);
1478  fSin = moMath<Real>::Sin(fYAngle);
1479  moMatrix3 kYMat(
1480  fCos,(Real)0.0,fSin,
1481  (Real)0.0,(Real)1.0,(Real)0.0,
1482  -fSin,(Real)0.0,fCos);
1483 
1484  fCos = moMath<Real>::Cos(fXAngle);
1485  fSin = moMath<Real>::Sin(fXAngle);
1486  moMatrix3 kXMat(
1487  (Real)1.0,(Real)0.0,(Real)0.0,
1488  (Real)0.0,fCos,-fSin,
1489  (Real)0.0,fSin,fCos);
1490 
1491  fCos = moMath<Real>::Cos(fZAngle);
1492  fSin = moMath<Real>::Sin(fZAngle);
1493  moMatrix3 kZMat(
1494  fCos,-fSin,(Real)0.0,
1495  fSin,fCos,(Real)0.0,
1496  (Real)0.0,(Real)0.0,(Real)1.0);
1497 
1498  *this = kYMat*(kXMat*kZMat);
1499  return *this;
1500  }
1501 
1502 
1503  moMatrix3<Real>& FromEulerAnglesYZX (Real fYAngle, Real fZAngle,
1504  Real fXAngle)
1505  {
1506  Real fCos, fSin;
1507 
1508  fCos = moMath<Real>::Cos(fYAngle);
1509  fSin = moMath<Real>::Sin(fYAngle);
1510  moMatrix3 kYMat(
1511  fCos,(Real)0.0,fSin,
1512  (Real)0.0,(Real)1.0,(Real)0.0,
1513  -fSin,(Real)0.0,fCos);
1514 
1515  fCos = moMath<Real>::Cos(fZAngle);
1516  fSin = moMath<Real>::Sin(fZAngle);
1517  moMatrix3 kZMat(
1518  fCos,-fSin,(Real)0.0,
1519  fSin,fCos,(Real)0.0,
1520  (Real)0.0,(Real)0.0,(Real)1.0);
1521 
1522  fCos = moMath<Real>::Cos(fXAngle);
1523  fSin = moMath<Real>::Sin(fXAngle);
1524  moMatrix3 kXMat(
1525  (Real)1.0,(Real)0.0,(Real)0.0,
1526  (Real)0.0,fCos,-fSin,
1527  (Real)0.0,fSin,fCos);
1528 
1529  *this = kYMat*(kZMat*kXMat);
1530  return *this;
1531  }
1532 
1533 
1534  moMatrix3<Real>& FromEulerAnglesZXY (Real fZAngle, Real fXAngle,
1535  Real fYAngle)
1536  {
1537  Real fCos, fSin;
1538 
1539  fCos = moMath<Real>::Cos(fZAngle);
1540  fSin = moMath<Real>::Sin(fZAngle);
1541  moMatrix3 kZMat(
1542  fCos,-fSin,(Real)0.0,
1543  fSin,fCos,(Real)0.0,
1544  (Real)0.0,(Real)0.0,(Real)1.0);
1545 
1546  fCos = moMath<Real>::Cos(fXAngle);
1547  fSin = moMath<Real>::Sin(fXAngle);
1548  moMatrix3 kXMat(
1549  (Real)1.0,(Real)0.0,(Real)0.0,
1550  (Real)0.0,fCos,-fSin,
1551  (Real)0.0,fSin,fCos);
1552 
1553  fCos = moMath<Real>::Cos(fYAngle);
1554  fSin = moMath<Real>::Sin(fYAngle);
1555  moMatrix3 kYMat(
1556  fCos,(Real)0.0,fSin,
1557  (Real)0.0,(Real)1.0,(Real)0.0,
1558  -fSin,(Real)0.0,fCos);
1559 
1560  *this = kZMat*(kXMat*kYMat);
1561  return *this;
1562  }
1563 
1564 
1565  moMatrix3<Real>& FromEulerAnglesZYX (Real fZAngle, Real fYAngle,
1566  Real fXAngle)
1567  {
1568  Real fCos, fSin;
1569 
1570  fCos = moMath<Real>::Cos(fZAngle);
1571  fSin = moMath<Real>::Sin(fZAngle);
1572  moMatrix3 kZMat(
1573  fCos,-fSin,(Real)0.0,
1574  fSin,fCos,(Real)0.0,
1575  (Real)0.0,(Real)0.0,(Real)1.0);
1576 
1577  fCos = moMath<Real>::Cos(fYAngle);
1578  fSin = moMath<Real>::Sin(fYAngle);
1579  moMatrix3 kYMat(
1580  fCos,(Real)0.0,fSin,
1581  (Real)0.0,(Real)1.0,(Real)0.0,
1582  -fSin,(Real)0.0,fCos);
1583 
1584  fCos = moMath<Real>::Cos(fXAngle);
1585  fSin = moMath<Real>::Sin(fXAngle);
1586  moMatrix3 kXMat(
1587  (Real)1.0,(Real)0.0,(Real)0.0,
1588  (Real)0.0,fCos,-fSin,
1589  (Real)0.0,fSin,fCos);
1590 
1591  *this = kZMat*(kYMat*kXMat);
1592  return *this;
1593  }
1594 
1595 
1596  bool ToEulerAnglesXYZ (Real& rfXAngle, Real& rfYAngle,
1597  Real& rfZAngle) const
1598  {
1599  // rot = cy*cz -cy*sz sy
1600  // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
1601  // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
1602 
1603  if (m_afEntry[2] < (Real)1.0)
1604  {
1605  if (m_afEntry[2] > -(Real)1.0)
1606  {
1607  rfXAngle = moMath<Real>::ATan2(-m_afEntry[5],m_afEntry[8]);
1608  rfYAngle = (Real)asin((double)m_afEntry[2]);
1609  rfZAngle = moMath<Real>::ATan2(-m_afEntry[1],m_afEntry[0]);
1610  return true;
1611  }
1612  else
1613  {
1614  // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
1615  rfXAngle = -moMath<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
1616  rfYAngle = -moMath<Real>::HALF_PI;
1617  rfZAngle = (Real)0.0;
1618  return false;
1619  }
1620  }
1621  else
1622  {
1623  // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
1624  rfXAngle = moMath<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
1625  rfYAngle = moMath<Real>::HALF_PI;
1626  rfZAngle = (Real)0.0;
1627  return false;
1628  }
1629  }
1630 
1631 
1632  bool ToEulerAnglesXZY (Real& rfXAngle, Real& rfZAngle,
1633  Real& rfYAngle) const
1634  {
1635  // rot = cy*cz -sz cz*sy
1636  // sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz
1637  // -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz
1638 
1639  if (m_afEntry[1] < (Real)1.0)
1640  {
1641  if (m_afEntry[1] > -(Real)1.0)
1642  {
1643  rfXAngle = moMath<Real>::ATan2(m_afEntry[7],m_afEntry[4]);
1644  rfZAngle = (Real)asin(-(double)m_afEntry[1]);
1645  rfYAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
1646  return true;
1647  }
1648  else
1649  {
1650  // WARNING. Not unique. XA - YA = atan2(r20,r22)
1651  rfXAngle = moMath<Real>::ATan2(m_afEntry[6],m_afEntry[8]);
1652  rfZAngle = moMath<Real>::HALF_PI;
1653  rfYAngle = (Real)0.0;
1654  return false;
1655  }
1656  }
1657  else
1658  {
1659  // WARNING. Not unique. XA + YA = atan2(-r20,r22)
1660  rfXAngle = moMath<Real>::ATan2(-m_afEntry[6],m_afEntry[8]);
1661  rfZAngle = -moMath<Real>::HALF_PI;
1662  rfYAngle = (Real)0.0;
1663  return false;
1664  }
1665  }
1666 
1667 
1668  bool ToEulerAnglesYXZ (Real& rfYAngle, Real& rfXAngle,
1669  Real& rfZAngle) const
1670  {
1671  // rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy
1672  // cx*sz cx*cz -sx
1673  // -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy
1674 
1675  if (m_afEntry[5] < (Real)1.0)
1676  {
1677  if (m_afEntry[5] > -(Real)1.0)
1678  {
1679  rfYAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[8]);
1680  rfXAngle = (Real)asin(-(double)m_afEntry[5]);
1681  rfZAngle = moMath<Real>::ATan2(m_afEntry[3],m_afEntry[4]);
1682  return true;
1683  }
1684  else
1685  {
1686  // WARNING. Not unique. YA - ZA = atan2(r01,r00)
1687  rfYAngle = moMath<Real>::ATan2(m_afEntry[1],m_afEntry[0]);
1688  rfXAngle = moMath<Real>::HALF_PI;
1689  rfZAngle = (Real)0.0;
1690  return false;
1691  }
1692  }
1693  else
1694  {
1695  // WARNING. Not unique. YA + ZA = atan2(-r01,r00)
1696  rfYAngle = moMath<Real>::ATan2(-m_afEntry[1],m_afEntry[0]);
1697  rfXAngle = -moMath<Real>::HALF_PI;
1698  rfZAngle = (Real)0.0;
1699  return false;
1700  }
1701  }
1702 
1703 
1704  bool ToEulerAnglesYZX (Real& rfYAngle, Real& rfZAngle,
1705  Real& rfXAngle) const
1706  {
1707  // rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz
1708  // sz cx*cz -cz*sx
1709  // -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz
1710 
1711  if (m_afEntry[3] < (Real)1.0)
1712  {
1713  if (m_afEntry[3] > -(Real)1.0)
1714  {
1715  rfYAngle = moMath<Real>::ATan2(-m_afEntry[6],m_afEntry[0]);
1716  rfZAngle = (Real)asin((double)m_afEntry[3]);
1717  rfXAngle = moMath<Real>::ATan2(-m_afEntry[5],m_afEntry[4]);
1718  return true;
1719  }
1720  else
1721  {
1722  // WARNING. Not unique. YA - XA = -atan2(r21,r22);
1723  rfYAngle = -moMath<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
1724  rfZAngle = -moMath<Real>::HALF_PI;
1725  rfXAngle = (Real)0.0;
1726  return false;
1727  }
1728  }
1729  else
1730  {
1731  // WARNING. Not unique. YA + XA = atan2(r21,r22)
1732  rfYAngle = moMath<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
1733  rfZAngle = moMath<Real>::HALF_PI;
1734  rfXAngle = (Real)0.0;
1735  return false;
1736  }
1737  }
1738 
1739 
1740  bool ToEulerAnglesZXY (Real& rfZAngle, Real& rfXAngle,
1741  Real& rfYAngle) const
1742  {
1743  // rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz
1744  // cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz
1745  // -cx*sy sx cx*cy
1746 
1747  if (m_afEntry[7] < (Real)1.0)
1748  {
1749  if (m_afEntry[7] > -(Real)1.0)
1750  {
1751  rfZAngle = moMath<Real>::ATan2(-m_afEntry[1],m_afEntry[4]);
1752  rfXAngle = (Real)asin((double)m_afEntry[7]);
1753  rfYAngle = moMath<Real>::ATan2(-m_afEntry[6],m_afEntry[8]);
1754  return true;
1755  }
1756  else
1757  {
1758  // WARNING. Not unique. ZA - YA = -atan(r02,r00)
1759  rfZAngle = -moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
1760  rfXAngle = -moMath<Real>::HALF_PI;
1761  rfYAngle = (Real)0.0;
1762  return false;
1763  }
1764  }
1765  else
1766  {
1767  // WARNING. Not unique. ZA + YA = atan2(r02,r00)
1768  rfZAngle = moMath<Real>::ATan2(m_afEntry[2],m_afEntry[0]);
1769  rfXAngle = moMath<Real>::HALF_PI;
1770  rfYAngle = (Real)0.0;
1771  return false;
1772  }
1773  }
1774 
1775 
1776  bool ToEulerAnglesZYX (Real& rfZAngle, Real& rfYAngle,
1777  Real& rfXAngle) const
1778  {
1779  // rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz
1780  // cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
1781  // -sy cy*sx cx*cy
1782 
1783  if (m_afEntry[6] < (Real)1.0)
1784  {
1785  if (m_afEntry[6] > -(Real)1.0)
1786  {
1787  rfZAngle = moMath<Real>::ATan2(m_afEntry[3],m_afEntry[0]);
1788  rfYAngle = (Real)asin(-(double)m_afEntry[6]);
1789  rfXAngle = moMath<Real>::ATan2(m_afEntry[7],m_afEntry[8]);
1790  return true;
1791  }
1792  else
1793  {
1794  // WARNING. Not unique. ZA - XA = -atan2(r01,r02)
1795  rfZAngle = -moMath<Real>::ATan2(m_afEntry[1],m_afEntry[2]);
1796  rfYAngle = moMath<Real>::HALF_PI;
1797  rfXAngle = (Real)0.0;
1798  return false;
1799  }
1800  }
1801  else
1802  {
1803  // WARNING. Not unique. ZA + XA = atan2(-r01,-r02)
1804  rfZAngle = moMath<Real>::ATan2(-m_afEntry[1],-m_afEntry[2]);
1805  rfYAngle = -moMath<Real>::HALF_PI;
1806  rfXAngle = (Real)0.0;
1807  return false;
1808  }
1809  }
1810 
1811 
1812  moMatrix3<Real>& Slerp (Real fT, const moMatrix3& rkR0,
1813  const moMatrix3& rkR1)
1814  {
1815  moVector3<Real> kAxis;
1816  Real fAngle;
1817  moMatrix3 kProd = rkR0.TransposeTimes(rkR1);
1818  kProd.ToAxisAngle(kAxis,fAngle);
1819  FromAxisAngle(kAxis,fT*fAngle);
1820  return *this;
1821  }
1822 
1823 
1824 
1825  void SingularValueDecomposition (moMatrix3& rkL, moMatrix3& rkD,
1826  moMatrix3& rkRTranspose) const
1827  {
1828  int iRow, iCol;
1829 
1830  moMatrix3 kA = *this;
1831  Bidiagonalize(kA,rkL,rkRTranspose);
1832  rkD.MakeZero();
1833 
1834  const int iMax = 32;
1835  const Real fEpsilon = (Real)1e-04;
1836  for (int i = 0; i < iMax; i++)
1837  {
1838  Real fTmp, fTmp0, fTmp1;
1839  Real fSin0, fCos0, fTan0;
1840  Real fSin1, fCos1, fTan1;
1841 
1842  bool bTest1 = (moMath<Real>::FAbs(kA[0][1]) <=
1843  fEpsilon*(moMath<Real>::FAbs(kA[0][0]) +
1844  moMath<Real>::FAbs(kA[1][1])));
1845  bool bTest2 = (moMath<Real>::FAbs(kA[1][2]) <=
1846  fEpsilon*(moMath<Real>::FAbs(kA[1][1]) +
1847  moMath<Real>::FAbs(kA[2][2])));
1848  if (bTest1)
1849  {
1850  if (bTest2)
1851  {
1852  rkD[0][0] = kA[0][0];
1853  rkD[1][1] = kA[1][1];
1854  rkD[2][2] = kA[2][2];
1855  break;
1856  }
1857  else
1858  {
1859  // 2x2 closed form factorization
1860  fTmp = (kA[1][1]*kA[1][1] - kA[2][2]*kA[2][2] +
1861  kA[1][2]*kA[1][2])/(kA[1][2]*kA[2][2]);
1862  fTan0 = ((Real)0.5)*(fTmp + moMath<Real>::Sqrt(fTmp*fTmp +
1863  ((Real)4.0)));
1864  fCos0 = moMath<Real>::InvSqrt(((Real)1.0)+fTan0*fTan0);
1865  fSin0 = fTan0*fCos0;
1866 
1867  for (iCol = 0; iCol < 3; iCol++)
1868  {
1869  fTmp0 = rkL[iCol][1];
1870  fTmp1 = rkL[iCol][2];
1871  rkL[iCol][1] = fCos0*fTmp0-fSin0*fTmp1;
1872  rkL[iCol][2] = fSin0*fTmp0+fCos0*fTmp1;
1873  }
1874 
1875  fTan1 = (kA[1][2]-kA[2][2]*fTan0)/kA[1][1];
1876  fCos1 = moMath<Real>::InvSqrt(((Real)1.0)+fTan1*fTan1);
1877  fSin1 = -fTan1*fCos1;
1878 
1879  for (iRow = 0; iRow < 3; iRow++)
1880  {
1881  fTmp0 = rkRTranspose[1][iRow];
1882  fTmp1 = rkRTranspose[2][iRow];
1883  rkRTranspose[1][iRow] = fCos1*fTmp0-fSin1*fTmp1;
1884  rkRTranspose[2][iRow] = fSin1*fTmp0+fCos1*fTmp1;
1885  }
1886 
1887  rkD[0][0] = kA[0][0];
1888  rkD[1][1] = fCos0*fCos1*kA[1][1] -
1889  fSin1*(fCos0*kA[1][2]-fSin0*kA[2][2]);
1890  rkD[2][2] = fSin0*fSin1*kA[1][1] +
1891  fCos1*(fSin0*kA[1][2]+fCos0*kA[2][2]);
1892  break;
1893  }
1894  }
1895  else
1896  {
1897  if (bTest2)
1898  {
1899  // 2x2 closed form factorization
1900  fTmp = (kA[0][0]*kA[0][0] + kA[1][1]*kA[1][1] -
1901  kA[0][1]*kA[0][1])/(kA[0][1]*kA[1][1]);
1902  fTan0 = ((Real)0.5)*(-fTmp + moMath<Real>::Sqrt(fTmp*fTmp +
1903  ((Real)4.0)));
1904  fCos0 = moMath<Real>::InvSqrt(((Real)1.0)+fTan0*fTan0);
1905  fSin0 = fTan0*fCos0;
1906 
1907  for (iCol = 0; iCol < 3; iCol++)
1908  {
1909  fTmp0 = rkL[iCol][0];
1910  fTmp1 = rkL[iCol][1];
1911  rkL[iCol][0] = fCos0*fTmp0-fSin0*fTmp1;
1912  rkL[iCol][1] = fSin0*fTmp0+fCos0*fTmp1;
1913  }
1914 
1915  fTan1 = (kA[0][1]-kA[1][1]*fTan0)/kA[0][0];
1916  fCos1 = moMath<Real>::InvSqrt(((Real)1.0)+fTan1*fTan1);
1917  fSin1 = -fTan1*fCos1;
1918 
1919  for (iRow = 0; iRow < 3; iRow++)
1920  {
1921  fTmp0 = rkRTranspose[0][iRow];
1922  fTmp1 = rkRTranspose[1][iRow];
1923  rkRTranspose[0][iRow] = fCos1*fTmp0-fSin1*fTmp1;
1924  rkRTranspose[1][iRow] = fSin1*fTmp0+fCos1*fTmp1;
1925  }
1926 
1927  rkD[0][0] = fCos0*fCos1*kA[0][0] -
1928  fSin1*(fCos0*kA[0][1]-fSin0*kA[1][1]);
1929  rkD[1][1] = fSin0*fSin1*kA[0][0] +
1930  fCos1*(fSin0*kA[0][1]+fCos0*kA[1][1]);
1931  rkD[2][2] = kA[2][2];
1932  break;
1933  }
1934  else
1935  {
1936  GolubKahanStep(kA,rkL,rkRTranspose);
1937  }
1938  }
1939  }
1940 
1941  // Make the diagonal entries positive.
1942  for (iRow = 0; iRow < 3; iRow++)
1943  {
1944  if (rkD[iRow][iRow] < (Real)0.0)
1945  {
1946  rkD[iRow][iRow] = -rkD[iRow][iRow];
1947  for (iCol = 0; iCol < 3; iCol++)
1948  {
1949  rkRTranspose[iRow][iCol] = -rkRTranspose[iRow][iCol];
1950  }
1951  }
1952  }
1953  }
1954 
1955 
1956  void SingularValueComposition (const moMatrix3& rkL,
1957  const moMatrix3& rkD, const moMatrix3& rkRTranspose)
1958  {
1959  *this = rkL*(rkD*rkRTranspose);
1960  }
1961 
1962 
1963  void PolarDecomposition (moMatrix3& rkQ, moMatrix3& rkS)
1964  {
1965  // Decompose M = L*D*R^T.
1966  moMatrix3 kL, kD, kRTranspose;
1967  SingularValueDecomposition(kL,kD,kRTranspose);
1968 
1969  // Compute Q = L*R^T.
1970  rkQ = kL*kRTranspose;
1971 
1972  // Compute S = R*D*R^T.
1973  rkS = kRTranspose.TransposeTimes(kD*kRTranspose);
1974 
1975  // Numerical round-off errors can cause S not to be symmetric in the
1976  // sense that S[i][j] and S[j][i] are slightly different for i != j.
1977  // Correct this by averaging S and Transpose(S).
1978  rkS[0][1] = ((Real)0.5)*(rkS[0][1] + rkS[1][0]);
1979  rkS[1][0] = rkS[0][1];
1980  rkS[0][2] = ((Real)0.5)*(rkS[0][2] + rkS[2][0]);
1981  rkS[2][0] = rkS[0][2];
1982  rkS[1][2] = ((Real)0.5)*(rkS[1][2] + rkS[2][1]);
1983  rkS[2][1] = rkS[1][2];
1984  }
1985 
1986 
1987  void QDUDecomposition (moMatrix3& rkQ, moMatrix3& rkD,
1988  moMatrix3& rkU) const
1989  {
1990  // Factor M = QR = QDU where Q is orthogonal (rotation), D is diagonal
1991  // (scaling), and U is upper triangular with ones on its diagonal
1992  // (shear). Algorithm uses Gram-Schmidt orthogonalization (the QR
1993  // algorithm).
1994  //
1995  // If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
1996  //
1997  // q0 = m0/|m0|
1998  // q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
1999  // q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
2000  //
2001  // where |V| indicates length of vector V and A*B indicates dot
2002  // product of vectors A and B. The matrix R has entries
2003  //
2004  // r00 = q0*m0 r01 = q0*m1 r02 = q0*m2
2005  // r10 = 0 r11 = q1*m1 r12 = q1*m2
2006  // r20 = 0 r21 = 0 r22 = q2*m2
2007  //
2008  // so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
2009  // u02 = r02/r00, and u12 = r12/r11.
2010 
2011  // build orthogonal matrix Q
2012  Real fInvLength = moMath<Real>::InvSqrt(m_afEntry[0]*m_afEntry[0] +
2013  m_afEntry[3]*m_afEntry[3] + m_afEntry[6]*m_afEntry[6]);
2014  rkQ[0][0] = m_afEntry[0]*fInvLength;
2015  rkQ[1][0] = m_afEntry[3]*fInvLength;
2016  rkQ[2][0] = m_afEntry[6]*fInvLength;
2017 
2018  Real fDot = rkQ[0][0]*m_afEntry[1] + rkQ[1][0]*m_afEntry[4] +
2019  rkQ[2][0]*m_afEntry[7];
2020  rkQ[0][1] = m_afEntry[1]-fDot*rkQ[0][0];
2021  rkQ[1][1] = m_afEntry[4]-fDot*rkQ[1][0];
2022  rkQ[2][1] = m_afEntry[7]-fDot*rkQ[2][0];
2023  fInvLength = moMath<Real>::InvSqrt(rkQ[0][1]*rkQ[0][1] +
2024  rkQ[1][1]*rkQ[1][1] + rkQ[2][1]*rkQ[2][1]);
2025  rkQ[0][1] *= fInvLength;
2026  rkQ[1][1] *= fInvLength;
2027  rkQ[2][1] *= fInvLength;
2028 
2029  fDot = rkQ[0][0]*m_afEntry[2] + rkQ[1][0]*m_afEntry[5] +
2030  rkQ[2][0]*m_afEntry[8];
2031  rkQ[0][2] = m_afEntry[2]-fDot*rkQ[0][0];
2032  rkQ[1][2] = m_afEntry[5]-fDot*rkQ[1][0];
2033  rkQ[2][2] = m_afEntry[8]-fDot*rkQ[2][0];
2034  fDot = rkQ[0][1]*m_afEntry[2] + rkQ[1][1]*m_afEntry[5] +
2035  rkQ[2][1]*m_afEntry[8];
2036  rkQ[0][2] -= fDot*rkQ[0][1];
2037  rkQ[1][2] -= fDot*rkQ[1][1];
2038  rkQ[2][2] -= fDot*rkQ[2][1];
2039  fInvLength = moMath<Real>::InvSqrt(rkQ[0][2]*rkQ[0][2] +
2040  rkQ[1][2]*rkQ[1][2] + rkQ[2][2]*rkQ[2][2]);
2041  rkQ[0][2] *= fInvLength;
2042  rkQ[1][2] *= fInvLength;
2043  rkQ[2][2] *= fInvLength;
2044 
2045  // guarantee that orthogonal matrix has determinant 1 (no reflections)
2046  Real fDet = rkQ[0][0]*rkQ[1][1]*rkQ[2][2] + rkQ[0][1]*rkQ[1][2]*rkQ[2][0]
2047  + rkQ[0][2]*rkQ[1][0]*rkQ[2][1] - rkQ[0][2]*rkQ[1][1]*rkQ[2][0]
2048  - rkQ[0][1]*rkQ[1][0]*rkQ[2][2] - rkQ[0][0]*rkQ[1][2]*rkQ[2][1];
2049 
2050  if (fDet < (Real)0.0)
2051  {
2052  for (int iRow = 0; iRow < 3; iRow++)
2053  {
2054  for (int iCol = 0; iCol < 3; iCol++)
2055  {
2056  rkQ[iRow][iCol] = -rkQ[iRow][iCol];
2057  }
2058  }
2059  }
2060 
2061  // build "right" matrix R
2062  moMatrix3 kR;
2063  kR[0][0] = rkQ[0][0]*m_afEntry[0] + rkQ[1][0]*m_afEntry[3] +
2064  rkQ[2][0]*m_afEntry[6];
2065  kR[0][1] = rkQ[0][0]*m_afEntry[1] + rkQ[1][0]*m_afEntry[4] +
2066  rkQ[2][0]*m_afEntry[7];
2067  kR[1][1] = rkQ[0][1]*m_afEntry[1] + rkQ[1][1]*m_afEntry[4] +
2068  rkQ[2][1]*m_afEntry[7];
2069  kR[0][2] = rkQ[0][0]*m_afEntry[2] + rkQ[1][0]*m_afEntry[5] +
2070  rkQ[2][0]*m_afEntry[8];
2071  kR[1][2] = rkQ[0][1]*m_afEntry[2] + rkQ[1][1]*m_afEntry[5] +
2072  rkQ[2][1]*m_afEntry[8];
2073  kR[2][2] = rkQ[0][2]*m_afEntry[2] + rkQ[1][2]*m_afEntry[5] +
2074  rkQ[2][2]*m_afEntry[8];
2075 
2076  // the scaling component
2077  rkD.MakeDiagonal(kR[0][0],kR[1][1],kR[2][2]);
2078 
2079  // the shear component
2080  Real fInvD0 = ((Real)1.0)/rkD[0][0];
2081  rkU[0][0] = (Real)1.0;
2082  rkU[0][1] = kR[0][1]*fInvD0;
2083  rkU[0][2] = kR[0][2]*fInvD0;
2084  rkU[1][0] = (Real)0.0;
2085  rkU[1][1] = (Real)1.0;
2086  rkU[1][2] = kR[1][2]/rkD[1][1];
2087  rkU[2][0] = (Real)0.0;
2088  rkU[2][1] = (Real)0.0;
2089  rkU[2][2] = (Real)1.0;
2090  }
2091 
2092 
2093  inline operator const Real* () const { return m_afEntry; }
2094  inline operator Real* () { return m_afEntry; }
2095  inline const Real* operator[] (int iRow) const { return &m_afEntry[3*iRow]; }
2096  inline Real* operator[] (int iRow) { return &m_afEntry[3*iRow]; }
2097  inline Real operator() (int iRow, int iCol) const { return m_afEntry[iCol+3*iRow]; }
2098  inline Real& operator() (int iRow, int iCol) { return m_afEntry[iCol+3*iRow]; }
2099 
2100 
2101  // assignment
2102  inline moMatrix3& operator= (const moMatrix3& rkM)
2103  {
2104  m_afEntry[0] = rkM.m_afEntry[0];
2105  m_afEntry[1] = rkM.m_afEntry[1];
2106  m_afEntry[2] = rkM.m_afEntry[2];
2107  m_afEntry[3] = rkM.m_afEntry[3];
2108  m_afEntry[4] = rkM.m_afEntry[4];
2109  m_afEntry[5] = rkM.m_afEntry[5];
2110  m_afEntry[6] = rkM.m_afEntry[6];
2111  m_afEntry[7] = rkM.m_afEntry[7];
2112  m_afEntry[8] = rkM.m_afEntry[8];
2113  return *this;
2114  }
2115 
2116  // comparison
2117 
2118  // arithmetic operations
2119  inline moMatrix3 operator+ (const moMatrix3& rkM) const
2120  {
2121  return moMatrix3<Real>(
2122  m_afEntry[0] + rkM.m_afEntry[0],
2123  m_afEntry[1] + rkM.m_afEntry[1],
2124  m_afEntry[2] + rkM.m_afEntry[2],
2125  m_afEntry[3] + rkM.m_afEntry[3],
2126  m_afEntry[4] + rkM.m_afEntry[4],
2127  m_afEntry[5] + rkM.m_afEntry[5],
2128  m_afEntry[6] + rkM.m_afEntry[6],
2129  m_afEntry[7] + rkM.m_afEntry[7],
2130  m_afEntry[8] + rkM.m_afEntry[8]);
2131  }
2132  inline moMatrix3 operator- (const moMatrix3& rkM) const
2133  {
2134  return moMatrix3<Real>(
2135  m_afEntry[0] - rkM.m_afEntry[0],
2136  m_afEntry[1] - rkM.m_afEntry[1],
2137  m_afEntry[2] - rkM.m_afEntry[2],
2138  m_afEntry[3] - rkM.m_afEntry[3],
2139  m_afEntry[4] - rkM.m_afEntry[4],
2140  m_afEntry[5] - rkM.m_afEntry[5],
2141  m_afEntry[6] - rkM.m_afEntry[6],
2142  m_afEntry[7] - rkM.m_afEntry[7],
2143  m_afEntry[8] - rkM.m_afEntry[8]);
2144  }
2145  inline moMatrix3 operator* (const moMatrix3& rkM) const
2146  {
2147  return moMatrix3<Real>(
2148  m_afEntry[0]*rkM.m_afEntry[0] +
2149  m_afEntry[1]*rkM.m_afEntry[3] +
2150  m_afEntry[2]*rkM.m_afEntry[6],
2151 
2152  m_afEntry[0]*rkM.m_afEntry[1] +
2153  m_afEntry[1]*rkM.m_afEntry[4] +
2154  m_afEntry[2]*rkM.m_afEntry[7],
2155 
2156  m_afEntry[0]*rkM.m_afEntry[2] +
2157  m_afEntry[1]*rkM.m_afEntry[5] +
2158  m_afEntry[2]*rkM.m_afEntry[8],
2159 
2160  m_afEntry[3]*rkM.m_afEntry[0] +
2161  m_afEntry[4]*rkM.m_afEntry[3] +
2162  m_afEntry[5]*rkM.m_afEntry[6],
2163 
2164  m_afEntry[3]*rkM.m_afEntry[1] +
2165  m_afEntry[4]*rkM.m_afEntry[4] +
2166  m_afEntry[5]*rkM.m_afEntry[7],
2167 
2168  m_afEntry[3]*rkM.m_afEntry[2] +
2169  m_afEntry[4]*rkM.m_afEntry[5] +
2170  m_afEntry[5]*rkM.m_afEntry[8],
2171 
2172  m_afEntry[6]*rkM.m_afEntry[0] +
2173  m_afEntry[7]*rkM.m_afEntry[3] +
2174  m_afEntry[8]*rkM.m_afEntry[6],
2175 
2176  m_afEntry[6]*rkM.m_afEntry[1] +
2177  m_afEntry[7]*rkM.m_afEntry[4] +
2178  m_afEntry[8]*rkM.m_afEntry[7],
2179 
2180  m_afEntry[6]*rkM.m_afEntry[2] +
2181  m_afEntry[7]*rkM.m_afEntry[5] +
2182  m_afEntry[8]*rkM.m_afEntry[8]);
2183  }
2184  inline moMatrix3 operator* (Real fScalar) const
2185  {
2186  return moMatrix3<Real>(
2187  fScalar*m_afEntry[0],
2188  fScalar*m_afEntry[1],
2189  fScalar*m_afEntry[2],
2190  fScalar*m_afEntry[3],
2191  fScalar*m_afEntry[4],
2192  fScalar*m_afEntry[5],
2193  fScalar*m_afEntry[6],
2194  fScalar*m_afEntry[7],
2195  fScalar*m_afEntry[8]);
2196  }
2197  inline moMatrix3 operator/ (Real fScalar) const
2198  {
2199  if (fScalar != (Real)0.0)
2200  {
2201  Real fInvScalar = ((Real)1.0)/fScalar;
2202  return moMatrix3<Real>(
2203  fInvScalar*m_afEntry[0],
2204  fInvScalar*m_afEntry[1],
2205  fInvScalar*m_afEntry[2],
2206  fInvScalar*m_afEntry[3],
2207  fInvScalar*m_afEntry[4],
2208  fInvScalar*m_afEntry[5],
2209  fInvScalar*m_afEntry[6],
2210  fInvScalar*m_afEntry[7],
2211  fInvScalar*m_afEntry[8]);
2212  }
2213 
2214  return moMatrix3<Real>(
2215  moMath<Real>::MAX_REAL,
2216  moMath<Real>::MAX_REAL,
2217  moMath<Real>::MAX_REAL,
2218  moMath<Real>::MAX_REAL,
2219  moMath<Real>::MAX_REAL,
2220  moMath<Real>::MAX_REAL,
2221  moMath<Real>::MAX_REAL,
2222  moMath<Real>::MAX_REAL,
2223  moMath<Real>::MAX_REAL);
2224  }
2225  inline moMatrix3 operator- () const
2226  {
2227  return moMatrix3<Real>(
2228  -m_afEntry[0],
2229  -m_afEntry[1],
2230  -m_afEntry[2],
2231  -m_afEntry[3],
2232  -m_afEntry[4],
2233  -m_afEntry[5],
2234  -m_afEntry[6],
2235  -m_afEntry[7],
2236  -m_afEntry[8]);
2237  }
2238 
2239  // arithmetic updates
2240  inline moMatrix3& operator+= (const moMatrix3& rkM)
2241  {
2242  m_afEntry[0] += rkM.m_afEntry[0];
2243  m_afEntry[1] += rkM.m_afEntry[1];
2244  m_afEntry[2] += rkM.m_afEntry[2];
2245  m_afEntry[3] += rkM.m_afEntry[3];
2246  m_afEntry[4] += rkM.m_afEntry[4];
2247  m_afEntry[5] += rkM.m_afEntry[5];
2248  m_afEntry[6] += rkM.m_afEntry[6];
2249  m_afEntry[7] += rkM.m_afEntry[7];
2250  m_afEntry[8] += rkM.m_afEntry[8];
2251  return *this;
2252  }
2253  inline moMatrix3& operator-= (const moMatrix3& rkM)
2254  {
2255  m_afEntry[0] -= rkM.m_afEntry[0];
2256  m_afEntry[1] -= rkM.m_afEntry[1];
2257  m_afEntry[2] -= rkM.m_afEntry[2];
2258  m_afEntry[3] -= rkM.m_afEntry[3];
2259  m_afEntry[4] -= rkM.m_afEntry[4];
2260  m_afEntry[5] -= rkM.m_afEntry[5];
2261  m_afEntry[6] -= rkM.m_afEntry[6];
2262  m_afEntry[7] -= rkM.m_afEntry[7];
2263  m_afEntry[8] -= rkM.m_afEntry[8];
2264  return *this;
2265  }
2266  inline moMatrix3& operator*= (Real fScalar)
2267  {
2268  m_afEntry[0] *= fScalar;
2269  m_afEntry[1] *= fScalar;
2270  m_afEntry[2] *= fScalar;
2271  m_afEntry[3] *= fScalar;
2272  m_afEntry[4] *= fScalar;
2273  m_afEntry[5] *= fScalar;
2274  m_afEntry[6] *= fScalar;
2275  m_afEntry[7] *= fScalar;
2276  m_afEntry[8] *= fScalar;
2277  return *this;
2278  }
2279  inline moMatrix3& operator/= (Real fScalar)
2280  {
2281  if (fScalar != (Real)0.0)
2282  {
2283  Real fInvScalar = ((Real)1.0)/fScalar;
2284  m_afEntry[0] *= fInvScalar;
2285  m_afEntry[1] *= fInvScalar;
2286  m_afEntry[2] *= fInvScalar;
2287  m_afEntry[3] *= fInvScalar;
2288  m_afEntry[4] *= fInvScalar;
2289  m_afEntry[5] *= fInvScalar;
2290  m_afEntry[6] *= fInvScalar;
2291  m_afEntry[7] *= fInvScalar;
2292  m_afEntry[8] *= fInvScalar;
2293  }
2294  else
2295  {
2296  m_afEntry[0] = moMath<Real>::MAX_REAL;
2297  m_afEntry[1] = moMath<Real>::MAX_REAL;
2298  m_afEntry[2] = moMath<Real>::MAX_REAL;
2299  m_afEntry[3] = moMath<Real>::MAX_REAL;
2300  m_afEntry[4] = moMath<Real>::MAX_REAL;
2301  m_afEntry[5] = moMath<Real>::MAX_REAL;
2302  m_afEntry[6] = moMath<Real>::MAX_REAL;
2303  m_afEntry[7] = moMath<Real>::MAX_REAL;
2304  m_afEntry[8] = moMath<Real>::MAX_REAL;
2305  }
2306 
2307  return *this;
2308  }
2309 
2310  // matrix times vector
2311  inline moVector3<Real> operator* (const moVector3<Real>& rkV) const // M * v
2312  {
2313  return moVector3<Real>(
2314  m_afEntry[0]*rkV[0] + m_afEntry[1]*rkV[1] + m_afEntry[2]*rkV[2],
2315  m_afEntry[3]*rkV[0] + m_afEntry[4]*rkV[1] + m_afEntry[5]*rkV[2],
2316  m_afEntry[6]*rkV[0] + m_afEntry[7]*rkV[1] + m_afEntry[8]*rkV[2]);
2317  }
2318 
2319 
2320 
2321  // special matrices
2322  static const moMatrix3 ZERO;
2323  static const moMatrix3 IDENTITY;
2324 
2325  private:
2326  // Support for eigendecomposition. The Tridiagonalize function applies
2327  // a Householder transformation to the matrix. If that transformation
2328  // is the identity (the matrix is already tridiagonal), then the return
2329  // value is 'false'. Otherwise, the transformation is a reflection and
2330  // the return value is 'true'. The QLAlgorithm returns 'true' iff the
2331  // QL iteration scheme converged.
2332  bool Tridiagonalize (Real afDiag[3], Real afSubd[2])
2333  {
2334  // Householder reduction T = Q^t M Q
2335  // Input:
2336  // mat, symmetric 3x3 matrix M
2337  // Output:
2338  // mat, orthogonal matrix Q (a reflection)
2339  // diag, diagonal entries of T
2340  // subd, subdiagonal entries of T (T is symmetric)
2341 
2342  Real fM00 = m_afEntry[0];
2343  Real fM01 = m_afEntry[1];
2344  Real fM02 = m_afEntry[2];
2345  Real fM11 = m_afEntry[4];
2346  Real fM12 = m_afEntry[5];
2347  Real fM22 = m_afEntry[8];
2348 
2349  afDiag[0] = fM00;
2350  if (moMath<Real>::FAbs(fM02) >= moMath<Real>::ZERO_TOLERANCE)
2351  {
2352  afSubd[0] = moMath<Real>::Sqrt(fM01*fM01+fM02*fM02);
2353  Real fInvLength = ((Real)1.0)/afSubd[0];
2354  fM01 *= fInvLength;
2355  fM02 *= fInvLength;
2356  Real fTmp = ((Real)2.0)*fM01*fM12+fM02*(fM22-fM11);
2357  afDiag[1] = fM11+fM02*fTmp;
2358  afDiag[2] = fM22-fM02*fTmp;
2359  afSubd[1] = fM12-fM01*fTmp;
2360 
2361  m_afEntry[0] = (Real)1.0;
2362  m_afEntry[1] = (Real)0.0;
2363  m_afEntry[2] = (Real)0.0;
2364  m_afEntry[3] = (Real)0.0;
2365  m_afEntry[4] = fM01;
2366  m_afEntry[5] = fM02;
2367  m_afEntry[6] = (Real)0.0;
2368  m_afEntry[7] = fM02;
2369  m_afEntry[8] = -fM01;
2370  return true;
2371  }
2372  else
2373  {
2374  afDiag[1] = fM11;
2375  afDiag[2] = fM22;
2376  afSubd[0] = fM01;
2377  afSubd[1] = fM12;
2378 
2379  m_afEntry[0] = (Real)1.0;
2380  m_afEntry[1] = (Real)0.0;
2381  m_afEntry[2] = (Real)0.0;
2382  m_afEntry[3] = (Real)0.0;
2383  m_afEntry[4] = (Real)1.0;
2384  m_afEntry[5] = (Real)0.0;
2385  m_afEntry[6] = (Real)0.0;
2386  m_afEntry[7] = (Real)0.0;
2387  m_afEntry[8] = (Real)1.0;
2388  return false;
2389  }
2390  }
2391 
2392 
2393  bool QLAlgorithm (Real afDiag[3], Real afSubd[2])
2394  {
2395  // This is an implementation of the symmetric QR algorithm from the book
2396  // "Matrix Computations" by Gene H. Golub and Charles F. Van Loan, second
2397  // edition. The algorithm is 8.2.3. The implementation has a slight
2398  // variation to actually make it a QL algorithm, and it traps the case
2399  // when either of the subdiagonal terms s0 or s1 is zero and reduces the
2400  // 2-by-2 subblock directly.
2401 
2402  const int iMax = 32;
2403  for (int i = 0; i < iMax; i++)
2404  {
2405  Real fSum, fDiff, fDiscr, fEValue0, fEValue1, fCos, fSin, fTmp;
2406  int iRow;
2407 
2408  fSum = moMath<Real>::FAbs(afDiag[0]) + moMath<Real>::FAbs(afDiag[1]);
2409  if (moMath<Real>::FAbs(afSubd[0]) + fSum == fSum)
2410  {
2411  // The matrix is effectively
2412  // +- -+
2413  // M = | d0 0 0 |
2414  // | 0 d1 s1 |
2415  // | 0 s1 d2 |
2416  // +- -+
2417 
2418  // Compute the eigenvalues as roots of a quadratic equation.
2419  fSum = afDiag[1] + afDiag[2];
2420  fDiff = afDiag[1] - afDiag[2];
2421  fDiscr = moMath<Real>::Sqrt(fDiff*fDiff +
2422  ((Real)4.0)*afSubd[1]*afSubd[1]);
2423  fEValue0 = ((Real)0.5)*(fSum - fDiscr);
2424  fEValue1 = ((Real)0.5)*(fSum + fDiscr);
2425 
2426  // Compute the Givens rotation.
2427  if (fDiff >= (Real)0.0)
2428  {
2429  fCos = afSubd[1];
2430  fSin = afDiag[1] - fEValue0;
2431  }
2432  else
2433  {
2434  fCos = afDiag[2] - fEValue0;
2435  fSin = afSubd[1];
2436  }
2437  fTmp = moMath<Real>::InvSqrt(fCos*fCos + fSin*fSin);
2438  fCos *= fTmp;
2439  fSin *= fTmp;
2440 
2441  // Postmultiply the current orthogonal matrix with the Givens
2442  // rotation.
2443  for (iRow = 0; iRow < 3; iRow++)
2444  {
2445  fTmp = m_afEntry[2+3*iRow];
2446  m_afEntry[2+3*iRow] = fSin*m_afEntry[1+3*iRow] + fCos*fTmp;
2447  m_afEntry[1+3*iRow] = fCos*m_afEntry[1+3*iRow] - fSin*fTmp;
2448  }
2449 
2450  // Update the tridiagonal matrix.
2451  afDiag[1] = fEValue0;
2452  afDiag[2] = fEValue1;
2453  afSubd[0] = (Real)0.0;
2454  afSubd[1] = (Real)0.0;
2455  return true;
2456  }
2457 
2458  fSum = moMath<Real>::FAbs(afDiag[1]) + moMath<Real>::FAbs(afDiag[2]);
2459  if (moMath<Real>::FAbs(afSubd[1]) + fSum == fSum)
2460  {
2461  // The matrix is effectively
2462  // +- -+
2463  // M = | d0 s0 0 |
2464  // | s0 d1 0 |
2465  // | 0 0 d2 |
2466  // +- -+
2467 
2468  // Compute the eigenvalues as roots of a quadratic equation.
2469  fSum = afDiag[0] + afDiag[1];
2470  fDiff = afDiag[0] - afDiag[1];
2471  fDiscr = moMath<Real>::Sqrt(fDiff*fDiff +
2472  ((Real)4.0)*afSubd[0]*afSubd[0]);
2473  fEValue0 = ((Real)0.5)*(fSum - fDiscr);
2474  fEValue1 = ((Real)0.5)*(fSum + fDiscr);
2475 
2476  // Compute the Givens rotation.
2477  if (fDiff >= (Real)0.0)
2478  {
2479  fCos = afSubd[0];
2480  fSin = afDiag[0] - fEValue0;
2481  }
2482  else
2483  {
2484  fCos = afDiag[1] - fEValue0;
2485  fSin = afSubd[0];
2486  }
2487  fTmp = moMath<Real>::InvSqrt(fCos*fCos + fSin*fSin);
2488  fCos *= fTmp;
2489  fSin *= fTmp;
2490 
2491  // Postmultiply the current orthogonal matrix with the Givens
2492  // rotation.
2493  for (iRow = 0; iRow < 3; iRow++)
2494  {
2495  fTmp = m_afEntry[1+3*iRow];
2496  m_afEntry[1+3*iRow] = fSin*m_afEntry[0+3*iRow] + fCos*fTmp;
2497  m_afEntry[0+3*iRow] = fCos*m_afEntry[0+3*iRow] - fSin*fTmp;
2498  }
2499 
2500  // Update the tridiagonal matrix.
2501  afDiag[0] = fEValue0;
2502  afDiag[1] = fEValue1;
2503  afSubd[0] = (Real)0.0;
2504  afSubd[1] = (Real)0.0;
2505  return true;
2506  }
2507 
2508  // M = | d0 s0 0 |
2509  // | s0 d1 s1 |
2510  // | 0 s1 d2 |
2511  // +- -+
2512 
2513  // Set up the parameters for the first pass of the QL step. The
2514  // value for A is the difference between diagonal term D[2] and the
2515  // implicit shift suggested by Wilkinson.
2516  Real fRatio = (afDiag[1]-afDiag[0])/(((Real)2.0f)*afSubd[0]);
2517  Real fRoot = moMath<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
2518  Real fB = afSubd[1];
2519  Real fA = afDiag[2] - afDiag[0];
2520  if (fRatio >= (Real)0.0)
2521  {
2522  fA += afSubd[0]/(fRatio + fRoot);
2523  }
2524  else
2525  {
2526  fA += afSubd[0]/(fRatio - fRoot);
2527  }
2528 
2529  // Compute the Givens rotation for the first pass.
2530  if (moMath<Real>::FAbs(fB) >= moMath<Real>::FAbs(fA))
2531  {
2532  fRatio = fA/fB;
2533  fSin = moMath<Real>::InvSqrt((Real)1.0 + fRatio*fRatio);
2534  fCos = fRatio*fSin;
2535  }
2536  else
2537  {
2538  fRatio = fB/fA;
2539  fCos = moMath<Real>::InvSqrt((Real)1.0 + fRatio*fRatio);
2540  fSin = fRatio*fCos;
2541  }
2542 
2543  // Postmultiply the current orthogonal matrix with the Givens
2544  // rotation.
2545  for (iRow = 0; iRow < 3; iRow++)
2546  {
2547  fTmp = m_afEntry[2+3*iRow];
2548  m_afEntry[2+3*iRow] = fSin*m_afEntry[1+3*iRow]+fCos*fTmp;
2549  m_afEntry[1+3*iRow] = fCos*m_afEntry[1+3*iRow]-fSin*fTmp;
2550  }
2551 
2552  // Set up the parameters for the second pass of the QL step. The
2553  // values tmp0 and tmp1 are required to fully update the tridiagonal
2554  // matrix at the end of the second pass.
2555  Real fTmp0 = (afDiag[1] - afDiag[2])*fSin +
2556  ((Real)2.0)*afSubd[1]*fCos;
2557  Real fTmp1 = fCos*afSubd[0];
2558  fB = fSin*afSubd[0];
2559  fA = fCos*fTmp0 - afSubd[1];
2560  fTmp0 *= fSin;
2561 
2562  // Compute the Givens rotation for the second pass. The subdiagonal
2563  // term S[1] in the tridiagonal matrix is updated at this time.
2564  if (moMath<Real>::FAbs(fB) >= moMath<Real>::FAbs(fA))
2565  {
2566  fRatio = fA/fB;
2567  fRoot = moMath<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
2568  afSubd[1] = fB*fRoot;
2569  fSin = ((Real)1.0)/fRoot;
2570  fCos = fRatio*fSin;
2571  }
2572  else
2573  {
2574  fRatio = fB/fA;
2575  fRoot = moMath<Real>::Sqrt((Real)1.0 + fRatio*fRatio);
2576  afSubd[1] = fA*fRoot;
2577  fCos = ((Real)1.0)/fRoot;
2578  fSin = fRatio*fCos;
2579  }
2580 
2581  // Postmultiply the current orthogonal matrix with the Givens
2582  // rotation.
2583  for (iRow = 0; iRow < 3; iRow++)
2584  {
2585  fTmp = m_afEntry[1+3*iRow];
2586  m_afEntry[1+3*iRow] = fSin*m_afEntry[0+3*iRow]+fCos*fTmp;
2587  m_afEntry[0+3*iRow] = fCos*m_afEntry[0+3*iRow]-fSin*fTmp;
2588  }
2589 
2590  // Complete the update of the tridiagonal matrix.
2591  Real fTmp2 = afDiag[1] - fTmp0;
2592  afDiag[2] += fTmp0;
2593  fTmp0 = (afDiag[0] - fTmp2)*fSin + ((Real)2.0)*fTmp1*fCos;
2594  afSubd[0] = fCos*fTmp0 - fTmp1;
2595  fTmp0 *= fSin;
2596  afDiag[1] = fTmp2 + fTmp0;
2597  afDiag[0] -= fTmp0;
2598  }
2599  return false;
2600  }
2601  /*
2602  static void Bidiagonalize (moMatrix3& rkA, moMatrix3& rkL, moMatrix3& rkR);
2603  static void GolubKahanStep (moMatrix3& rkA, moMatrix3& rkL, moMatrix3& rkR);
2604  */
2605 
2606  static void Bidiagonalize (moMatrix3& rkA, moMatrix3& rkL, moMatrix3& rkR)
2607  {
2608  Real afV[3], afW[3];
2609  Real fLength, fSign, fT1, fInvT1, fT2;
2610  bool bIdentity;
2611 
2612  // map first column to (*,0,0)
2613  fLength = moMath<Real>::Sqrt(rkA[0][0]*rkA[0][0] + rkA[1][0]*rkA[1][0] +
2614  rkA[2][0]*rkA[2][0]);
2615  if (fLength > (Real)0.0)
2616  {
2617  fSign = (rkA[0][0] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
2618  fT1 = rkA[0][0] + fSign*fLength;
2619  fInvT1 = ((Real)1.0)/fT1;
2620  afV[1] = rkA[1][0]*fInvT1;
2621  afV[2] = rkA[2][0]*fInvT1;
2622 
2623  fT2 = -((Real)2.0)/(((Real)1.0)+afV[1]*afV[1]+afV[2]*afV[2]);
2624  afW[0] = fT2*(rkA[0][0]+rkA[1][0]*afV[1]+rkA[2][0]*afV[2]);
2625  afW[1] = fT2*(rkA[0][1]+rkA[1][1]*afV[1]+rkA[2][1]*afV[2]);
2626  afW[2] = fT2*(rkA[0][2]+rkA[1][2]*afV[1]+rkA[2][2]*afV[2]);
2627  rkA[0][0] += afW[0];
2628  rkA[0][1] += afW[1];
2629  rkA[0][2] += afW[2];
2630  rkA[1][1] += afV[1]*afW[1];
2631  rkA[1][2] += afV[1]*afW[2];
2632  rkA[2][1] += afV[2]*afW[1];
2633  rkA[2][2] += afV[2]*afW[2];
2634 
2635  rkL[0][0] = ((Real)1.0)+fT2;
2636  rkL[0][1] = fT2*afV[1];
2637  rkL[1][0] = rkL[0][1];
2638  rkL[0][2] = fT2*afV[2];
2639  rkL[2][0] = rkL[0][2];
2640  rkL[1][1] = ((Real)1.0)+fT2*afV[1]*afV[1];
2641  rkL[1][2] = fT2*afV[1]*afV[2];
2642  rkL[2][1] = rkL[1][2];
2643  rkL[2][2] = ((Real)1.0)+fT2*afV[2]*afV[2];
2644  bIdentity = false;
2645  }
2646  else
2647  {
2648  rkL = moMatrix3::IDENTITY;
2649  bIdentity = true;
2650  }
2651 
2652  // map first row to (*,*,0)
2653  fLength = moMath<Real>::Sqrt(rkA[0][1]*rkA[0][1]+rkA[0][2]*rkA[0][2]);
2654  if (fLength > (Real)0.0)
2655  {
2656  fSign = (rkA[0][1] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
2657  fT1 = rkA[0][1] + fSign*fLength;
2658  afV[2] = rkA[0][2]/fT1;
2659 
2660  fT2 = -((Real)2.0)/(((Real)1.0)+afV[2]*afV[2]);
2661  afW[0] = fT2*(rkA[0][1]+rkA[0][2]*afV[2]);
2662  afW[1] = fT2*(rkA[1][1]+rkA[1][2]*afV[2]);
2663  afW[2] = fT2*(rkA[2][1]+rkA[2][2]*afV[2]);
2664  rkA[0][1] += afW[0];
2665  rkA[1][1] += afW[1];
2666  rkA[1][2] += afW[1]*afV[2];
2667  rkA[2][1] += afW[2];
2668  rkA[2][2] += afW[2]*afV[2];
2669 
2670  rkR[0][0] = (Real)1.0;
2671  rkR[0][1] = (Real)0.0;
2672  rkR[1][0] = (Real)0.0;
2673  rkR[0][2] = (Real)0.0;
2674  rkR[2][0] = (Real)0.0;
2675  rkR[1][1] = ((Real)1.0)+fT2;
2676  rkR[1][2] = fT2*afV[2];
2677  rkR[2][1] = rkR[1][2];
2678  rkR[2][2] = ((Real)1.0)+fT2*afV[2]*afV[2];
2679  }
2680  else
2681  {
2682  rkR = moMatrix3::IDENTITY;
2683  }
2684 
2685  // map second column to (*,*,0)
2686  fLength = moMath<Real>::Sqrt(rkA[1][1]*rkA[1][1]+rkA[2][1]*rkA[2][1]);
2687  if (fLength > (Real)0.0)
2688  {
2689  fSign = (rkA[1][1] > (Real)0.0 ? (Real)1.0 : -(Real)1.0);
2690  fT1 = rkA[1][1] + fSign*fLength;
2691  afV[2] = rkA[2][1]/fT1;
2692 
2693  fT2 = -((Real)2.0)/(((Real)1.0)+afV[2]*afV[2]);
2694  afW[1] = fT2*(rkA[1][1]+rkA[2][1]*afV[2]);
2695  afW[2] = fT2*(rkA[1][2]+rkA[2][2]*afV[2]);
2696  rkA[1][1] += afW[1];
2697  rkA[1][2] += afW[2];
2698  rkA[2][2] += afV[2]*afW[2];
2699 
2700  Real fA = ((Real)1.0)+fT2;
2701  Real fB = fT2*afV[2];
2702  Real fC = ((Real)1.0)+fB*afV[2];
2703 
2704  if (bIdentity)
2705  {
2706  rkL[0][0] = (Real)1.0;
2707  rkL[0][1] = (Real)0.0;
2708  rkL[1][0] = (Real)0.0;
2709  rkL[0][2] = (Real)0.0;
2710  rkL[2][0] = (Real)0.0;
2711  rkL[1][1] = fA;
2712  rkL[1][2] = fB;
2713  rkL[2][1] = fB;
2714  rkL[2][2] = fC;
2715  }
2716  else
2717  {
2718  for (int iRow = 0; iRow < 3; iRow++)
2719  {
2720  Real fTmp0 = rkL[iRow][1];
2721  Real fTmp1 = rkL[iRow][2];
2722  rkL[iRow][1] = fA*fTmp0+fB*fTmp1;
2723  rkL[iRow][2] = fB*fTmp0+fC*fTmp1;
2724  }
2725  }
2726  }
2727  }
2728 
2729 
2730 
2731  static void GolubKahanStep (moMatrix3& rkA, moMatrix3& rkL, moMatrix3& rkR)
2732  {
2733  Real fT11 = rkA[0][1]*rkA[0][1]+rkA[1][1]*rkA[1][1];
2734  Real fT22 = rkA[1][2]*rkA[1][2]+rkA[2][2]*rkA[2][2];
2735  Real fT12 = rkA[1][1]*rkA[1][2];
2736  Real fTrace = fT11+fT22;
2737  Real fDiff = fT11-fT22;
2738  Real fDiscr = moMath<Real>::Sqrt(fDiff*fDiff+((Real)4.0)*fT12*fT12);
2739  Real fRoot1 = ((Real)0.5)*(fTrace+fDiscr);
2740  Real fRoot2 = ((Real)0.5)*(fTrace-fDiscr);
2741 
2742  // adjust right
2743  Real fY = rkA[0][0] - (moMath<Real>::FAbs(fRoot1-fT22) <=
2744  moMath<Real>::FAbs(fRoot2-fT22) ? fRoot1 : fRoot2);
2745  Real fZ = rkA[0][1];
2746  Real fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2747  Real fSin = fZ*fInvLength;
2748  Real fCos = -fY*fInvLength;
2749 
2750  Real fTmp0 = rkA[0][0];
2751  Real fTmp1 = rkA[0][1];
2752  rkA[0][0] = fCos*fTmp0-fSin*fTmp1;
2753  rkA[0][1] = fSin*fTmp0+fCos*fTmp1;
2754  rkA[1][0] = -fSin*rkA[1][1];
2755  rkA[1][1] *= fCos;
2756 
2757  int iRow;
2758  for (iRow = 0; iRow < 3; iRow++)
2759  {
2760  fTmp0 = rkR[0][iRow];
2761  fTmp1 = rkR[1][iRow];
2762  rkR[0][iRow] = fCos*fTmp0-fSin*fTmp1;
2763  rkR[1][iRow] = fSin*fTmp0+fCos*fTmp1;
2764  }
2765 
2766  // adjust left
2767  fY = rkA[0][0];
2768  fZ = rkA[1][0];
2769  fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2770  fSin = fZ*fInvLength;
2771  fCos = -fY*fInvLength;
2772 
2773  rkA[0][0] = fCos*rkA[0][0]-fSin*rkA[1][0];
2774  fTmp0 = rkA[0][1];
2775  fTmp1 = rkA[1][1];
2776  rkA[0][1] = fCos*fTmp0-fSin*fTmp1;
2777  rkA[1][1] = fSin*fTmp0+fCos*fTmp1;
2778  rkA[0][2] = -fSin*rkA[1][2];
2779  rkA[1][2] *= fCos;
2780 
2781  int iCol;
2782  for (iCol = 0; iCol < 3; iCol++)
2783  {
2784  fTmp0 = rkL[iCol][0];
2785  fTmp1 = rkL[iCol][1];
2786  rkL[iCol][0] = fCos*fTmp0-fSin*fTmp1;
2787  rkL[iCol][1] = fSin*fTmp0+fCos*fTmp1;
2788  }
2789 
2790  // adjust right
2791  fY = rkA[0][1];
2792  fZ = rkA[0][2];
2793  fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2794  fSin = fZ*fInvLength;
2795  fCos = -fY*fInvLength;
2796 
2797  rkA[0][1] = fCos*rkA[0][1]-fSin*rkA[0][2];
2798  fTmp0 = rkA[1][1];
2799  fTmp1 = rkA[1][2];
2800  rkA[1][1] = fCos*fTmp0-fSin*fTmp1;
2801  rkA[1][2] = fSin*fTmp0+fCos*fTmp1;
2802  rkA[2][1] = -fSin*rkA[2][2];
2803  rkA[2][2] *= fCos;
2804 
2805  for (iRow = 0; iRow < 3; iRow++)
2806  {
2807  fTmp0 = rkR[1][iRow];
2808  fTmp1 = rkR[2][iRow];
2809  rkR[1][iRow] = fCos*fTmp0-fSin*fTmp1;
2810  rkR[2][iRow] = fSin*fTmp0+fCos*fTmp1;
2811  }
2812 
2813  // adjust left
2814  fY = rkA[1][1];
2815  fZ = rkA[2][1];
2816  fInvLength = moMath<Real>::InvSqrt(fY*fY+fZ*fZ);
2817  fSin = fZ*fInvLength;
2818  fCos = -fY*fInvLength;
2819 
2820  rkA[1][1] = fCos*rkA[1][1]-fSin*rkA[2][1];
2821  fTmp0 = rkA[1][2];
2822  fTmp1 = rkA[2][2];
2823  rkA[1][2] = fCos*fTmp0-fSin*fTmp1;
2824  rkA[2][2] = fSin*fTmp0+fCos*fTmp1;
2825 
2826  for (iCol = 0; iCol < 3; iCol++)
2827  {
2828  fTmp0 = rkL[iCol][1];
2829  fTmp1 = rkL[iCol][2];
2830  rkL[iCol][1] = fCos*fTmp0-fSin*fTmp1;
2831  rkL[iCol][2] = fSin*fTmp0+fCos*fTmp1;
2832  }
2833  }
2834 
2835  // support for comparisons
2836  int CompareArrays (const moMatrix3& rkM) const
2837  {
2838  return memcmp(m_afEntry,rkM.m_afEntry,9*sizeof(Real));
2839  }
2840 
2841  Real m_afEntry[9];
2842 };
2843 
2844 // c * M
2845 template <class Real>
2846 inline moMatrix3<Real> operator* (Real fScalar, const moMatrix3<Real>& rkM)
2847 {
2848  return rkM*fScalar;
2849 }
2850 
2851 // v^T * M
2852 template <class Real>
2853 inline moVector3<Real> operator* (const moVector3<Real>& rkV,
2854  const moMatrix3<Real>& rkM)
2855 {
2856  return moVector3<Real>(
2857  rkV[0]*rkM[0][0] + rkV[1]*rkM[1][0] + rkV[2]*rkM[2][0],
2858  rkV[0]*rkM[0][1] + rkV[1]*rkM[1][1] + rkV[2]*rkM[2][1],
2859  rkV[0]*rkM[0][2] + rkV[1]*rkM[1][2] + rkV[2]*rkM[2][2]);
2860 }
2861 
2862 #ifndef MO_MACOSX
2863 #ifndef MO_WIN32
2864 #ifndef MO_RASPBIAN
2865 template class LIBMOLDEO_API moMatrix3<MOfloat>;
2866 #endif
2867 #endif
2868 #endif
2869 typedef moMatrix3<MOfloat> moMatrix3f;
2870 
2871 #ifndef MO_MACOSX
2872 #ifndef MO_WIN32
2873 #ifndef MO_RASPBIAN
2874 template class LIBMOLDEO_API moMatrix3<MOdouble>;
2875 #endif
2876 #endif
2877 #endif
2878 typedef moMatrix3<MOdouble> moMatrix3d;
2879 
2880 moDeclareExportedDynamicArray( moMatrix3f, moMatrix3fArray );
2881 moDeclareExportedDynamicArray( moMatrix3d, moMatrix3dArray );
2882 
2883 
2884 // moMatrix4 class ------------------------------------------------------------
2885 
2886 template <class Real>
2887 class LIBMOLDEO_API moMatrix4 : public moAbstract
2888 {
2889  public:
2890  // If bZero is true, create the zero matrix. Otherwise, create the
2891  // identity matrix.
2892  moMatrix4 (bool bZero = true)
2893  {
2894  if (bZero)
2895  {
2896  MakeZero();
2897  }
2898  else
2899  {
2900  MakeIdentity();
2901  }
2902  }
2903 
2904  // copy constructor
2905  moMatrix4 (const moMatrix4& rkM) : moAbstract()
2906  {
2907  m_afEntry[ 0] = rkM.m_afEntry[ 0];
2908  m_afEntry[ 1] = rkM.m_afEntry[ 1];
2909  m_afEntry[ 2] = rkM.m_afEntry[ 2];
2910  m_afEntry[ 3] = rkM.m_afEntry[ 3];
2911  m_afEntry[ 4] = rkM.m_afEntry[ 4];
2912  m_afEntry[ 5] = rkM.m_afEntry[ 5];
2913  m_afEntry[ 6] = rkM.m_afEntry[ 6];
2914  m_afEntry[ 7] = rkM.m_afEntry[ 7];
2915  m_afEntry[ 8] = rkM.m_afEntry[ 8];
2916  m_afEntry[ 9] = rkM.m_afEntry[ 9];
2917  m_afEntry[10] = rkM.m_afEntry[10];
2918  m_afEntry[11] = rkM.m_afEntry[11];
2919  m_afEntry[12] = rkM.m_afEntry[12];
2920  m_afEntry[13] = rkM.m_afEntry[13];
2921  m_afEntry[14] = rkM.m_afEntry[14];
2922  m_afEntry[15] = rkM.m_afEntry[15];
2923 }
2924 
2925  // input Mrc is in row r, column c.
2926  moMatrix4 (Real fM00, Real fM01, Real fM02, Real fM03,
2927  Real fM10, Real fM11, Real fM12, Real fM13,
2928  Real fM20, Real fM21, Real fM22, Real fM23,
2929  Real fM30, Real fM31, Real fM32, Real fM33) {
2930  m_afEntry[ 0] = fM00;
2931  m_afEntry[ 1] = fM01;
2932  m_afEntry[ 2] = fM02;
2933  m_afEntry[ 3] = fM03;
2934  m_afEntry[ 4] = fM10;
2935  m_afEntry[ 5] = fM11;
2936  m_afEntry[ 6] = fM12;
2937  m_afEntry[ 7] = fM13;
2938  m_afEntry[ 8] = fM20;
2939  m_afEntry[ 9] = fM21;
2940  m_afEntry[10] = fM22;
2941  m_afEntry[11] = fM23;
2942  m_afEntry[12] = fM30;
2943  m_afEntry[13] = fM31;
2944  m_afEntry[14] = fM32;
2945  m_afEntry[15] = fM33;
2946 }
2947 
2948  moMatrix4 (const Real afEntry[16], bool bRowMajor = true )
2949  {
2950  if (bRowMajor)
2951  {
2952  m_afEntry[ 0] = afEntry[ 0];
2953  m_afEntry[ 1] = afEntry[ 1];
2954  m_afEntry[ 2] = afEntry[ 2];
2955  m_afEntry[ 3] = afEntry[ 3];
2956  m_afEntry[ 4] = afEntry[ 4];
2957  m_afEntry[ 5] = afEntry[ 5];
2958  m_afEntry[ 6] = afEntry[ 6];
2959  m_afEntry[ 7] = afEntry[ 7];
2960  m_afEntry[ 8] = afEntry[ 8];
2961  m_afEntry[ 9] = afEntry[ 9];
2962  m_afEntry[10] = afEntry[10];
2963  m_afEntry[11] = afEntry[11];
2964  m_afEntry[12] = afEntry[12];
2965  m_afEntry[13] = afEntry[13];
2966  m_afEntry[14] = afEntry[14];
2967  m_afEntry[15] = afEntry[15];
2968  }
2969  else
2970  {
2971  m_afEntry[ 0] = afEntry[ 0];
2972  m_afEntry[ 1] = afEntry[ 4];
2973  m_afEntry[ 2] = afEntry[ 8];
2974  m_afEntry[ 3] = afEntry[12];
2975  m_afEntry[ 4] = afEntry[ 1];
2976  m_afEntry[ 5] = afEntry[ 5];
2977  m_afEntry[ 6] = afEntry[ 9];
2978  m_afEntry[ 7] = afEntry[13];
2979  m_afEntry[ 8] = afEntry[ 2];
2980  m_afEntry[ 9] = afEntry[ 6];
2981  m_afEntry[10] = afEntry[10];
2982  m_afEntry[11] = afEntry[14];
2983  m_afEntry[12] = afEntry[ 3];
2984  m_afEntry[13] = afEntry[ 7];
2985  m_afEntry[14] = afEntry[11];
2986  m_afEntry[15] = afEntry[15];
2987  }
2988  }
2989 
2990  moMatrix4<Real>& MakeZero ()
2991  {
2992  m_afEntry[ 0] = (Real)0.0;
2993  m_afEntry[ 1] = (Real)0.0;
2994  m_afEntry[ 2] = (Real)0.0;
2995  m_afEntry[ 3] = (Real)0.0;
2996  m_afEntry[ 4] = (Real)0.0;
2997  m_afEntry[ 5] = (Real)0.0;
2998  m_afEntry[ 6] = (Real)0.0;
2999  m_afEntry[ 7] = (Real)0.0;
3000  m_afEntry[ 8] = (Real)0.0;
3001  m_afEntry[ 9] = (Real)0.0;
3002  m_afEntry[10] = (Real)0.0;
3003  m_afEntry[11] = (Real)0.0;
3004  m_afEntry[12] = (Real)0.0;
3005  m_afEntry[13] = (Real)0.0;
3006  m_afEntry[14] = (Real)0.0;
3007  m_afEntry[15] = (Real)0.0;
3008  (*this) = moMatrix4::ZERO;
3009  return (*this);
3010  }
3011 
3012  moMatrix4<Real>& MakeIdentity ()
3013  {
3014  m_afEntry[ 0] = (Real)1.0;
3015  m_afEntry[ 1] = (Real)0.0;
3016  m_afEntry[ 2] = (Real)0.0;
3017  m_afEntry[ 3] = (Real)0.0;
3018  m_afEntry[ 4] = (Real)0.0;
3019  m_afEntry[ 5] = (Real)1.0;
3020  m_afEntry[ 6] = (Real)0.0;
3021  m_afEntry[ 7] = (Real)0.0;
3022  m_afEntry[ 8] = (Real)0.0;
3023  m_afEntry[ 9] = (Real)0.0;
3024  m_afEntry[10] = (Real)1.0;
3025  m_afEntry[11] = (Real)0.0;
3026  m_afEntry[12] = (Real)0.0;
3027  m_afEntry[13] = (Real)0.0;
3028  m_afEntry[14] = (Real)0.0;
3029  m_afEntry[15] = (Real)1.0;
3030  (*this) = moMatrix4::IDENTITY;
3031  return (*this);
3032  }
3033 
3034  void SetRow (int iRow, const moVector4<Real>& rkV)
3035  {
3036  int i0 = 4*iRow, i1 = i0+1, i2 = i1+1, i3 = i2+1;
3037  m_afEntry[i0] = rkV[0];
3038  m_afEntry[i1] = rkV[1];
3039  m_afEntry[i2] = rkV[2];
3040  m_afEntry[i3] = rkV[3];
3041  }
3042 
3043  moVector4<Real> GetRow (int iRow) const
3044  {
3045  int i0 = 4*iRow, i1 = i0+1, i2 = i1+1, i3 = i2+1;
3046  return moVector4<Real>(m_afEntry[i0],m_afEntry[i1],m_afEntry[i2],
3047  m_afEntry[i3]);
3048  }
3049 
3050  void SetColumn (int iCol, const moVector4<Real>& rkV)
3051  {
3052  m_afEntry[iCol] = rkV[0];
3053  m_afEntry[iCol+4] = rkV[1];
3054  m_afEntry[iCol+8] = rkV[2];
3055  m_afEntry[iCol+12] = rkV[3];
3056  }
3057 
3058  moVector4<Real> GetColumn (int iCol) const
3059  {
3060  return moVector4<Real>(m_afEntry[iCol],m_afEntry[iCol+4],m_afEntry[iCol+8],
3061  m_afEntry[iCol+12]);
3062  }
3063 
3064  void GetColumnMajor (Real* afCMajor) const
3065  {
3066  afCMajor[ 0] = m_afEntry[ 0];
3067  afCMajor[ 1] = m_afEntry[ 4];
3068  afCMajor[ 2] = m_afEntry[ 8];
3069  afCMajor[ 3] = m_afEntry[12];
3070  afCMajor[ 4] = m_afEntry[ 1];
3071  afCMajor[ 5] = m_afEntry[ 5];
3072  afCMajor[ 6] = m_afEntry[ 9];
3073  afCMajor[ 7] = m_afEntry[13];
3074  afCMajor[ 8] = m_afEntry[ 2];
3075  afCMajor[ 9] = m_afEntry[ 6];
3076  afCMajor[10] = m_afEntry[10];
3077  afCMajor[11] = m_afEntry[14];
3078  afCMajor[12] = m_afEntry[ 3];
3079  afCMajor[13] = m_afEntry[ 7];
3080  afCMajor[14] = m_afEntry[11];
3081  afCMajor[15] = m_afEntry[15];
3082  }
3083 
3084  bool operator== (const moMatrix4& rkM) const
3085  {
3086  return CompareArrays(rkM) == 0;
3087  }
3088 
3089  bool operator!= (const moMatrix4& rkM) const
3090  {
3091  return CompareArrays(rkM) != 0;
3092  }
3093 
3094  bool operator< (const moMatrix4& rkM) const
3095  {
3096  return CompareArrays(rkM) < 0;
3097  }
3098 
3099  bool operator<= (const moMatrix4& rkM) const
3100  {
3101  return CompareArrays(rkM) <= 0;
3102  }
3103 
3104  bool operator> (const moMatrix4& rkM) const
3105  {
3106  return CompareArrays(rkM) > 0;
3107  }
3108 
3109  bool operator>= (const moMatrix4& rkM) const
3110  {
3111  return CompareArrays(rkM) >= 0;
3112  }
3113 
3114  moMatrix4<Real> Transpose () const
3115  {
3116  return moMatrix4<Real>(
3117  m_afEntry[ 0],
3118  m_afEntry[ 4],
3119  m_afEntry[ 8],
3120  m_afEntry[12],
3121  m_afEntry[ 1],
3122  m_afEntry[ 5],
3123  m_afEntry[ 9],
3124  m_afEntry[13],
3125  m_afEntry[ 2],
3126  m_afEntry[ 6],
3127  m_afEntry[10],
3128  m_afEntry[14],
3129  m_afEntry[ 3],
3130  m_afEntry[ 7],
3131  m_afEntry[11],
3132  m_afEntry[15]);
3133  }
3134 
3135  moMatrix4<Real> TransposeTimes (const moMatrix4& rkM) const
3136  {
3137  // P = A^T*B
3138  return moMatrix4<Real>(
3139  m_afEntry[ 0]*rkM.m_afEntry[ 0] +
3140  m_afEntry[ 4]*rkM.m_afEntry[ 4] +
3141  m_afEntry[ 8]*rkM.m_afEntry[ 8] +
3142  m_afEntry[12]*rkM.m_afEntry[12],
3143 
3144  m_afEntry[ 0]*rkM.m_afEntry[ 1] +
3145  m_afEntry[ 4]*rkM.m_afEntry[ 5] +
3146  m_afEntry[ 8]*rkM.m_afEntry[ 9] +
3147  m_afEntry[12]*rkM.m_afEntry[13],
3148 
3149  m_afEntry[ 0]*rkM.m_afEntry[ 2] +
3150  m_afEntry[ 4]*rkM.m_afEntry[ 6] +
3151  m_afEntry[ 8]*rkM.m_afEntry[10] +
3152  m_afEntry[12]*rkM.m_afEntry[14],
3153 
3154  m_afEntry[ 0]*rkM.m_afEntry[ 3] +
3155  m_afEntry[ 4]*rkM.m_afEntry[ 7] +
3156  m_afEntry[ 8]*rkM.m_afEntry[11] +
3157  m_afEntry[12]*rkM.m_afEntry[15],
3158 
3159  m_afEntry[ 1]*rkM.m_afEntry[ 0] +
3160  m_afEntry[ 5]*rkM.m_afEntry[ 4] +
3161  m_afEntry[ 9]*rkM.m_afEntry[ 8] +
3162  m_afEntry[13]*rkM.m_afEntry[12],
3163 
3164  m_afEntry[ 1]*rkM.m_afEntry[ 1] +
3165  m_afEntry[ 5]*rkM.m_afEntry[ 5] +
3166  m_afEntry[ 9]*rkM.m_afEntry[ 9] +
3167  m_afEntry[13]*rkM.m_afEntry[13],
3168 
3169  m_afEntry[ 1]*rkM.m_afEntry[ 2] +
3170  m_afEntry[ 5]*rkM.m_afEntry[ 6] +
3171  m_afEntry[ 9]*rkM.m_afEntry[10] +
3172  m_afEntry[13]*rkM.m_afEntry[14],
3173 
3174  m_afEntry[ 1]*rkM.m_afEntry[ 3] +
3175  m_afEntry[ 5]*rkM.m_afEntry[ 7] +
3176  m_afEntry[ 9]*rkM.m_afEntry[11] +
3177  m_afEntry[13]*rkM.m_afEntry[15],
3178 
3179  m_afEntry[ 2]*rkM.m_afEntry[ 0] +
3180  m_afEntry[ 6]*rkM.m_afEntry[ 4] +
3181  m_afEntry[10]*rkM.m_afEntry[ 8] +
3182  m_afEntry[14]*rkM.m_afEntry[12],
3183 
3184  m_afEntry[ 2]*rkM.m_afEntry[ 1] +
3185  m_afEntry[ 6]*rkM.m_afEntry[ 5] +
3186  m_afEntry[10]*rkM.m_afEntry[ 9] +
3187  m_afEntry[14]*rkM.m_afEntry[13],
3188 
3189  m_afEntry[ 2]*rkM.m_afEntry[ 2] +
3190  m_afEntry[ 6]*rkM.m_afEntry[ 6] +
3191  m_afEntry[10]*rkM.m_afEntry[10] +
3192  m_afEntry[14]*rkM.m_afEntry[14],
3193 
3194  m_afEntry[ 2]*rkM.m_afEntry[ 3] +
3195  m_afEntry[ 6]*rkM.m_afEntry[ 7] +
3196  m_afEntry[10]*rkM.m_afEntry[11] +
3197  m_afEntry[14]*rkM.m_afEntry[15],
3198 
3199  m_afEntry[ 3]*rkM.m_afEntry[ 0] +
3200  m_afEntry[ 7]*rkM.m_afEntry[ 4] +
3201  m_afEntry[11]*rkM.m_afEntry[ 8] +
3202  m_afEntry[15]*rkM.m_afEntry[12],
3203 
3204  m_afEntry[ 3]*rkM.m_afEntry[ 1] +
3205  m_afEntry[ 7]*rkM.m_afEntry[ 5] +
3206  m_afEntry[11]*rkM.m_afEntry[ 9] +
3207  m_afEntry[15]*rkM.m_afEntry[13],
3208 
3209  m_afEntry[ 3]*rkM.m_afEntry[ 2] +
3210  m_afEntry[ 7]*rkM.m_afEntry[ 6] +
3211  m_afEntry[11]*rkM.m_afEntry[10] +
3212  m_afEntry[15]*rkM.m_afEntry[14],
3213 
3214  m_afEntry[ 3]*rkM.m_afEntry[ 3] +
3215  m_afEntry[ 7]*rkM.m_afEntry[ 7] +
3216  m_afEntry[11]*rkM.m_afEntry[11] +
3217  m_afEntry[15]*rkM.m_afEntry[15]);
3218  }
3219 
3220  moMatrix4<Real> TimesTranspose (const moMatrix4& rkM) const
3221  {
3222  // P = A*B^T
3223  return moMatrix4<Real>(
3224  m_afEntry[ 0]*rkM.m_afEntry[ 0] +
3225  m_afEntry[ 1]*rkM.m_afEntry[ 1] +
3226  m_afEntry[ 2]*rkM.m_afEntry[ 2] +
3227  m_afEntry[ 3]*rkM.m_afEntry[ 3],
3228 
3229  m_afEntry[ 0]*rkM.m_afEntry[ 4] +
3230  m_afEntry[ 1]*rkM.m_afEntry[ 5] +
3231  m_afEntry[ 2]*rkM.m_afEntry[ 6] +
3232  m_afEntry[ 3]*rkM.m_afEntry[ 7],
3233 
3234  m_afEntry[ 0]*rkM.m_afEntry[ 8] +
3235  m_afEntry[ 1]*rkM.m_afEntry[ 9] +
3236  m_afEntry[ 2]*rkM.m_afEntry[10] +
3237  m_afEntry[ 3]*rkM.m_afEntry[11],
3238 
3239  m_afEntry[ 0]*rkM.m_afEntry[12] +
3240  m_afEntry[ 1]*rkM.m_afEntry[13] +
3241  m_afEntry[ 2]*rkM.m_afEntry[14] +
3242  m_afEntry[ 3]*rkM.m_afEntry[15],
3243 
3244  m_afEntry[ 4]*rkM.m_afEntry[ 0] +
3245  m_afEntry[ 5]*rkM.m_afEntry[ 1] +
3246  m_afEntry[ 6]*rkM.m_afEntry[ 2] +
3247  m_afEntry[ 7]*rkM.m_afEntry[ 3],
3248 
3249  m_afEntry[ 4]*rkM.m_afEntry[ 4] +
3250  m_afEntry[ 5]*rkM.m_afEntry[ 5] +
3251  m_afEntry[ 6]*rkM.m_afEntry[ 6] +
3252  m_afEntry[ 7]*rkM.m_afEntry[ 7],
3253 
3254  m_afEntry[ 4]*rkM.m_afEntry[ 8] +
3255  m_afEntry[ 5]*rkM.m_afEntry[ 9] +
3256  m_afEntry[ 6]*rkM.m_afEntry[10] +
3257  m_afEntry[ 7]*rkM.m_afEntry[11],
3258 
3259  m_afEntry[ 4]*rkM.m_afEntry[12] +
3260  m_afEntry[ 5]*rkM.m_afEntry[13] +
3261  m_afEntry[ 6]*rkM.m_afEntry[14] +
3262  m_afEntry[ 7]*rkM.m_afEntry[15],
3263 
3264  m_afEntry[ 8]*rkM.m_afEntry[ 0] +
3265  m_afEntry[ 9]*rkM.m_afEntry[ 1] +
3266  m_afEntry[10]*rkM.m_afEntry[ 2] +
3267  m_afEntry[11]*rkM.m_afEntry[ 3],
3268 
3269  m_afEntry[ 8]*rkM.m_afEntry[ 4] +
3270  m_afEntry[ 9]*rkM.m_afEntry[ 5] +
3271  m_afEntry[10]*rkM.m_afEntry[ 6] +
3272  m_afEntry[11]*rkM.m_afEntry[ 7],
3273 
3274  m_afEntry[ 8]*rkM.m_afEntry[ 8] +
3275  m_afEntry[ 9]*rkM.m_afEntry[ 9] +
3276  m_afEntry[10]*rkM.m_afEntry[10] +
3277  m_afEntry[11]*rkM.m_afEntry[11],
3278 
3279  m_afEntry[ 8]*rkM.m_afEntry[12] +
3280  m_afEntry[ 9]*rkM.m_afEntry[13] +
3281  m_afEntry[10]*rkM.m_afEntry[14] +
3282  m_afEntry[11]*rkM.m_afEntry[15],
3283 
3284  m_afEntry[12]*rkM.m_afEntry[ 0] +
3285  m_afEntry[13]*rkM.m_afEntry[ 1] +
3286  m_afEntry[14]*rkM.m_afEntry[ 2] +
3287  m_afEntry[15]*rkM.m_afEntry[ 3],
3288 
3289  m_afEntry[12]*rkM.m_afEntry[ 4] +
3290  m_afEntry[13]*rkM.m_afEntry[ 5] +
3291  m_afEntry[14]*rkM.m_afEntry[ 6] +
3292  m_afEntry[15]*rkM.m_afEntry[ 7],
3293 
3294  m_afEntry[12]*rkM.m_afEntry[ 8] +
3295  m_afEntry[13]*rkM.m_afEntry[ 9] +
3296  m_afEntry[14]*rkM.m_afEntry[10] +
3297  m_afEntry[15]*rkM.m_afEntry[11],
3298 
3299  m_afEntry[12]*rkM.m_afEntry[12] +
3300  m_afEntry[13]*rkM.m_afEntry[13] +
3301  m_afEntry[14]*rkM.m_afEntry[14] +
3302  m_afEntry[15]*rkM.m_afEntry[15]);
3303  }
3304 
3305  moMatrix4<Real> Inverse () const
3306  {
3307  Real fA0 = m_afEntry[ 0]*m_afEntry[ 5] - m_afEntry[ 1]*m_afEntry[ 4];
3308  Real fA1 = m_afEntry[ 0]*m_afEntry[ 6] - m_afEntry[ 2]*m_afEntry[ 4];
3309  Real fA2 = m_afEntry[ 0]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 4];
3310  Real fA3 = m_afEntry[ 1]*m_afEntry[ 6] - m_afEntry[ 2]*m_afEntry[ 5];
3311  Real fA4 = m_afEntry[ 1]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 5];
3312  Real fA5 = m_afEntry[ 2]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 6];
3313  Real fB0 = m_afEntry[ 8]*m_afEntry[13] - m_afEntry[ 9]*m_afEntry[12];
3314  Real fB1 = m_afEntry[ 8]*m_afEntry[14] - m_afEntry[10]*m_afEntry[12];
3315  Real fB2 = m_afEntry[ 8]*m_afEntry[15] - m_afEntry[11]*m_afEntry[12];
3316  Real fB3 = m_afEntry[ 9]*m_afEntry[14] - m_afEntry[10]*m_afEntry[13];
3317  Real fB4 = m_afEntry[ 9]*m_afEntry[15] - m_afEntry[11]*m_afEntry[13];
3318  Real fB5 = m_afEntry[10]*m_afEntry[15] - m_afEntry[11]*m_afEntry[14];
3319 
3320  Real fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
3321  if (moMath<Real>::FAbs(fDet) <= moMath<Real>::ZERO_TOLERANCE)
3322  {
3323  return moMatrix4<Real>::ZERO;
3324  }
3325 
3326  moMatrix4 kInv;
3327  kInv.m_afEntry[ 0] =
3328  + m_afEntry[ 5]*fB5 - m_afEntry[ 6]*fB4 + m_afEntry[ 7]*fB3;
3329  kInv.m_afEntry[ 4] =
3330  - m_afEntry[ 4]*fB5 + m_afEntry[ 6]*fB2 - m_afEntry[ 7]*fB1;
3331  kInv.m_afEntry[ 8] =
3332  + m_afEntry[ 4]*fB4 - m_afEntry[ 5]*fB2 + m_afEntry[ 7]*fB0;
3333  kInv.m_afEntry[12] =
3334  - m_afEntry[ 4]*fB3 + m_afEntry[ 5]*fB1 - m_afEntry[ 6]*fB0;
3335  kInv.m_afEntry[ 1] =
3336  - m_afEntry[ 1]*fB5 + m_afEntry[ 2]*fB4 - m_afEntry[ 3]*fB3;
3337  kInv.m_afEntry[ 5] =
3338  + m_afEntry[ 0]*fB5 - m_afEntry[ 2]*fB2 + m_afEntry[ 3]*fB1;
3339  kInv.m_afEntry[ 9] =
3340  - m_afEntry[ 0]*fB4 + m_afEntry[ 1]*fB2 - m_afEntry[ 3]*fB0;
3341  kInv.m_afEntry[13] =
3342  + m_afEntry[ 0]*fB3 - m_afEntry[ 1]*fB1 + m_afEntry[ 2]*fB0;
3343  kInv.m_afEntry[ 2] =
3344  + m_afEntry[13]*fA5 - m_afEntry[14]*fA4 + m_afEntry[15]*fA3;
3345  kInv.m_afEntry[ 6] =
3346  - m_afEntry[12]*fA5 + m_afEntry[14]*fA2 - m_afEntry[15]*fA1;
3347  kInv.m_afEntry[10] =
3348  + m_afEntry[12]*fA4 - m_afEntry[13]*fA2 + m_afEntry[15]*fA0;
3349  kInv.m_afEntry[14] =
3350  - m_afEntry[12]*fA3 + m_afEntry[13]*fA1 - m_afEntry[14]*fA0;
3351  kInv.m_afEntry[ 3] =
3352  - m_afEntry[ 9]*fA5 + m_afEntry[10]*fA4 - m_afEntry[11]*fA3;
3353  kInv.m_afEntry[ 7] =
3354  + m_afEntry[ 8]*fA5 - m_afEntry[10]*fA2 + m_afEntry[11]*fA1;
3355  kInv.m_afEntry[11] =
3356  - m_afEntry[ 8]*fA4 + m_afEntry[ 9]*fA2 - m_afEntry[11]*fA0;
3357  kInv.m_afEntry[15] =
3358  + m_afEntry[ 8]*fA3 - m_afEntry[ 9]*fA1 + m_afEntry[10]*fA0;
3359 
3360  Real fInvDet = ((Real)1.0)/fDet;
3361  kInv.m_afEntry[ 0] *= fInvDet;
3362  kInv.m_afEntry[ 1] *= fInvDet;
3363  kInv.m_afEntry[ 2] *= fInvDet;
3364  kInv.m_afEntry[ 3] *= fInvDet;
3365  kInv.m_afEntry[ 4] *= fInvDet;
3366  kInv.m_afEntry[ 5] *= fInvDet;
3367  kInv.m_afEntry[ 6] *= fInvDet;
3368  kInv.m_afEntry[ 7] *= fInvDet;
3369  kInv.m_afEntry[ 8] *= fInvDet;
3370  kInv.m_afEntry[ 9] *= fInvDet;
3371  kInv.m_afEntry[10] *= fInvDet;
3372  kInv.m_afEntry[11] *= fInvDet;
3373  kInv.m_afEntry[12] *= fInvDet;
3374  kInv.m_afEntry[13] *= fInvDet;
3375  kInv.m_afEntry[14] *= fInvDet;
3376  kInv.m_afEntry[15] *= fInvDet;
3377 
3378  return kInv;
3379  }
3380 
3381  moMatrix4<Real> Adjoint () const
3382  {
3383  Real fA0 = m_afEntry[ 0]*m_afEntry[ 5] - m_afEntry[ 1]*m_afEntry[ 4];
3384  Real fA1 = m_afEntry[ 0]*m_afEntry[ 6] - m_afEntry[ 2]*m_afEntry[ 4];
3385  Real fA2 = m_afEntry[ 0]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 4];
3386  Real fA3 = m_afEntry[ 1]*m_afEntry[ 6] - m_afEntry[ 2]*m_afEntry[ 5];
3387  Real fA4 = m_afEntry[ 1]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 5];
3388  Real fA5 = m_afEntry[ 2]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 6];
3389  Real fB0 = m_afEntry[ 8]*m_afEntry[13] - m_afEntry[ 9]*m_afEntry[12];
3390  Real fB1 = m_afEntry[ 8]*m_afEntry[14] - m_afEntry[10]*m_afEntry[12];
3391  Real fB2 = m_afEntry[ 8]*m_afEntry[15] - m_afEntry[11]*m_afEntry[12];
3392  Real fB3 = m_afEntry[ 9]*m_afEntry[14] - m_afEntry[10]*m_afEntry[13];
3393  Real fB4 = m_afEntry[ 9]*m_afEntry[15] - m_afEntry[11]*m_afEntry[13];
3394  Real fB5 = m_afEntry[10]*m_afEntry[15] - m_afEntry[11]*m_afEntry[14];
3395 
3396  return moMatrix4<Real>(
3397  + m_afEntry[ 5]*fB5 - m_afEntry[ 6]*fB4 + m_afEntry[ 7]*fB3,
3398  - m_afEntry[ 1]*fB5 + m_afEntry[ 2]*fB4 - m_afEntry[ 3]*fB3,
3399  + m_afEntry[13]*fA5 - m_afEntry[14]*fA4 + m_afEntry[15]*fA3,
3400  - m_afEntry[ 9]*fA5 + m_afEntry[10]*fA4 - m_afEntry[11]*fA3,
3401  - m_afEntry[ 4]*fB5 + m_afEntry[ 6]*fB2 - m_afEntry[ 7]*fB1,
3402  + m_afEntry[ 0]*fB5 - m_afEntry[ 2]*fB2 + m_afEntry[ 3]*fB1,
3403  - m_afEntry[12]*fA5 + m_afEntry[14]*fA2 - m_afEntry[15]*fA1,
3404  + m_afEntry[ 8]*fA5 - m_afEntry[10]*fA2 + m_afEntry[11]*fA1,
3405  + m_afEntry[ 4]*fB4 - m_afEntry[ 5]*fB2 + m_afEntry[ 7]*fB0,
3406  - m_afEntry[ 0]*fB4 + m_afEntry[ 1]*fB2 - m_afEntry[ 3]*fB0,
3407  + m_afEntry[12]*fA4 - m_afEntry[13]*fA2 + m_afEntry[15]*fA0,
3408  - m_afEntry[ 8]*fA4 + m_afEntry[ 9]*fA2 - m_afEntry[11]*fA0,
3409  - m_afEntry[ 4]*fB3 + m_afEntry[ 5]*fB1 - m_afEntry[ 6]*fB0,
3410  + m_afEntry[ 0]*fB3 - m_afEntry[ 1]*fB1 + m_afEntry[ 2]*fB0,
3411  - m_afEntry[12]*fA3 + m_afEntry[13]*fA1 - m_afEntry[14]*fA0,
3412  + m_afEntry[ 8]*fA3 - m_afEntry[ 9]*fA1 + m_afEntry[10]*fA0);
3413  }
3414 
3415  Real Determinant () const
3416  {
3417  Real fA0 = m_afEntry[ 0]*m_afEntry[ 5] - m_afEntry[ 1]*m_afEntry[ 4];
3418  Real fA1 = m_afEntry[ 0]*m_afEntry[ 6] - m_afEntry[ 2]*m_afEntry[ 4];
3419  Real fA2 = m_afEntry[ 0]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 4];
3420  Real fA3 = m_afEntry[ 1]*m_afEntry[ 6] - m_afEntry[ 2]*m_afEntry[ 5];
3421  Real fA4 = m_afEntry[ 1]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 5];
3422  Real fA5 = m_afEntry[ 2]*m_afEntry[ 7] - m_afEntry[ 3]*m_afEntry[ 6];
3423  Real fB0 = m_afEntry[ 8]*m_afEntry[13] - m_afEntry[ 9]*m_afEntry[12];
3424  Real fB1 = m_afEntry[ 8]*m_afEntry[14] - m_afEntry[10]*m_afEntry[12];
3425  Real fB2 = m_afEntry[ 8]*m_afEntry[15] - m_afEntry[11]*m_afEntry[12];
3426  Real fB3 = m_afEntry[ 9]*m_afEntry[14] - m_afEntry[10]*m_afEntry[13];
3427  Real fB4 = m_afEntry[ 9]*m_afEntry[15] - m_afEntry[11]*m_afEntry[13];
3428  Real fB5 = m_afEntry[10]*m_afEntry[15] - m_afEntry[11]*m_afEntry[14];
3429  Real fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
3430  return fDet;
3431  }
3432 
3433  Real QForm (const moVector4<Real>& rkU,
3434  const moVector4<Real>& rkV) const
3435  {
3436  return rkU.Dot((*this)*rkV);
3437  }
3438 
3439  void MakeObliqueProjection (const moVector3<Real>& rkNormal,
3440  const moVector3<Real>& rkPoint, const moVector3<Real>& rkDirection)
3441  {
3442  // The projection plane is Dot(N,X-P) = 0 where N is a 3-by-1 unit-length
3443  // normal vector and P is a 3-by-1 point on the plane. The projection
3444  // is oblique to the plane, in the direction of the 3-by-1 vector D.
3445  // Necessarily Dot(N,D) is not zero for this projection to make sense.
3446  // Given a 3-by-1 point U, compute the intersection of the line U+t*D
3447  // with the plane to obtain t = -Dot(N,U-P)/Dot(N,D). Then
3448  //
3449  // projection(U) = P + [I - D*N^T/Dot(N,D)]*(U-P)
3450  //
3451  // A 4-by-4 homogeneous transformation representing the projection is
3452  //
3453  // +- -+
3454  // M = | D*N^T - Dot(N,D)*I -Dot(N,P)D |
3455  // | 0^T -Dot(N,D) |
3456  // +- -+
3457  //
3458  // where M applies to [U^T 1]^T by M*[U^T 1]^T. The matrix is chosen so
3459  // that M[3][3] > 0 whenever Dot(N,D) < 0 (projection is onto the
3460  // "positive side" of the plane).
3461 
3462  Real fNdD = rkNormal.Dot(rkDirection);
3463  Real fNdP = rkNormal.Dot(rkPoint);
3464  m_afEntry[ 0] = rkDirection[0]*rkNormal[0] - fNdD;
3465  m_afEntry[ 1] = rkDirection[0]*rkNormal[1];
3466  m_afEntry[ 2] = rkDirection[0]*rkNormal[2];
3467  m_afEntry[ 3] = -fNdP*rkDirection[0];
3468  m_afEntry[ 4] = rkDirection[1]*rkNormal[0];
3469  m_afEntry[ 5] = rkDirection[1]*rkNormal[1] - fNdD;
3470  m_afEntry[ 6] = rkDirection[1]*rkNormal[2];
3471  m_afEntry[ 7] = -fNdP*rkDirection[1];
3472  m_afEntry[ 8] = rkDirection[2]*rkNormal[0];
3473  m_afEntry[ 9] = rkDirection[2]*rkNormal[1];
3474  m_afEntry[10] = rkDirection[2]*rkNormal[2] - fNdD;
3475  m_afEntry[11] = -fNdP*rkDirection[2];
3476  m_afEntry[12] = 0.0f;
3477  m_afEntry[13] = 0.0f;
3478  m_afEntry[14] = 0.0f;
3479  m_afEntry[15] = -fNdD;
3480  }
3481 
3482  void MakePerspectiveProjection (const moVector3<Real>& rkNormal,
3483  const moVector3<Real>& rkPoint, const moVector3<Real>& rkEye)
3484  {
3485  // +- -+
3486  // M = | Dot(N,E-P)*I - E*N^T -(Dot(N,E-P)*I - E*N^T)*E |
3487  // | -N^t Dot(N,E) |
3488  // +- -+
3489  //
3490  // where E is the eye point, P is a point on the plane, and N is a
3491  // unit-length plane normal.
3492 
3493  Real fNdEmP = rkNormal.Dot(rkEye-rkPoint);
3494 
3495  m_afEntry[ 0] = fNdEmP - rkEye[0]*rkNormal[0];
3496  m_afEntry[ 1] = -rkEye[0]*rkNormal[1];
3497  m_afEntry[ 2] = -rkEye[0]*rkNormal[2];
3498  m_afEntry[ 3] = -(m_afEntry[0]*rkEye[0] + m_afEntry[1]*rkEye[1] +
3499  m_afEntry[2]*rkEye[2]);
3500  m_afEntry[ 4] = -rkEye[1]*rkNormal[0];
3501  m_afEntry[ 5] = fNdEmP - rkEye[1]*rkNormal[1];
3502  m_afEntry[ 6] = -rkEye[1]*rkNormal[2];
3503  m_afEntry[ 7] = -(m_afEntry[4]*rkEye[0] + m_afEntry[5]*rkEye[1] +
3504  m_afEntry[6]*rkEye[2]);
3505  m_afEntry[ 8] = -rkEye[2]*rkNormal[0];
3506  m_afEntry[ 9] = -rkEye[2]*rkNormal[1];
3507  m_afEntry[10] = fNdEmP- rkEye[2]*rkNormal[2];
3508  m_afEntry[11] = -(m_afEntry[8]*rkEye[0] + m_afEntry[9]*rkEye[1] +
3509  m_afEntry[10]*rkEye[2]);
3510  m_afEntry[12] = -rkNormal[0];
3511  m_afEntry[13] = -rkNormal[1];
3512  m_afEntry[14] = -rkNormal[2];
3513  m_afEntry[15] = rkNormal.Dot(rkEye);
3514  }
3515 
3516  void MakeReflection (const moVector3<Real>& rkNormal,
3517  const moVector3<Real>& rkPoint)
3518  {
3519  // +- -+
3520  // M = | I-2*N*N^T 2*Dot(N,P)*N |
3521  // | 0^T 1 |
3522  // +- -+
3523  //
3524  // where P is a point on the plane and N is a unit-length plane normal.
3525 
3526  Real fTwoNdP = ((Real)2.0)*(rkNormal.Dot(rkPoint));
3527 
3528  m_afEntry[ 0] = (Real)1.0 - ((Real)2.0)*rkNormal[0]*rkNormal[0];
3529  m_afEntry[ 1] = -((Real)2.0)*rkNormal[0]*rkNormal[1];
3530  m_afEntry[ 2] = -((Real)2.0)*rkNormal[0]*rkNormal[2];
3531  m_afEntry[ 3] = fTwoNdP*rkNormal[0];
3532  m_afEntry[ 4] = -((Real)2.0)*rkNormal[1]*rkNormal[0];
3533  m_afEntry[ 5] = (Real)1.0 - ((Real)2.0)*rkNormal[1]*rkNormal[1];
3534  m_afEntry[ 6] = -((Real)2.0)*rkNormal[1]*rkNormal[2];
3535  m_afEntry[ 7] = fTwoNdP*rkNormal[1];
3536  m_afEntry[ 8] = -((Real)2.0)*rkNormal[2]*rkNormal[0];
3537  m_afEntry[ 9] = -((Real)2.0)*rkNormal[2]*rkNormal[1];
3538  m_afEntry[10] = (Real)1.0 - ((Real)2.0)*rkNormal[2]*rkNormal[2];
3539  m_afEntry[11] = fTwoNdP*rkNormal[2];
3540  m_afEntry[12] = (Real)0.0;
3541  m_afEntry[13] = (Real)0.0;
3542  m_afEntry[14] = (Real)0.0;
3543  m_afEntry[15] = (Real)1.0;
3544 }
3545  // Create a matrix from an array of numbers. The input array is
3546  // interpreted based on the Boolean input as
3547  // true: entry[0..15]={m00,m01,m02,m03,m10,m11,m12,m13,m20,m21,m22,
3548  // m23,m30,m31,m32,m33} [row major]
3549  // false: entry[0..15]={m00,m10,m20,m30,m01,m11,m21,m31,m02,m12,m22,
3550  // m32,m03,m13,m23,m33} [col major]
3551 
3552 
3553  const Real* GetPointer() const { return &m_afEntry[0]; }
3554  Real* GetPointer() { return &m_afEntry[0]; }
3555  // member access
3556  inline operator const Real* () const { return m_afEntry; }
3557  inline operator Real* () { return m_afEntry; }
3558  inline const Real* operator[] (int iRow) const { return &m_afEntry[4*iRow]; }
3559  inline Real* operator[] (int iRow) { return &m_afEntry[4*iRow]; }
3560  inline Real operator() (int iRow, int iCol) const { return m_afEntry[iCol+4*iRow]; }
3561  inline Real& operator() (int iRow, int iCol) { return m_afEntry[iCol+4*iRow]; }
3562 
3563 
3564  // assignment
3565  inline moMatrix4<Real>& operator= (const moMatrix4& rkM)
3566  {
3567  m_afEntry[ 0] = rkM.m_afEntry[ 0];
3568  m_afEntry[ 1] = rkM.m_afEntry[ 1];
3569  m_afEntry[ 2] = rkM.m_afEntry[ 2];
3570  m_afEntry[ 3] = rkM.m_afEntry[ 3];
3571  m_afEntry[ 4] = rkM.m_afEntry[ 4];
3572  m_afEntry[ 5] = rkM.m_afEntry[ 5];
3573  m_afEntry[ 6] = rkM.m_afEntry[ 6];
3574  m_afEntry[ 7] = rkM.m_afEntry[ 7];
3575  m_afEntry[ 8] = rkM.m_afEntry[ 8];
3576  m_afEntry[ 9] = rkM.m_afEntry[ 9];
3577  m_afEntry[10] = rkM.m_afEntry[10];
3578  m_afEntry[11] = rkM.m_afEntry[11];
3579  m_afEntry[12] = rkM.m_afEntry[12];
3580  m_afEntry[13] = rkM.m_afEntry[13];
3581  m_afEntry[14] = rkM.m_afEntry[14];
3582  m_afEntry[15] = rkM.m_afEntry[15];
3583  return *this;
3584  }
3585 
3586  // comparison
3587 
3588  // arithmetic operations
3589  inline moMatrix4<Real> operator+ (const moMatrix4& rkM) const
3590  {
3591  return moMatrix4<Real>(
3592  m_afEntry[ 0] + rkM.m_afEntry[ 0],
3593  m_afEntry[ 1] + rkM.m_afEntry[ 1],
3594  m_afEntry[ 2] + rkM.m_afEntry[ 2],
3595  m_afEntry[ 3] + rkM.m_afEntry[ 3],
3596  m_afEntry[ 4] + rkM.m_afEntry[ 4],
3597  m_afEntry[ 5] + rkM.m_afEntry[ 5],
3598  m_afEntry[ 6] + rkM.m_afEntry[ 6],
3599  m_afEntry[ 7] + rkM.m_afEntry[ 7],
3600  m_afEntry[ 8] + rkM.m_afEntry[ 8],
3601  m_afEntry[ 9] + rkM.m_afEntry[ 9],
3602  m_afEntry[10] + rkM.m_afEntry[10],
3603  m_afEntry[11] + rkM.m_afEntry[11],
3604  m_afEntry[12] + rkM.m_afEntry[12],
3605  m_afEntry[13] + rkM.m_afEntry[13],
3606  m_afEntry[14] + rkM.m_afEntry[14],
3607  m_afEntry[15] + rkM.m_afEntry[15]);
3608  }
3609  inline moMatrix4<Real> operator- (const moMatrix4& rkM) const
3610  {
3611  return moMatrix4<Real>(
3612  m_afEntry[ 0] - rkM.m_afEntry[ 0],
3613  m_afEntry[ 1] - rkM.m_afEntry[ 1],
3614  m_afEntry[ 2] - rkM.m_afEntry[ 2],
3615  m_afEntry[ 3] - rkM.m_afEntry[ 3],
3616  m_afEntry[ 4] - rkM.m_afEntry[ 4],
3617  m_afEntry[ 5] - rkM.m_afEntry[ 5],
3618  m_afEntry[ 6] - rkM.m_afEntry[ 6],
3619  m_afEntry[ 7] - rkM.m_afEntry[ 7],
3620  m_afEntry[ 8] - rkM.m_afEntry[ 8],
3621  m_afEntry[ 9] - rkM.m_afEntry[ 9],
3622  m_afEntry[10] - rkM.m_afEntry[10],
3623  m_afEntry[11] - rkM.m_afEntry[11],
3624  m_afEntry[12] - rkM.m_afEntry[12],
3625  m_afEntry[13] - rkM.m_afEntry[13],
3626  m_afEntry[14] - rkM.m_afEntry[14],
3627  m_afEntry[15] - rkM.m_afEntry[15]);
3628  }
3629  inline moMatrix4<Real> operator* (const moMatrix4& rkM) const
3630  {
3631  return moMatrix4<Real>(
3632  m_afEntry[ 0]*rkM.m_afEntry[ 0] +
3633  m_afEntry[ 1]*rkM.m_afEntry[ 4] +
3634  m_afEntry[ 2]*rkM.m_afEntry[ 8] +
3635  m_afEntry[ 3]*rkM.m_afEntry[12],
3636 
3637  m_afEntry[ 0]*rkM.m_afEntry[ 1] +
3638  m_afEntry[ 1]*rkM.m_afEntry[ 5] +
3639  m_afEntry[ 2]*rkM.m_afEntry[ 9] +
3640  m_afEntry[ 3]*rkM.m_afEntry[13],
3641 
3642  m_afEntry[ 0]*rkM.m_afEntry[ 2] +
3643  m_afEntry[ 1]*rkM.m_afEntry[ 6] +
3644  m_afEntry[ 2]*rkM.m_afEntry[10] +
3645  m_afEntry[ 3]*rkM.m_afEntry[14],
3646 
3647  m_afEntry[ 0]*rkM.m_afEntry[ 3] +
3648  m_afEntry[ 1]*rkM.m_afEntry[ 7] +
3649  m_afEntry[ 2]*rkM.m_afEntry[11] +
3650  m_afEntry[ 3]*rkM.m_afEntry[15],
3651 
3652  m_afEntry[ 4]*rkM.m_afEntry[ 0] +
3653  m_afEntry[ 5]*rkM.m_afEntry[ 4] +
3654  m_afEntry[ 6]*rkM.m_afEntry[ 8] +
3655  m_afEntry[ 7]*rkM.m_afEntry[12],
3656 
3657  m_afEntry[ 4]*rkM.m_afEntry[ 1] +
3658  m_afEntry[ 5]*rkM.m_afEntry[ 5] +
3659  m_afEntry[ 6]*rkM.m_afEntry[ 9] +
3660  m_afEntry[ 7]*rkM.m_afEntry[13],
3661 
3662  m_afEntry[ 4]*rkM.m_afEntry[ 2] +
3663  m_afEntry[ 5]*rkM.m_afEntry[ 6] +
3664  m_afEntry[ 6]*rkM.m_afEntry[10] +
3665  m_afEntry[ 7]*rkM.m_afEntry[14],
3666 
3667  m_afEntry[ 4]*rkM.m_afEntry[ 3] +
3668  m_afEntry[ 5]*rkM.m_afEntry[ 7] +
3669  m_afEntry[ 6]*rkM.m_afEntry[11] +
3670  m_afEntry[ 7]*rkM.m_afEntry[15],
3671 
3672  m_afEntry[ 8]*rkM.m_afEntry[ 0] +
3673  m_afEntry[ 9]*rkM.m_afEntry[ 4] +
3674  m_afEntry[10]*rkM.m_afEntry[ 8] +
3675  m_afEntry[11]*rkM.m_afEntry[12],
3676 
3677  m_afEntry[ 8]*rkM.m_afEntry[ 1] +
3678  m_afEntry[ 9]*rkM.m_afEntry[ 5] +
3679  m_afEntry[10]*rkM.m_afEntry[ 9] +
3680  m_afEntry[11]*rkM.m_afEntry[13],
3681 
3682  m_afEntry[ 8]*rkM.m_afEntry[ 2] +
3683  m_afEntry[ 9]*rkM.m_afEntry[ 6] +
3684  m_afEntry[10]*rkM.m_afEntry[10] +
3685  m_afEntry[11]*rkM.m_afEntry[14],
3686 
3687  m_afEntry[ 8]*rkM.m_afEntry[ 3] +
3688  m_afEntry[ 9]*rkM.m_afEntry[ 7] +
3689  m_afEntry[10]*rkM.m_afEntry[11] +
3690  m_afEntry[11]*rkM.m_afEntry[15],
3691 
3692  m_afEntry[12]*rkM.m_afEntry[ 0] +
3693  m_afEntry[13]*rkM.m_afEntry[ 4] +
3694  m_afEntry[14]*rkM.m_afEntry[ 8] +
3695  m_afEntry[15]*rkM.m_afEntry[12],
3696 
3697  m_afEntry[12]*rkM.m_afEntry[ 1] +
3698  m_afEntry[13]*rkM.m_afEntry[ 5] +
3699  m_afEntry[14]*rkM.m_afEntry[ 9] +
3700  m_afEntry[15]*rkM.m_afEntry[13],
3701 
3702  m_afEntry[12]*rkM.m_afEntry[ 2] +
3703  m_afEntry[13]*rkM.m_afEntry[ 6] +
3704  m_afEntry[14]*rkM.m_afEntry[10] +
3705  m_afEntry[15]*rkM.m_afEntry[14],
3706 
3707  m_afEntry[12]*rkM.m_afEntry[ 3] +
3708  m_afEntry[13]*rkM.m_afEntry[ 7] +
3709  m_afEntry[14]*rkM.m_afEntry[11] +
3710  m_afEntry[15]*rkM.m_afEntry[15]);
3711  }
3712  inline moMatrix4<Real> operator* (Real fScalar) const
3713  {
3714  return moMatrix4<Real>(
3715  fScalar*m_afEntry[ 0],
3716  fScalar*m_afEntry[ 1],
3717  fScalar*m_afEntry[ 2],
3718  fScalar*m_afEntry[ 3],
3719  fScalar*m_afEntry[ 4],
3720  fScalar*m_afEntry[ 5],
3721  fScalar*m_afEntry[ 6],
3722  fScalar*m_afEntry[ 7],
3723  fScalar*m_afEntry[ 8],
3724  fScalar*m_afEntry[ 9],
3725  fScalar*m_afEntry[10],
3726  fScalar*m_afEntry[11],
3727  fScalar*m_afEntry[12],
3728  fScalar*m_afEntry[13],
3729  fScalar*m_afEntry[14],
3730  fScalar*m_afEntry[15]);
3731  }
3732  inline moMatrix4<Real> operator/ (Real fScalar) const
3733  {
3734  if (fScalar != (Real)0.0)
3735  {
3736  Real fInvScalar = ((Real)1.0)/fScalar;
3737  return moMatrix4<Real>(
3738  fInvScalar*m_afEntry[ 0],
3739  fInvScalar*m_afEntry[ 1],
3740  fInvScalar*m_afEntry[ 2],
3741  fInvScalar*m_afEntry[ 3],
3742  fInvScalar*m_afEntry[ 4],
3743  fInvScalar*m_afEntry[ 5],
3744  fInvScalar*m_afEntry[ 6],
3745  fInvScalar*m_afEntry[ 7],
3746  fInvScalar*m_afEntry[ 8],
3747  fInvScalar*m_afEntry[ 9],
3748  fInvScalar*m_afEntry[10],
3749  fInvScalar*m_afEntry[11],
3750  fInvScalar*m_afEntry[12],
3751  fInvScalar*m_afEntry[13],
3752  fInvScalar*m_afEntry[14],
3753  fInvScalar*m_afEntry[15]);
3754  }
3755 
3756  return moMatrix4<Real>(
3757  moMath<Real>::MAX_REAL,
3758  moMath<Real>::MAX_REAL,
3759  moMath<Real>::MAX_REAL,
3760  moMath<Real>::MAX_REAL,
3761  moMath<Real>::MAX_REAL,
3762  moMath<Real>::MAX_REAL,
3763  moMath<Real>::MAX_REAL,
3764  moMath<Real>::MAX_REAL,
3765  moMath<Real>::MAX_REAL,
3766  moMath<Real>::MAX_REAL,
3767  moMath<Real>::MAX_REAL,
3768  moMath<Real>::MAX_REAL,
3769  moMath<Real>::MAX_REAL,
3770  moMath<Real>::MAX_REAL,
3771  moMath<Real>::MAX_REAL,
3772  moMath<Real>::MAX_REAL);
3773  }
3774  inline moMatrix4<Real> operator- () const
3775  {
3776  return moMatrix4<Real>(
3777  -m_afEntry[ 0],
3778  -m_afEntry[ 1],
3779  -m_afEntry[ 2],
3780  -m_afEntry[ 3],
3781  -m_afEntry[ 4],
3782  -m_afEntry[ 5],
3783  -m_afEntry[ 6],
3784  -m_afEntry[ 7],
3785  -m_afEntry[ 8],
3786  -m_afEntry[ 9],
3787  -m_afEntry[10],
3788  -m_afEntry[11],
3789  -m_afEntry[12],
3790  -m_afEntry[13],
3791  -m_afEntry[14],
3792  -m_afEntry[15]);
3793  }
3794 
3795  // arithmetic updates
3796  inline moMatrix4<Real>& operator+= (const moMatrix4& rkM)
3797  {
3798  m_afEntry[ 0] += rkM.m_afEntry[ 0];
3799  m_afEntry[ 1] += rkM.m_afEntry[ 1];
3800  m_afEntry[ 2] += rkM.m_afEntry[ 2];
3801  m_afEntry[ 3] += rkM.m_afEntry[ 3];
3802  m_afEntry[ 4] += rkM.m_afEntry[ 4];
3803  m_afEntry[ 5] += rkM.m_afEntry[ 5];
3804  m_afEntry[ 6] += rkM.m_afEntry[ 6];
3805  m_afEntry[ 7] += rkM.m_afEntry[ 7];
3806  m_afEntry[ 8] += rkM.m_afEntry[ 8];
3807  m_afEntry[ 9] += rkM.m_afEntry[ 9];
3808  m_afEntry[10] += rkM.m_afEntry[10];
3809  m_afEntry[11] += rkM.m_afEntry[11];
3810  m_afEntry[12] += rkM.m_afEntry[12];
3811  m_afEntry[13] += rkM.m_afEntry[13];
3812  m_afEntry[14] += rkM.m_afEntry[14];
3813  m_afEntry[15] += rkM.m_afEntry[15];
3814  return *this;
3815  }
3816  inline moMatrix4<Real>& operator-= (const moMatrix4& rkM)
3817  {
3818  m_afEntry[ 0] -= rkM.m_afEntry[ 0];
3819  m_afEntry[ 1] -= rkM.m_afEntry[ 1];
3820  m_afEntry[ 2] -= rkM.m_afEntry[ 2];
3821  m_afEntry[ 3] -= rkM.m_afEntry[ 3];
3822  m_afEntry[ 4] -= rkM.m_afEntry[ 4];
3823  m_afEntry[ 5] -= rkM.m_afEntry[ 5];
3824  m_afEntry[ 6] -= rkM.m_afEntry[ 6];
3825  m_afEntry[ 7] -= rkM.m_afEntry[ 7];
3826  m_afEntry[ 8] -= rkM.m_afEntry[ 8];
3827  m_afEntry[ 9] -= rkM.m_afEntry[ 9];
3828  m_afEntry[10] -= rkM.m_afEntry[10];
3829  m_afEntry[11] -= rkM.m_afEntry[11];
3830  m_afEntry[12] -= rkM.m_afEntry[12];
3831  m_afEntry[13] -= rkM.m_afEntry[13];
3832  m_afEntry[14] -= rkM.m_afEntry[14];
3833  m_afEntry[15] -= rkM.m_afEntry[15];
3834  return *this;
3835  }
3836  inline moMatrix4<Real>& operator*= (Real fScalar)
3837  {
3838  m_afEntry[ 0] *= fScalar;
3839  m_afEntry[ 1] *= fScalar;
3840  m_afEntry[ 2] *= fScalar;
3841  m_afEntry[ 3] *= fScalar;
3842  m_afEntry[ 4] *= fScalar;
3843  m_afEntry[ 5] *= fScalar;
3844  m_afEntry[ 6] *= fScalar;
3845  m_afEntry[ 7] *= fScalar;
3846  m_afEntry[ 8] *= fScalar;
3847  m_afEntry[ 9] *= fScalar;
3848  m_afEntry[10] *= fScalar;
3849  m_afEntry[11] *= fScalar;
3850  m_afEntry[12] *= fScalar;
3851  m_afEntry[13] *= fScalar;
3852  m_afEntry[14] *= fScalar;
3853  m_afEntry[15] *= fScalar;
3854  return *this;
3855  }
3856  inline moMatrix4<Real>& operator/= (Real fScalar)
3857  {
3858  if (fScalar != (Real)0.0)
3859  {
3860  Real fInvScalar = ((Real)1.0)/fScalar;
3861  m_afEntry[ 0] *= fInvScalar;
3862  m_afEntry[ 1] *= fInvScalar;
3863  m_afEntry[ 2] *= fInvScalar;
3864  m_afEntry[ 3] *= fInvScalar;
3865  m_afEntry[ 4] *= fInvScalar;
3866  m_afEntry[ 5] *= fInvScalar;
3867  m_afEntry[ 6] *= fInvScalar;
3868  m_afEntry[ 7] *= fInvScalar;
3869  m_afEntry[ 8] *= fInvScalar;
3870  m_afEntry[ 9] *= fInvScalar;
3871  m_afEntry[10] *= fInvScalar;
3872  m_afEntry[11] *= fInvScalar;
3873  m_afEntry[12] *= fInvScalar;
3874  m_afEntry[13] *= fInvScalar;
3875  m_afEntry[14] *= fInvScalar;
3876  m_afEntry[15] *= fInvScalar;
3877  }
3878  else
3879  {
3880  m_afEntry[ 0] = moMath<Real>::MAX_REAL;
3881  m_afEntry[ 1] = moMath<Real>::MAX_REAL;
3882  m_afEntry[ 2] = moMath<Real>::MAX_REAL;
3883  m_afEntry[ 3] = moMath<Real>::MAX_REAL;
3884  m_afEntry[ 4] = moMath<Real>::MAX_REAL;
3885  m_afEntry[ 5] = moMath<Real>::MAX_REAL;
3886  m_afEntry[ 6] = moMath<Real>::MAX_REAL;
3887  m_afEntry[ 7] = moMath<Real>::MAX_REAL;
3888  m_afEntry[ 8] = moMath<Real>::MAX_REAL;
3889  m_afEntry[ 9] = moMath<Real>::MAX_REAL;
3890  m_afEntry[10] = moMath<Real>::MAX_REAL;
3891  m_afEntry[11] = moMath<Real>::MAX_REAL;
3892  m_afEntry[12] = moMath<Real>::MAX_REAL;
3893  m_afEntry[13] = moMath<Real>::MAX_REAL;
3894  m_afEntry[14] = moMath<Real>::MAX_REAL;
3895  m_afEntry[15] = moMath<Real>::MAX_REAL;
3896  }
3897 
3898  return *this;
3899  }
3900 
3901  // matrix times vector
3902  inline moVector4<Real> operator* (const moVector4<Real>& rkV) const // M * v
3903  {
3904  return moVector4<Real>(
3905  m_afEntry[ 0]*rkV[0] +
3906  m_afEntry[ 1]*rkV[1] +
3907  m_afEntry[ 2]*rkV[2] +
3908  m_afEntry[ 3]*rkV[3],
3909 
3910  m_afEntry[ 4]*rkV[0] +
3911  m_afEntry[ 5]*rkV[1] +
3912  m_afEntry[ 6]*rkV[2] +
3913  m_afEntry[ 7]*rkV[3],
3914 
3915  m_afEntry[ 8]*rkV[0] +
3916  m_afEntry[ 9]*rkV[1] +
3917  m_afEntry[10]*rkV[2] +
3918  m_afEntry[11]*rkV[3],
3919 
3920  m_afEntry[12]*rkV[0] +
3921  m_afEntry[13]*rkV[1] +
3922  m_afEntry[14]*rkV[2] +
3923  m_afEntry[15]*rkV[3]);
3924  }
3925 
3926  // special matrices
3927  static const moMatrix4 ZERO;
3928  static const moMatrix4 IDENTITY;
3929 
3930 private:
3931  // support for comparisons
3932  int CompareArrays (const moMatrix4& rkM) const
3933  {
3934  return memcmp(m_afEntry,rkM.m_afEntry,16*sizeof(Real));
3935  }
3936 
3937 
3938  Real m_afEntry[16];
3939 };
3940 
3941 // c * M
3942 template <class Real>
3943 inline moMatrix4<Real> operator* (Real fScalar, const moMatrix4<Real>& rkM)
3944 {
3945  return rkM*fScalar;
3946 }
3947 
3948 // v^T * M
3949 template <class Real>
3950 inline moVector4<Real> operator* (const moVector4<Real>& rkV,
3951  const moMatrix4<Real>& rkM)
3952 {
3953  return moVector4<Real>(
3954  rkV[0]*rkM[0][0]+rkV[1]*rkM[1][0]+rkV[2]*rkM[2][0]+rkV[3]*rkM[3][0],
3955  rkV[0]*rkM[0][1]+rkV[1]*rkM[1][1]+rkV[2]*rkM[2][1]+rkV[3]*rkM[3][1],
3956  rkV[0]*rkM[0][2]+rkV[1]*rkM[1][2]+rkV[2]*rkM[2][2]+rkV[3]*rkM[3][2],
3957  rkV[0]*rkM[0][3]+rkV[1]*rkM[1][3]+rkV[2]*rkM[2][3]+rkV[3]*rkM[3][3]);
3958 }
3959 
3960 #ifndef MO_MACOSX
3961 #ifndef MO_WIN32
3962 #ifndef MO_RASPBIAN
3963 template class LIBMOLDEO_API moMatrix4<MOfloat>;
3964 #endif
3965 #endif
3966 #endif
3967 typedef moMatrix4<MOfloat> moMatrix4f;
3968 
3969 #ifndef MO_MACOSX
3970 #ifndef MO_WIN32
3971 #ifndef MO_RASPBIAN
3972 template class LIBMOLDEO_API moMatrix4<MOdouble>;
3973 #endif
3974 #endif
3975 #endif
3976 typedef moMatrix4<MOdouble> moMatrix4d;
3977 
3978 moDeclareExportedDynamicArray( moMatrix4f, moMatrix4fArray );
3979 moDeclareExportedDynamicArray( moMatrix4d, moMatrix4dArray );
3980 
3981 
3982 #endif
3983 
FromAxisAngle
moMatrix3< Real > & FromAxisAngle(const moVector3< Real > &rkAxis, Real fAngle)
Definition: moMathMatrix.h:849
moMatrix3< MOfloat >
template class LIBMOLDEO_API moMatrix3< MOfloat >
Definition: moMathMatrix.h:2865
operator<=
bool operator<=(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:960
FromEulerAnglesYXZ
moMatrix3< Real > & FromEulerAnglesYXZ(Real fYAngle, Real fXAngle, Real fZAngle)
Definition: moMathMatrix.h:1472
moMatrix2::moMatrix2
moMatrix2(const moVector2< Real > *akV, bool bColumns)
Definition: moMathMatrix.h:157
FromEulerAnglesZYX
moMatrix3< Real > & FromEulerAnglesZYX(Real fZAngle, Real fYAngle, Real fXAngle)
Definition: moMathMatrix.h:1565
moMatrix2::FromAngle
void FromAngle(Real fAngle)
Definition: moMathMatrix.h:219
moMatrix2::Orthonormalize
void Orthonormalize()
Definition: moMathMatrix.h:397
moMatrix2::QForm
Real QForm(const moVector2< Real > &rkU, const moVector2< Real > &rkV) const
Definition: moMathMatrix.h:384
moMatrix4< MOfloat >
template class LIBMOLDEO_API moMatrix4< MOfloat >
Definition: moMathMatrix.h:3963
moDeclareExportedDynamicArray
moDeclareExportedDynamicArray(moMatrix2f, moMatrix2fArray)
moMatrix2::IDENTITY
static const moMatrix2 IDENTITY
Definition: moMathMatrix.h:596
QDUDecomposition
void QDUDecomposition(moMatrix3 &rkQ, moMatrix3 &rkD, moMatrix3 &rkU) const
Definition: moMathMatrix.h:1987
operator-
moMatrix3 operator-(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:2132
moMatrix2::MakeDiagonal
void MakeDiagonal(Real fM00, Real fM11)
Definition: moMathMatrix.h:210
moMath::ACos
static Real ACos(Real fValue)
Definition: moMath.h:81
ToEulerAnglesZXY
bool ToEulerAnglesZXY(Real &rfZAngle, Real &rfXAngle, Real &rfYAngle) const
Definition: moMathMatrix.h:1740
SingularValueComposition
void SingularValueComposition(const moMatrix3 &rkL, const moMatrix3 &rkD, const moMatrix3 &rkRTranspose)
Definition: moMathMatrix.h:1956
operator/
moMatrix3 operator/(Real fScalar) const
Definition: moMathMatrix.h:2197
operator>
bool operator>(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:966
moMath::ATan2
static Real ATan2(Real fY, Real fX)
Definition: moMath.h:140
ToAxisAngle
void ToAxisAngle(moVector3< Real > &rkAxis, Real &rfAngle) const
Definition: moMathMatrix.h:1178
ToEulerAnglesYXZ
bool ToEulerAnglesYXZ(Real &rfYAngle, Real &rfXAngle, Real &rfZAngle) const
Definition: moMathMatrix.h:1668
QForm
Real QForm(const moVector3< Real > &rkU, const moVector3< Real > &rkV) const
Definition: moMathMatrix.h:1153
moMatrix2::moMatrix2
moMatrix2(const moMatrix2 &rkM)
Definition: moMathMatrix.h:103
Inverse
moMatrix3< Real > Inverse() const
Definition: moMathMatrix.h:1077
moMatrix2::GetColumn
moVector2< Real > GetColumn(int iCol) const
Definition: moMathMatrix.h:260
moMath::Sin
static Real Sin(Real fValue)
Definition: moMath.h:260
moMatrix3f
moMatrix3< MOfloat > moMatrix3f
Definition: moMathMatrix.h:2869
moAbstract
Clase base abstracta de donde deben derivar los objetos [virtual pura].
Definition: moAbstract.h:191
moMatrix2f
moMatrix2< MOfloat > moMatrix2f
Definition: moMathMatrix.h:632
ToEulerAnglesXYZ
bool ToEulerAnglesXYZ(Real &rfXAngle, Real &rfYAngle, Real &rfZAngle) const
Definition: moMathMatrix.h:1596
LIBMOLDEO_API
#define LIBMOLDEO_API
Definition: moTypes.h:180
moMatrix2::ToAngle
void ToAngle(Real &rfAngle) const
Definition: moMathMatrix.h:390
SetRow
void SetRow(int iRow, const moVector3< Real > &rkV)
Definition: moMathMatrix.h:895
moMatrix2::GetRow
moVector2< Real > GetRow(int iRow) const
Definition: moMathMatrix.h:246
moMatrix2::EigenDecomposition
void EigenDecomposition(moMatrix2 &rkRot, moMatrix2 &rkDiag) const
Definition: moMathMatrix.h:428
moMatrix4< MOdouble >
template class LIBMOLDEO_API moMatrix4< MOdouble >
Definition: moMathMatrix.h:3972
moMatrix2::Determinant
Real Determinant() const
Definition: moMathMatrix.h:378
moMatrix2::Transpose
moMatrix2< Real > Transpose() const
Definition: moMathMatrix.h:313
GetColumnMajor
void GetColumnMajor(Real *afCMajor) const
Definition: moMathMatrix.h:925
operator*
moMatrix2< Real > operator*(Real fScalar, const moMatrix2< Real > &rkM)
Definition: moMathMatrix.h:612
operator-=
moMatrix3 & operator-=(const moMatrix3 &rkM)
Definition: moMathMatrix.h:2253
moMatrix2::moMatrix2
moMatrix2(const Real afEntry[4], bool bRowMajor)
Definition: moMathMatrix.h:120
moMath::InvSqrt
static Real InvSqrt(Real fValue)
Definition: moMath.h:210
FromEulerAnglesXZY
moMatrix3< Real > & FromEulerAnglesXZY(Real fXAngle, Real fZAngle, Real fYAngle)
Definition: moMathMatrix.h:1441
operator==
bool operator==(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:942
SetColumn
void SetColumn(int iCol, const moVector3< Real > &rkV)
Definition: moMathMatrix.h:911
m_afEntry
Real m_afEntry[9]
Definition: moMathMatrix.h:2841
GetRow
moVector3< Real > GetRow(int iRow) const
Definition: moMathMatrix.h:904
ToEulerAnglesXZY
bool ToEulerAnglesXZY(Real &rfXAngle, Real &rfZAngle, Real &rfYAngle) const
Definition: moMathMatrix.h:1632
moMath::Sqrt
static Real Sqrt(Real fValue)
Definition: moMath.h:279
MakeDiagonal
moMatrix3< Real > & MakeDiagonal(Real fM00, Real fM11, Real fM22)
Definition: moMathMatrix.h:834
Determinant
Real Determinant() const
Definition: moMathMatrix.h:1143
operator<
bool operator<(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:954
DiagonalTimes
moMatrix3< Real > DiagonalTimes(const moVector3< Real > &rkDiag) const
Definition: moMathMatrix.h:1169
moMatrix2::TimesTranspose
moMatrix2< Real > TimesTranspose(const moMatrix2 &rkM) const
Definition: moMathMatrix.h:333
ToEulerAnglesYZX
bool ToEulerAnglesYZX(Real &rfYAngle, Real &rfZAngle, Real &rfXAngle) const
Definition: moMathMatrix.h:1704
moVector3::Dot
Real Dot(const moVector3 &rkV) const
Definition: moMathVector3.h:198
moMatrix2::TransposeTimes
moMatrix2< Real > TransposeTimes(const moMatrix2 &rkM) const
Definition: moMathMatrix.h:323
ToEulerAnglesZYX
bool ToEulerAnglesZYX(Real &rfZAngle, Real &rfYAngle, Real &rfXAngle) const
Definition: moMathMatrix.h:1776
Slerp
moMatrix3< Real > & Slerp(Real fT, const moMatrix3 &rkR0, const moMatrix3 &rkR1)
Definition: moMathMatrix.h:1812
moMatrix3< MOdouble >
template class LIBMOLDEO_API moMatrix3< MOdouble >
Definition: moMathMatrix.h:2874
moMatrix2::moMatrix2
moMatrix2(bool bZero=true)
Definition: moMathMatrix.h:91
TimesDiagonal
moMatrix3< Real > TimesDiagonal(const moVector3< Real > &rkDiag) const
Definition: moMathMatrix.h:1160
GetColumn
moVector3< Real > GetColumn(int iCol) const
Definition: moMathMatrix.h:919
moVector3::Normalize
Real Normalize()
Definition: moMathVector3.h:205
Orthonormalize
void Orthonormalize()
Definition: moMathMatrix.h:1276
FromEulerAnglesZXY
moMatrix3< Real > & FromEulerAnglesZXY(Real fZAngle, Real fXAngle, Real fYAngle)
Definition: moMathMatrix.h:1534
operator/=
moMatrix3 & operator/=(Real fScalar)
Definition: moMathMatrix.h:2279
moMatrix2::moMatrix2
moMatrix2(Real fM00, Real fM01, Real fM10, Real fM11)
Definition: moMathMatrix.h:112
MakeZero
moMatrix3< Real > & MakeZero()
Definition: moMathMatrix.h:804
Adjoint
moMatrix3< Real > Adjoint() const
Definition: moMathMatrix.h:1128
FromEulerAnglesXYZ
moMatrix3< Real > & FromEulerAnglesXYZ(Real fXAngle, Real fYAngle, Real fZAngle)
Definition: moMathMatrix.h:1410
moMatrix2::SetColumn
void SetColumn(int iCol, const moVector2< Real > &rkV)
Definition: moMathMatrix.h:253
moMatrix4f
moMatrix4< MOfloat > moMatrix4f
Definition: moMathMatrix.h:3967
moMath::Cos
static Real Cos(Real fValue)
Definition: moMath.h:160
moVector2::Dot
Real Dot(const moVector2 &rkV) const
Definition: moMathVector.h:176
TimesTranspose
moMatrix3< Real > TimesTranspose(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:1035
EigenDecomposition
void EigenDecomposition(moMatrix3 &rkRot, moMatrix3 &rkDiag) const
Definition: moMathMatrix.h:1331
moMatrix2::MakeZero
void MakeZero()
Definition: moMathMatrix.h:192
operator!=
bool operator!=(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:948
moMatrix2::moMatrix2
moMatrix2(Real fM00, Real fM11)
Definition: moMathMatrix.h:175
moMatrix2::GetColumnMajor
void GetColumnMajor(Real *afCMajor) const
Definition: moMathMatrix.h:266
operator*=
moMatrix3 & operator*=(Real fScalar)
Definition: moMathMatrix.h:2266
moMatrix3d
moMatrix3< MOdouble > moMatrix3d
Definition: moMathMatrix.h:2878
moMatrix3
moMatrix3(const moMatrix3 &rkM)
Definition: moMathMatrix.h:669
operator>=
bool operator>=(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:972
moMatrix2::Inverse
moMatrix2< Real > Inverse() const
Definition: moMathMatrix.h:343
operator=
moMatrix3 & operator=(const moMatrix3 &rkM)
Definition: moMathMatrix.h:2102
moMatrix2d
moMatrix2< MOdouble > moMatrix2d
Definition: moMathMatrix.h:639
moMatrix2::moMatrix2
moMatrix2(const moVector2< Real > &rkU, const moVector2< Real > &rkV)
Definition: moMathMatrix.h:186
moMatrix4d
moMatrix4< MOdouble > moMatrix4d
Definition: moMathMatrix.h:3976
moMath
Definition: moMath.h:64
moMatrix2::ZERO
static const moMatrix2 ZERO
Definition: moMathMatrix.h:595
Transpose
moMatrix3< Real > Transpose() const
Definition: moMathMatrix.h:978
moVector4::Dot
Real Dot(const moVector4 &rkV) const
Definition: moMathVector4.h:221
operator()
Real operator()(int iRow, int iCol) const
Definition: moMathMatrix.h:2097
MakeTensorProduct
moMatrix3< Real > & MakeTensorProduct(const moVector3< Real > &rkU, const moVector3< Real > &rkV)
Definition: moMathMatrix.h:879
PolarDecomposition
void PolarDecomposition(moMatrix3 &rkQ, moMatrix3 &rkS)
Definition: moMathMatrix.h:1963
moAbstract::m_bInitialized
MOboolean m_bInitialized
Valor de inicialización
Definition: moAbstract.h:223
MakeIdentity
moMatrix3< Real > & MakeIdentity()
Definition: moMathMatrix.h:819
operator+
moMatrix3 operator+(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:2119
moMath::FAbs
static Real FAbs(Real fValue)
Definition: moMath.h:180
moMatrix2::moMatrix2
moMatrix2(const moVector2< Real > &rkU, const moVector2< Real > &rkV, bool bColumns)
Definition: moMathMatrix.h:138
FromEulerAnglesYZX
moMatrix3< Real > & FromEulerAnglesYZX(Real fYAngle, Real fZAngle, Real fXAngle)
Definition: moMathMatrix.h:1503
operator+=
moMatrix3 & operator+=(const moMatrix3 &rkM)
Definition: moMathMatrix.h:2240
moMatrix2::MakeIdentity
void MakeIdentity()
Definition: moMathMatrix.h:201
SingularValueDecomposition
void SingularValueDecomposition(moMatrix3 &rkL, moMatrix3 &rkD, moMatrix3 &rkRTranspose) const
Definition: moMathMatrix.h:1825
moMatrix2::MakeTensorProduct
void MakeTensorProduct(const moVector2< Real > &rkU, const moVector2< Real > &rkV)
Definition: moMathMatrix.h:228
moMatrix2::SetRow
void SetRow(int iRow, const moVector2< Real > &rkV)
Definition: moMathMatrix.h:238
TransposeTimes
moMatrix3< Real > TransposeTimes(const moMatrix3 &rkM) const
Definition: moMathMatrix.h:993
operator[]
const Real * operator[](int iRow) const
Definition: moMathMatrix.h:2095
moMatrix2::moMatrix2
moMatrix2(Real fAngle)
Definition: moMathMatrix.h:180
moMatrix2::Adjoint
moMatrix2< Real > Adjoint() const
Definition: moMathMatrix.h:369