All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
RotationMatrix.h
Go to the documentation of this file.
1 //=================================================================================================
20 //=================================================================================================
21 
22 #ifndef _BLAZE_MATH_ROTATIONMATRIX_H_
23 #define _BLAZE_MATH_ROTATIONMATRIX_H_
24 
25 
26 //*************************************************************************************************
27 // Includes
28 //*************************************************************************************************
29 
30 #include <algorithm>
31 #include <cmath>
32 #include <ostream>
33 #include <limits>
34 #include <blaze/math/Accuracy.h>
35 #include <blaze/math/DenseMatrix.h>
36 #include <blaze/math/Forward.h>
37 #include <blaze/math/shims/Equal.h>
39 #include <blaze/math/shims/IsNaN.h>
44 #include <blaze/system/Precision.h>
45 #include <blaze/util/Assert.h>
49 #include <blaze/util/Types.h>
50 
51 
52 namespace blaze {
53 
54 //=================================================================================================
55 //
56 // EULER ROTATIONS
57 //
58 //=================================================================================================
59 
60 //*************************************************************************************************
68  XYZs = 0,
69  ZYXr = 1,
70  XYXs = 2,
71  XYXr = 3,
72  XZYs = 4,
73  YZXr = 5,
74  XZXs = 6,
75  XZXr = 7,
76  YZXs = 8,
77  XZYr = 9,
78  YZYs = 10,
79  YZYr = 11,
80  YXZs = 12,
81  ZXYr = 13,
82  YXYs = 14,
83  YXYr = 15,
84  ZXYs = 16,
85  YXZr = 17,
86  ZXZs = 18,
87  ZXZr = 19,
88  ZYXs = 20,
89  XYZr = 21,
90  ZYZs = 22,
91  ZYZr = 23
92 };
93 //*************************************************************************************************
94 
95 
96 
97 
98 //=================================================================================================
99 //
100 // CLASS DEFINITION
101 //
102 //=================================================================================================
103 
104 //*************************************************************************************************
140 template< typename Type > // Data type of the rotation matrix
141 class RotationMatrix : public DenseMatrix< RotationMatrix<Type>, false >
142 {
143  public:
144  //**Type definitions****************************************************************************
146  typedef This ResultType;
147  typedef Type ElementType;
148  typedef const RotationMatrix& CompositeType;
149  //**********************************************************************************************
150 
151  //**Constructors********************************************************************************
154  explicit inline RotationMatrix();
155 
156  template< typename Axis >
157  explicit RotationMatrix( StaticVector<Axis,3UL> axis, Type angle );
158 
159  inline RotationMatrix( const RotationMatrix& m );
160 
161  template< typename Other >
162  inline RotationMatrix( const RotationMatrix<Other>& m );
164  //**********************************************************************************************
165 
166  //**Destructor**********************************************************************************
167  // No explicitly declared destructor.
168  //**********************************************************************************************
169 
170  //**Operators***********************************************************************************
173  inline RotationMatrix& operator= ( const RotationMatrix& rhs );
174  template< typename Other > inline RotationMatrix& operator= ( const RotationMatrix<Other>& rhs );
175  inline Type operator[]( size_t index ) const;
176  inline Type operator()( size_t i, size_t j ) const;
177  template< typename Other > inline RotationMatrix& operator*=( const RotationMatrix<Other>& rhs );
179  //**********************************************************************************************
180 
181  //**Utility functions***************************************************************************
184  inline size_t rows() const;
185  inline size_t columns() const;
186  inline void reset();
187  inline Type getDeterminant() const;
188  inline RotationMatrix& transpose();
189  inline RotationMatrix& invert();
190  inline void swap( RotationMatrix& m ) /* throw() */;
192  //**********************************************************************************************
193 
194  //**Expression template evaluation functions****************************************************
197  template< typename Other > inline bool isAliased( const Other* alias ) const;
199  //**********************************************************************************************
200 
201  //**Math functions******************************************************************************
204  template< typename Other >
205  inline const StaticMatrix< typename MultTrait<Type,Other>::Type, 3UL, 3UL, false >
206  rotate( const StaticMatrix<Other,3UL,3UL,false>& m ) const;
207 
208  template< typename Other >
209  inline const StaticMatrix< typename MultTrait<Type,Other>::Type, 3UL, 3UL, false >
212  //**********************************************************************************************
213 
214  //**Euler rotations*****************************************************************************
235  inline const StaticVector<Type,3UL> getEulerAnglesXYZ() const;
238  //**********************************************************************************************
239 
240  private:
241  //**Constructors********************************************************************************
244  explicit inline RotationMatrix( Type xx, Type xy, Type xz,
245  Type yx, Type yy, Type yz,
246  Type zx, Type zy, Type zz );
248  //**********************************************************************************************
249 
250  //**Member variables****************************************************************************
253  Type v_[9];
254 
262  //**********************************************************************************************
263 
264  //**Friend declarations*************************************************************************
266  template< typename Other > friend class Quaternion;
267 
268  template< typename Other >
269  friend const RotationMatrix<Other> trans( const RotationMatrix<Other>& m );
270 
271  template< typename Other >
272  friend const RotationMatrix<Other> inv( const RotationMatrix<Other>& m );
273 
274  template< typename T1, typename T2 >
276  operator*( const RotationMatrix<T1>& lhs, const RotationMatrix<T2>& rhs );
278  //**********************************************************************************************
279 
280  //**Compile time checks*************************************************************************
286  //**********************************************************************************************
287 };
288 //*************************************************************************************************
289 
290 
291 
292 
293 //=================================================================================================
294 //
295 // CONSTRUCTORS
296 //
297 //=================================================================================================
298 
299 //*************************************************************************************************
305 template< typename Type > // Data type of the rotation matrix
307 {
308  v_[0] = v_[4] = v_[8] = Type(1);
309  v_[1] = v_[2] = v_[3] = v_[5] = v_[6] = v_[7] = Type(0);
310 }
311 //*************************************************************************************************
312 
313 
314 //*************************************************************************************************
331 template< typename Type > // Data type of the rotation matrix
332 template< typename Axis > // Data type of the rotation axis
334 {
336 
337  BLAZE_USER_ASSERT( ( axis.sqrLength() > Axis(0) || angle == Type(0) ), "Invalid matrix parameters" );
338 
339  const Type sina( std::sin(angle) );
340  const Type cosa( std::cos(angle) );
341  const Type tmp( Type(1)-cosa );
342 
343  axis.normalize();
344 
345  v_[0] = cosa + axis[0]*axis[0]*tmp;
346  v_[1] = axis[0]*axis[1]*tmp - axis[2]*sina;
347  v_[2] = axis[0]*axis[2]*tmp + axis[1]*sina;
348  v_[3] = axis[1]*axis[0]*tmp + axis[2]*sina;
349  v_[4] = cosa + axis[1]*axis[1]*tmp;
350  v_[5] = axis[1]*axis[2]*tmp - axis[0]*sina;
351  v_[6] = axis[2]*axis[0]*tmp - axis[1]*sina;
352  v_[7] = axis[2]*axis[1]*tmp + axis[0]*sina;
353  v_[8] = cosa + axis[2]*axis[2]*tmp;
354 }
355 //*************************************************************************************************
356 
357 
358 //*************************************************************************************************
365 template< typename Type > // Data type of the rotation matrix
367 {
368  v_[0] = m.v_[0];
369  v_[1] = m.v_[1];
370  v_[2] = m.v_[2];
371  v_[3] = m.v_[3];
372  v_[4] = m.v_[4];
373  v_[5] = m.v_[5];
374  v_[6] = m.v_[6];
375  v_[7] = m.v_[7];
376  v_[8] = m.v_[8];
377 }
378 //*************************************************************************************************
379 
380 
381 //*************************************************************************************************
386 template< typename Type > // Data type of the rotation matrix
387 template< typename Other > // Data type of the foreign rotation matrix
389 {
390  v_[0] = m[0];
391  v_[1] = m[1];
392  v_[2] = m[2];
393  v_[3] = m[3];
394  v_[4] = m[4];
395  v_[5] = m[5];
396  v_[6] = m[6];
397  v_[7] = m[7];
398  v_[8] = m[8];
399 }
400 //*************************************************************************************************
401 
402 
403 //*************************************************************************************************
416 template< typename Type > // Data type of the rotation matrix
417 inline RotationMatrix<Type>::RotationMatrix( Type xx, Type xy, Type xz,
418  Type yx, Type yy, Type yz,
419  Type zx, Type zy, Type zz )
420 {
421  v_[0] = xx; v_[1] = xy; v_[2] = xz;
422  v_[3] = yx; v_[4] = yy; v_[5] = yz;
423  v_[6] = zx; v_[7] = zy; v_[8] = zz;
424 }
425 //*************************************************************************************************
426 
427 
428 
429 
430 //=================================================================================================
431 //
432 // OPERATORS
433 //
434 //=================================================================================================
435 
436 //*************************************************************************************************
444 template< typename Type > // Data type of the rotation matrix
446 {
447  // This implementation is faster than the synthesized default copy assignment operator and
448  // faster than an implementation with the C library function 'memcpy' in combination with a
449  // protection against self-assignment. Additionally, this version goes without a protection
450  // against self-assignment.
451  v_[0] = rhs.v_[0];
452  v_[1] = rhs.v_[1];
453  v_[2] = rhs.v_[2];
454  v_[3] = rhs.v_[3];
455  v_[4] = rhs.v_[4];
456  v_[5] = rhs.v_[5];
457  v_[6] = rhs.v_[6];
458  v_[7] = rhs.v_[7];
459  v_[8] = rhs.v_[8];
460  return *this;
461 }
462 //*************************************************************************************************
463 
464 
465 //*************************************************************************************************
471 template< typename Type > // Data type of the rotation matrix
472 template< typename Other > // Data type of the foreign rotation matrix
474 {
475  // This implementation is faster than the synthesized default copy assignment operator and
476  // faster than an implementation with the C library function 'memcpy' in combination with a
477  // protection against self-assignment. Additionally, this version goes without a protection
478  // against self-assignment.
479  v_[0] = rhs[0];
480  v_[1] = rhs[1];
481  v_[2] = rhs[2];
482  v_[3] = rhs[3];
483  v_[4] = rhs[4];
484  v_[5] = rhs[5];
485  v_[6] = rhs[6];
486  v_[7] = rhs[7];
487  v_[8] = rhs[8];
488  return *this;
489 }
490 //*************************************************************************************************
491 
492 
493 //*************************************************************************************************
501 template< typename Type > // Data type of the rotation matrix
502 inline Type RotationMatrix<Type>::operator[]( size_t index ) const
503 {
504  BLAZE_USER_ASSERT( index < 9, "Invalid rotation matrix access index" );
505  return v_[index];
506 }
507 //*************************************************************************************************
508 
509 
510 //*************************************************************************************************
517 template< typename Type > // Data type of the rotation matrix
518 inline Type RotationMatrix<Type>::operator()( size_t i, size_t j ) const
519 {
520  BLAZE_USER_ASSERT( i<3 && j<3, "Invalid rotation matrix access index" );
521  return v_[i*3+j];
522 }
523 //*************************************************************************************************
524 
525 
526 //*************************************************************************************************
533 template< typename Type > // Data type of the rotation matrix
534 template< typename Other > // Data type of the right-hand side rotation matrix
536 {
537  // Creating a temporary due to data dependencies
538  const RotationMatrix tmp( v_[0]*rhs[0] + v_[1]*rhs[3] + v_[2]*rhs[6],
539  v_[0]*rhs[1] + v_[1]*rhs[4] + v_[2]*rhs[7],
540  v_[0]*rhs[2] + v_[1]*rhs[5] + v_[2]*rhs[8],
541  v_[3]*rhs[0] + v_[4]*rhs[3] + v_[5]*rhs[6],
542  v_[3]*rhs[1] + v_[4]*rhs[4] + v_[5]*rhs[7],
543  v_[3]*rhs[2] + v_[4]*rhs[5] + v_[5]*rhs[8],
544  v_[6]*rhs[0] + v_[7]*rhs[3] + v_[8]*rhs[6],
545  v_[6]*rhs[1] + v_[7]*rhs[4] + v_[8]*rhs[7],
546  v_[6]*rhs[2] + v_[7]*rhs[5] + v_[8]*rhs[8] );
547 
548  return this->operator=( tmp );
549 }
550 //*************************************************************************************************
551 
552 
553 
554 
555 //=================================================================================================
556 //
557 // UTILITY FUNCTIONS
558 //
559 //=================================================================================================
560 
561 //*************************************************************************************************
566 template< typename Type > // Data type of the rotation matrix
567 inline size_t RotationMatrix<Type>::rows() const
568 {
569  return size_t(3);
570 }
571 //*************************************************************************************************
572 
573 
574 //*************************************************************************************************
579 template< typename Type > // Data type of the rotation matrix
580 inline size_t RotationMatrix<Type>::columns() const
581 {
582  return size_t(3);
583 }
584 //*************************************************************************************************
585 
586 
587 //*************************************************************************************************
600 template< typename Type > // Data type of the rotation matrix
602 {
603  v_[0] = v_[4] = v_[8] = Type(1);
604  v_[1] = v_[2] = v_[3] = v_[5] = v_[6] = v_[7] = Type(0);
605 }
606 //*************************************************************************************************
607 
608 
609 //*************************************************************************************************
614 template< typename Type > // Data type of the rotation matrix
616 {
617  // Although the determinant of a rotation matrix should always be exactly one, the
618  // function calculates the actual determinant to enable checks.
619  return v_[0]*v_[4]*v_[8] + v_[1]*v_[5]*v_[6] + v_[2]*v_[3]*v_[7] -
620  v_[6]*v_[4]*v_[2] - v_[7]*v_[5]*v_[0] - v_[8]*v_[3]*v_[1];
621 }
622 //*************************************************************************************************
623 
624 
625 //*************************************************************************************************
632 template< typename Type > // Data type of the rotation matrix
634 {
635  std::swap( v_[1], v_[3] );
636  std::swap( v_[2], v_[6] );
637  std::swap( v_[5], v_[7] );
638  return *this;
639 }
640 //*************************************************************************************************
641 
642 
643 //*************************************************************************************************
650 template< typename Type > // Data type of the rotation matrix
652 {
653  std::swap( v_[1], v_[3] );
654  std::swap( v_[2], v_[6] );
655  std::swap( v_[5], v_[7] );
656  return *this;
657 }
658 //*************************************************************************************************
659 
660 
661 //*************************************************************************************************
668 template< typename Type > // Data type of the rotation matrix
669 inline void RotationMatrix<Type>::swap( RotationMatrix& m ) /* throw() */
670 {
671  std::swap( v_[0], m.v_[0] );
672  std::swap( v_[1], m.v_[1] );
673  std::swap( v_[2], m.v_[2] );
674  std::swap( v_[3], m.v_[3] );
675  std::swap( v_[4], m.v_[4] );
676  std::swap( v_[5], m.v_[5] );
677  std::swap( v_[6], m.v_[6] );
678  std::swap( v_[7], m.v_[7] );
679  std::swap( v_[8], m.v_[8] );
680 }
681 //*************************************************************************************************
682 
683 
684 
685 
686 //=================================================================================================
687 //
688 // EXPRESSION TEMPLATE EVALUATION FUNCTIONS
689 //
690 //=================================================================================================
691 
692 //*************************************************************************************************
698 template< typename Type > // Data type of the matrix
699 template< typename Other > // Data type of the foreign expression
700 inline bool RotationMatrix<Type>::isAliased( const Other* alias ) const
701 {
702  return static_cast<const void*>( this ) == static_cast<const void*>( alias );
703 }
704 //*************************************************************************************************
705 
706 
707 
708 
709 //=================================================================================================
710 //
711 // MATH FUNCTIONS
712 //
713 //=================================================================================================
714 
715 //*************************************************************************************************
728 template< typename Type > // Data type of the rotation matrix
729 template< typename Other > // Data type of the standard matrix
730 inline const StaticMatrix< typename MultTrait<Type,Other>::Type, 3UL, 3UL, false >
732 {
734 
735  typedef StaticMatrix<typename MultTrait<Type,Other>::Type,3UL,3UL,false> MT;
736 
737  //--Multiplication in two steps (number of FLOP = 90, 1 additional temporary matrix)------------
738 
739  // Precalculation of tmp = m * R(-1)
740  const MT tmp( m.v_[0]*v_[0] + m.v_[1]*v_[1] + m.v_[2]*v_[2],
741  m.v_[0]*v_[3] + m.v_[1]*v_[4] + m.v_[2]*v_[5],
742  m.v_[0]*v_[6] + m.v_[1]*v_[7] + m.v_[2]*v_[8],
743  m.v_[3]*v_[0] + m.v_[4]*v_[1] + m.v_[5]*v_[2],
744  m.v_[3]*v_[3] + m.v_[4]*v_[4] + m.v_[5]*v_[5],
745  m.v_[3]*v_[6] + m.v_[4]*v_[7] + m.v_[5]*v_[8],
746  m.v_[6]*v_[0] + m.v_[7]*v_[1] + m.v_[8]*v_[2],
747  m.v_[6]*v_[3] + m.v_[7]*v_[4] + m.v_[8]*v_[5],
748  m.v_[6]*v_[6] + m.v_[7]*v_[7] + m.v_[8]*v_[8] );
749 
750  // Calculating ROT = R * tmp
751  return MT( v_[0]*tmp.v_[0] + v_[1]*tmp.v_[3] + v_[2]*tmp.v_[6],
752  v_[0]*tmp.v_[1] + v_[1]*tmp.v_[4] + v_[2]*tmp.v_[7],
753  v_[0]*tmp.v_[2] + v_[1]*tmp.v_[5] + v_[2]*tmp.v_[8],
754  v_[3]*tmp.v_[0] + v_[4]*tmp.v_[3] + v_[5]*tmp.v_[6],
755  v_[3]*tmp.v_[1] + v_[4]*tmp.v_[4] + v_[5]*tmp.v_[7],
756  v_[3]*tmp.v_[2] + v_[4]*tmp.v_[5] + v_[5]*tmp.v_[8],
757  v_[6]*tmp.v_[0] + v_[7]*tmp.v_[3] + v_[8]*tmp.v_[6],
758  v_[6]*tmp.v_[1] + v_[7]*tmp.v_[4] + v_[8]*tmp.v_[7],
759  v_[6]*tmp.v_[2] + v_[7]*tmp.v_[5] + v_[8]*tmp.v_[8] );
760 
761  //--Multiplication in one step (number of FLOP = 180, no additional temporary matrix)-----------
762  /*
763  return MT( m.v_[0]*v_[0]*v_[0] + m.v_[4]*v_[1]*v_[1] + m.v_[8]*v_[2]*v_[2] + v_[0]*v_[1]*( m.v_[1]+m.v_[3] ) + v_[0]*v_[2]*( m.v_[2]+m.v_[6] ) + v_[1]*v_[2]*( m.v_[5]+m.v_[7] ),
764  v_[0]*( m.v_[0]*v_[3] + m.v_[1]*v_[4] + m.v_[2]*v_[5] ) + v_[1]*( m.v_[3]*v_[3] + m.v_[4]*v_[4] + m.v_[5]*v_[5] ) + v_[2]*( m.v_[6]*v_[3] + m.v_[7]*v_[4] + m.v_[8]*v_[5] ),
765  v_[0]*( m.v_[0]*v_[6] + m.v_[1]*v_[7] + m.v_[2]*v_[8] ) + v_[1]*( m.v_[3]*v_[6] + m.v_[4]*v_[7] + m.v_[5]*v_[8] ) + v_[2]*( m.v_[6]*v_[6] + m.v_[7]*v_[7] + m.v_[8]*v_[8] ),
766  v_[3]*( m.v_[0]*v_[0] + m.v_[1]*v_[1] + m.v_[2]*v_[2] ) + v_[4]*( m.v_[3]*v_[0] + m.v_[4]*v_[1] + m.v_[5]*v_[2] ) + v_[5]*( m.v_[6]*v_[0] + m.v_[7]*v_[1] + m.v_[8]*v_[2] ),
767  m.v_[0]*v_[3]*v_[3] + m.v_[4]*v_[4]*v_[4] + m.v_[8]*v_[5]*v_[5] + v_[3]*v_[4]*( m.v_[1]+m.v_[3] ) + v_[3]*v_[5]*( m.v_[2]+m.v_[6] ) + v_[4]*v_[5]*( m.v_[5]+m.v_[7] ),
768  v_[3]*( m.v_[0]*v_[6] + m.v_[1]*v_[7] + m.v_[2]*v_[8] ) + v_[4]*( m.v_[3]*v_[6] + m.v_[4]*v_[7] + m.v_[5]*v_[8] ) + v_[5]*( m.v_[6]*v_[6] + m.v_[7]*v_[7] + m.v_[8]*v_[8] ),
769  v_[6]*( m.v_[0]*v_[0] + m.v_[1]*v_[1] + m.v_[2]*v_[2] ) + v_[7]*( m.v_[3]*v_[0] + m.v_[4]*v_[1] + m.v_[5]*v_[2] ) + v_[8]*( m.v_[6]*v_[0] + m.v_[7]*v_[1] + m.v_[8]*v_[2] ),
770  v_[6]*( m.v_[0]*v_[3] + m.v_[1]*v_[4] + m.v_[2]*v_[5] ) + v_[7]*( m.v_[3]*v_[3] + m.v_[4]*v_[4] + m.v_[5]*v_[5] ) + v_[8]*( m.v_[6]*v_[3] + m.v_[7]*v_[4] + m.v_[8]*v_[5] ),
771  m.v_[0]*v_[6]*v_[6] + m.v_[4]*v_[7]*v_[7] + m.v_[8]*v_[8]*v_[8] + v_[6]*v_[7]*( m.v_[1]+m.v_[3] ) + v_[6]*v_[8]*( m.v_[2]+m.v_[6] ) + v_[7]*v_[8]*( m.v_[5]+m.v_[7] ) );
772  */
773 }
774 //*************************************************************************************************
775 
776 
777 //*************************************************************************************************
792 template< typename Type > // Data type of the rotation matrix
793 template< typename Other > // Data type of the diagonal standard matrix
794 inline const StaticMatrix< typename MultTrait<Type,Other>::Type, 3UL, 3UL, false >
796 {
798 
799  typedef StaticMatrix<typename MultTrait<Type,Other>::Type,3UL,3UL,false> MT;
800 
801  // Precalculating tmp = m * R(-1)
802  const MT tmp( m.v_[0]*v_[0], m.v_[0]*v_[3], m.v_[0]*v_[6],
803  m.v_[4]*v_[1], m.v_[4]*v_[4], m.v_[4]*v_[7],
804  m.v_[8]*v_[2], m.v_[8]*v_[5], m.v_[8]*v_[8] );
805 
806  // Calculating ROT = R * tmp
807  return MT( v_[0]*tmp.v_[0] + v_[1]*tmp.v_[3] + v_[2]*tmp.v_[6],
808  v_[0]*tmp.v_[1] + v_[1]*tmp.v_[4] + v_[2]*tmp.v_[7],
809  v_[0]*tmp.v_[2] + v_[1]*tmp.v_[5] + v_[2]*tmp.v_[8],
810  v_[3]*tmp.v_[0] + v_[4]*tmp.v_[3] + v_[5]*tmp.v_[6],
811  v_[3]*tmp.v_[1] + v_[4]*tmp.v_[4] + v_[5]*tmp.v_[7],
812  v_[3]*tmp.v_[2] + v_[4]*tmp.v_[5] + v_[5]*tmp.v_[8],
813  v_[6]*tmp.v_[0] + v_[7]*tmp.v_[3] + v_[8]*tmp.v_[6],
814  v_[6]*tmp.v_[1] + v_[7]*tmp.v_[4] + v_[8]*tmp.v_[7],
815  v_[6]*tmp.v_[2] + v_[7]*tmp.v_[5] + v_[8]*tmp.v_[8] );
816 }
817 //*************************************************************************************************
818 
819 
820 
821 
822 //=================================================================================================
823 //
824 // EULER ROTATIONS
825 //
826 //=================================================================================================
827 
828 //*************************************************************************************************
835 template< typename Type > // Data type of the rotation matrix
837 {
838  const Type cy( std::sqrt( v_[0]*v_[0] + v_[3]*v_[3] ) );
839 
840  if( cy > accuracy ) {
841  return StaticVector<Type,3UL>( std::atan2( v_[7], v_[8] ), std::atan2( -v_[6], cy ), std::atan2( v_[3], v_[0] ) );
842  }
843  else {
844  return StaticVector<Type,3UL>( std::atan2( -v_[5], v_[4] ), std::atan2( -v_[6], cy ), Type(0) );
845  }
846 }
847 //*************************************************************************************************
848 
849 
850 //*************************************************************************************************
856 template< typename Type > // Data type of the rotation matrix
858 {
859  static const unsigned int eulSafe[4] = { 0, 1, 2, 0 };
860  static const unsigned int eulNext[4] = { 1, 2, 0, 1 };
861 
863 
864  // Unpacking the euler order
865  const unsigned int frame( order&1 );
866  const unsigned int repetition( (order&2)>>1 );
867  const unsigned int parity( (order&4)>>2 );
868  const unsigned int i( eulSafe[(order&24)>>3] );
869  const unsigned int j( eulNext[i+parity] );
870  const unsigned int k( eulNext[i+1-parity] );
871 
872  // Treatment of rotations with repetition
873  if( repetition ) {
874  const Type sy( std::sqrt( v_[i*3+j]*v_[i*3+j] + v_[i*3+k]*v_[i*3+k] ) );
875  if( sy > accuracy ) {
876  ea[0] = std::atan2( v_[i*3+j], v_[i*3+k] );
877  ea[1] = std::atan2( sy, v_[i*3+i] );
878  ea[2] = std::atan2( v_[j*3+i], -v_[k*3+i] );
879  }
880  else {
881  ea[0] = std::atan2( -v_[j*3+k], v_[j*3+j] );
882  ea[1] = std::atan2( sy, v_[i*3+i] );
883  ea[2] = Type(0);
884  }
885  }
886 
887  // Treatment of rotations without repetition
888  else {
889  const Type cy( std::sqrt( v_[i*3+i]*v_[i*3+i] + v_[j*3+i]*v_[j*3+i] ) );
890  if( cy > accuracy ) {
891  ea[0] = std::atan2( v_[k*3+j], v_[k*3+k] );
892  ea[1] = std::atan2( -v_[k*3+i], cy );
893  ea[2] = std::atan2( v_[j*3+i], v_[i*3+i] );
894  }
895  else {
896  ea[0] = std::atan2( -v_[j*3+k], v_[j*3+j] );
897  ea[1] = std::atan2( -v_[k*3+i], cy );
898  ea[2] = Type(0);
899  }
900  }
901 
902  // Treatment of an odd partity
903  if( parity ) {
904  ea[0] = -ea[0];
905  ea[1] = -ea[1];
906  ea[2] = -ea[2];
907  }
908 
909  // Treatment of a rotating frame
910  if( frame ) {
911  Type tmp = ea[0];
912  ea[0] = ea[2];
913  ea[2] = tmp;
914  }
915 
916  return ea;
917 }
918 //*************************************************************************************************
919 
920 
921 
922 
923 //=================================================================================================
924 //
925 // GLOBAL OPERATORS
926 //
927 //=================================================================================================
928 
929 //*************************************************************************************************
932 template< typename T1, typename T2 >
933 inline bool operator==( const RotationMatrix<T1>& lhs, const RotationMatrix<T2>& rhs );
934 
935 template< typename T1, typename T2 >
936 inline bool operator!=( const RotationMatrix<T1>& lhs, const RotationMatrix<T2>& rhs );
937 
938 template< typename Type >
939 std::ostream& operator<<( std::ostream& os, const RotationMatrix<Type>& m );
940 
941 template< typename Type >
942 inline bool isnan( const RotationMatrix<Type>& m );
943 
944 template< typename Type >
946 
947 template< typename Type >
949 
950 template< typename Type >
951 inline void reset( RotationMatrix<Type>& m );
952 
953 template< typename Type >
954 inline void clear( RotationMatrix<Type>& m );
955 
956 template< typename Type >
957 inline bool isDefault( const RotationMatrix<Type>& m );
958 
959 template< typename Type >
960 inline const RotationMatrix<Type> trans( const RotationMatrix<Type>& m );
961 
962 template< typename Type >
963 inline const RotationMatrix<Type> inv( const RotationMatrix<Type>& m );
964 
965 template< typename Type >
966 inline const RotationMatrix<Type> sq( const RotationMatrix<Type>& m );
967 
968 template< typename Type >
969 inline void swap( RotationMatrix<Type>& a, RotationMatrix<Type>& b ) /* throw() */;
971 //*************************************************************************************************
972 
973 
974 //*************************************************************************************************
982 template< typename T1 // Data type of the left-hand side rotation matrix
983  , typename T2 > // Data type of the right-hand side rotation matrix
984 inline bool operator==( const RotationMatrix<T1>& lhs, const RotationMatrix<T2>& rhs )
985 {
986  // In order to compare the two matrices, the data values of the lower-order data
987  // type are converted to the higher-order data type within the equal function.
988  if( !equal( lhs[0], rhs[0] ) ||
989  !equal( lhs[1], rhs[1] ) ||
990  !equal( lhs[2], rhs[2] ) ||
991  !equal( lhs[3], rhs[3] ) ||
992  !equal( lhs[4], rhs[4] ) ||
993  !equal( lhs[5], rhs[5] ) ||
994  !equal( lhs[6], rhs[6] ) ||
995  !equal( lhs[7], rhs[7] ) ||
996  !equal( lhs[8], rhs[8] ) )
997  return false;
998  else return true;
999 }
1000 //*************************************************************************************************
1001 
1002 
1003 //*************************************************************************************************
1011 template< typename T1 // Data type of the left-hand side rotation matrix
1012  , typename T2 > // Data type of the right-hand side rotation matrix
1013 inline bool operator!=( const RotationMatrix<T1>& lhs, const RotationMatrix<T2>& rhs )
1014 {
1015  return !( lhs == rhs );
1016 }
1017 //*************************************************************************************************
1018 
1019 
1020 //*************************************************************************************************
1028 template< typename Type > // Data type of the rotation matrix
1029 std::ostream& operator<<( std::ostream& os, const RotationMatrix<Type>& m )
1030 {
1031  return os << " ( " << m[0] << " , " << m[1] << " , " << m[2] << " )\n"
1032  << " ( " << m[3] << " , " << m[4] << " , " << m[5] << " )\n"
1033  << " ( " << m[6] << " , " << m[7] << " , " << m[8] << " )\n";
1034 }
1035 //*************************************************************************************************
1036 
1037 
1038 //*************************************************************************************************
1045 template< typename Type > // Data type of the rotation matrix
1046 inline bool isnan( const RotationMatrix<Type>& m )
1047 {
1048  if( isnan( m[0] ) || isnan( m[1] ) || isnan( m[2] ) ||
1049  isnan( m[3] ) || isnan( m[4] ) || isnan( m[5] ) ||
1050  isnan( m[6] ) || isnan( m[7] ) || isnan( m[8] ) )
1051  return true;
1052  else return false;
1053 }
1054 //*************************************************************************************************
1055 
1056 
1057 //*************************************************************************************************
1067 template< typename Type > // Data type of the rotation matrix
1069 {
1070  using std::abs;
1071  return StaticMatrix<Type,3UL,3UL,false>( abs(m[0]), abs(m[1]), abs(m[2]),
1072  abs(m[3]), abs(m[4]), abs(m[5]),
1073  abs(m[6]), abs(m[7]), abs(m[8]) );
1074 }
1075 //*************************************************************************************************
1076 
1077 
1078 //*************************************************************************************************
1088 template< typename Type > // Data type of the rotation matrix
1090 {
1091  using std::fabs;
1092  return StaticMatrix<Type,3UL,3UL,false>( fabs(m[0]), fabs(m[1]), fabs(m[2]),
1093  fabs(m[3]), fabs(m[4]), fabs(m[5]),
1094  fabs(m[6]), fabs(m[7]), fabs(m[8]) );
1095 }
1096 //*************************************************************************************************
1097 
1098 
1099 //*************************************************************************************************
1106 template< typename Type > // Data type of the rotation matrix
1107 inline void reset( RotationMatrix<Type>& m )
1108 {
1109  m.reset();
1110 }
1111 //*************************************************************************************************
1112 
1113 
1114 //*************************************************************************************************
1123 template< typename Type > // Data type of the rotation matrix
1124 inline void clear( RotationMatrix<Type>& m )
1125 {
1126  m.reset();
1127 }
1128 //*************************************************************************************************
1129 
1130 
1131 //*************************************************************************************************
1138 template< typename Type > // Data type of the rotation matrix
1139 inline bool isDefault( const RotationMatrix<Type>& m )
1140 {
1141  return ( m[0] == Type(1) ) && ( m[1] == Type(0) ) && ( m[2] == Type(0) ) &&
1142  ( m[3] == Type(0) ) && ( m[4] == Type(1) ) && ( m[5] == Type(0) ) &&
1143  ( m[6] == Type(0) ) && ( m[7] == Type(0) ) && ( m[8] == Type(1) );
1144 }
1145 //*************************************************************************************************
1146 
1147 
1148 //*************************************************************************************************
1165 template< typename Type > // Data type of the rotation matrix
1167 {
1168  return RotationMatrix<Type>( m[0], m[3], m[6],
1169  m[1], m[4], m[7],
1170  m[2], m[5], m[8] );
1171 }
1172 //*************************************************************************************************
1173 
1174 
1175 //*************************************************************************************************
1192 template< typename Type > // Data type of the rotation matrix
1194 {
1195  return RotationMatrix<Type>( m[0], m[3], m[6], m[1], m[4], m[7], m[2], m[5], m[8] );
1196 }
1197 //*************************************************************************************************
1198 
1199 
1200 //*************************************************************************************************
1210 template< typename Type > // Data type of the rotation matrix
1212 {
1213  return m * m;
1214 }
1215 //*************************************************************************************************
1216 
1217 
1218 //*************************************************************************************************
1227 template< typename Type > // Data type of the rotation matrices
1228 inline void swap( RotationMatrix<Type>& a, RotationMatrix<Type>& b ) /* throw() */
1229 {
1230  a.swap( b );
1231 }
1232 //*************************************************************************************************
1233 
1234 
1235 
1236 
1237 //=================================================================================================
1238 //
1239 // GLOBAL ARITHMETIC OPERATORS
1240 //
1241 //=================================================================================================
1242 
1243 //*************************************************************************************************
1246 template< typename T1, typename T2 >
1247 inline const StaticVector< typename MultTrait<T1,T2>::Type, 3UL, false >
1248  operator*( const RotationMatrix<T1>& lhs, const StaticVector<T2,3UL,false>& rhs );
1249 
1250 template< typename T1, typename T2 >
1251 inline const StaticVector< typename MultTrait<T1,T2>::Type, 3UL, true >
1252  operator*( const StaticVector<T1,3UL,true>& lhs, const RotationMatrix<T2>& rhs );
1253 
1254 template< typename T1, typename T2 >
1255 inline const StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL, false >
1256  operator*( const RotationMatrix<T1>& lhs, const StaticMatrix<T2,3UL,3UL,false>& rhs );
1257 
1258 template< typename T1, typename T2 >
1259 inline const StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL, false >
1260  operator*( const StaticMatrix<T1,3UL,3UL,false>& lhs, const RotationMatrix<T2>& rhs );
1261 
1262 template< typename T1, typename T2 >
1263 inline const RotationMatrix< typename MultTrait<T1,T2>::Type >
1264  operator*( const RotationMatrix<T1>& lhs, const RotationMatrix<T2>& rhs );
1266 //*************************************************************************************************
1267 
1268 
1269 //*************************************************************************************************
1282 template< typename T1 // Data type of the left-hand side rotation matrix
1283  , typename T2 > // Data type of the right-hand side vector
1284 inline const StaticVector< typename MultTrait<T1,T2>::Type, 3UL, false >
1286 {
1287  typedef typename MultTrait<T1,T2>::Type MT;
1288  return StaticVector<MT,3UL,false>( lhs[0]*rhs[0] + lhs[1]*rhs[1] + lhs[2]*rhs[2],
1289  lhs[3]*rhs[0] + lhs[4]*rhs[1] + lhs[5]*rhs[2],
1290  lhs[6]*rhs[0] + lhs[7]*rhs[1] + lhs[8]*rhs[2] );
1291 }
1292 //*************************************************************************************************
1293 
1294 
1295 //*************************************************************************************************
1308 template< typename T1 // Data type of the left-hand side vector
1309  , typename T2 > // Data type of the right-hand side rotation matrix
1310 inline const StaticVector< typename MultTrait<T1,T2>::Type, 3UL, true >
1312 {
1313  typedef typename MultTrait<T1,T2>::Type MT;
1314  return StaticVector<MT,3UL,true>( lhs[0]*rhs[0] + lhs[1]*rhs[3] + lhs[2]*rhs[6],
1315  lhs[0]*rhs[1] + lhs[1]*rhs[4] + lhs[2]*rhs[7],
1316  lhs[0]*rhs[2] + lhs[1]*rhs[5] + lhs[2]*rhs[8] );
1317 }
1318 //*************************************************************************************************
1319 
1320 
1321 //*************************************************************************************************
1334 template< typename T1 // Data type of the left-hand side rotation matrix
1335  , typename T2 > // Data type of the right-hand side standard matrix
1336 inline const StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL, false >
1338 {
1339  typedef StaticMatrix<typename MultTrait<T1,T2>::Type,3UL,3UL,false> MT;
1340  return MT( lhs[0]*rhs[0] + lhs[1]*rhs[3] + lhs[2]*rhs[6],
1341  lhs[0]*rhs[1] + lhs[1]*rhs[4] + lhs[2]*rhs[7],
1342  lhs[0]*rhs[2] + lhs[1]*rhs[5] + lhs[2]*rhs[8],
1343  lhs[3]*rhs[0] + lhs[4]*rhs[3] + lhs[5]*rhs[6],
1344  lhs[3]*rhs[1] + lhs[4]*rhs[4] + lhs[5]*rhs[7],
1345  lhs[3]*rhs[2] + lhs[4]*rhs[5] + lhs[5]*rhs[8],
1346  lhs[6]*rhs[0] + lhs[7]*rhs[3] + lhs[8]*rhs[6],
1347  lhs[6]*rhs[1] + lhs[7]*rhs[4] + lhs[8]*rhs[7],
1348  lhs[6]*rhs[2] + lhs[7]*rhs[5] + lhs[8]*rhs[8] );
1349 }
1350 //*************************************************************************************************
1351 
1352 
1353 //*************************************************************************************************
1366 template< typename T1 // Data type of the left-hand side standard matrix
1367  , typename T2 > // Data type of the right-hand side rotation matrix
1368 inline const StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL, false >
1370 {
1371  typedef StaticMatrix<typename MultTrait<T1,T2>::Type,3UL,3UL,false> MT;
1372  return MT( lhs[0]*rhs[0] + lhs[1]*rhs[3] + lhs[2]*rhs[6],
1373  lhs[0]*rhs[1] + lhs[1]*rhs[4] + lhs[2]*rhs[7],
1374  lhs[0]*rhs[2] + lhs[1]*rhs[5] + lhs[2]*rhs[8],
1375  lhs[3]*rhs[0] + lhs[4]*rhs[3] + lhs[5]*rhs[6],
1376  lhs[3]*rhs[1] + lhs[4]*rhs[4] + lhs[5]*rhs[7],
1377  lhs[3]*rhs[2] + lhs[4]*rhs[5] + lhs[5]*rhs[8],
1378  lhs[6]*rhs[0] + lhs[7]*rhs[3] + lhs[8]*rhs[6],
1379  lhs[6]*rhs[1] + lhs[7]*rhs[4] + lhs[8]*rhs[7],
1380  lhs[6]*rhs[2] + lhs[7]*rhs[5] + lhs[8]*rhs[8] );
1381 }
1382 //*************************************************************************************************
1383 
1384 
1385 //*************************************************************************************************
1397 template< typename T1 // Data type of the left-hand side rotation matrix
1398  , typename T2 > // Data type of the right-hand side rotation matrix
1399 inline const RotationMatrix< typename MultTrait<T1,T2>::Type >
1401 {
1402  typedef typename MultTrait<T1,T2>::Type MT;
1403  return RotationMatrix<MT>( lhs.v_[0]*rhs.v_[0] + lhs.v_[1]*rhs.v_[3] + lhs.v_[2]*rhs.v_[6],
1404  lhs.v_[0]*rhs.v_[1] + lhs.v_[1]*rhs.v_[4] + lhs.v_[2]*rhs.v_[7],
1405  lhs.v_[0]*rhs.v_[2] + lhs.v_[1]*rhs.v_[5] + lhs.v_[2]*rhs.v_[8],
1406  lhs.v_[3]*rhs.v_[0] + lhs.v_[4]*rhs.v_[3] + lhs.v_[5]*rhs.v_[6],
1407  lhs.v_[3]*rhs.v_[1] + lhs.v_[4]*rhs.v_[4] + lhs.v_[5]*rhs.v_[7],
1408  lhs.v_[3]*rhs.v_[2] + lhs.v_[4]*rhs.v_[5] + lhs.v_[5]*rhs.v_[8],
1409  lhs.v_[6]*rhs.v_[0] + lhs.v_[7]*rhs.v_[3] + lhs.v_[8]*rhs.v_[6],
1410  lhs.v_[6]*rhs.v_[1] + lhs.v_[7]*rhs.v_[4] + lhs.v_[8]*rhs.v_[7],
1411  lhs.v_[6]*rhs.v_[2] + lhs.v_[7]*rhs.v_[5] + lhs.v_[8]*rhs.v_[8] );
1412 }
1413 //*************************************************************************************************
1414 
1415 
1416 
1417 
1418 //=================================================================================================
1419 //
1420 // MULTTRAIT SPECIALIZATIONS
1421 //
1422 //=================================================================================================
1423 
1424 //*************************************************************************************************
1426 template< typename T1, typename T2 >
1427 struct MultTrait< RotationMatrix<T1>, StaticVector<T2,3UL,false> >
1428 {
1429  typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL, false > Type;
1430 };
1431 
1432 template< typename T1, typename T2 >
1433 struct MultTrait< StaticVector<T1,3UL,true>, RotationMatrix<T2> >
1434 {
1435  typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL, true > Type;
1436 };
1437 
1438 template< typename T1, typename T2 >
1439 struct MultTrait< RotationMatrix<T1>, DynamicVector<T2,false> >
1440 {
1441  typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL, false > Type;
1442 };
1443 
1444 template< typename T1, typename T2 >
1445 struct MultTrait< DynamicVector<T1,true>, RotationMatrix<T2> >
1446 {
1447  typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL, true > Type;
1448 };
1449 
1450 template< typename T1, typename T2 >
1451 struct MultTrait< RotationMatrix<T1>, CompressedVector<T2,false> >
1452 {
1453  typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL, false > Type;
1454 };
1455 
1456 template< typename T1, typename T2 >
1457 struct MultTrait< CompressedVector<T1,true>, RotationMatrix<T2> >
1458 {
1459  typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL, true > Type;
1460 };
1461 
1462 template< typename T1, typename T2 >
1463 struct MultTrait< RotationMatrix<T1>, StaticMatrix<T2,3UL,3UL,false> >
1464 {
1465  typedef StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL, false > Type;
1466 };
1467 
1468 template< typename T1, typename T2 >
1469 struct MultTrait< StaticMatrix<T1,3UL,3UL,false>, RotationMatrix<T2> >
1470 {
1471  typedef StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL, false > Type;
1472 };
1473 
1474 template< typename T1, typename T2, bool SO >
1475 struct MultTrait< RotationMatrix<T1>, DynamicMatrix<T2,SO> >
1476 {
1477  typedef DynamicMatrix< typename MultTrait<T1,T2>::Type, false > Type;
1478 };
1479 
1480 template< typename T1, bool SO, typename T2 >
1481 struct MultTrait< DynamicMatrix<T1,SO>, RotationMatrix<T2> >
1482 {
1483  typedef DynamicMatrix< typename MultTrait<T1,T2>::Type, false > Type;
1484 };
1485 
1486 template< typename T1, typename T2, bool SO >
1487 struct MultTrait< RotationMatrix<T1>, CompressedMatrix<T2,SO> >
1488 {
1489  typedef DynamicMatrix< typename MultTrait<T1,T2>::Type, false > Type;
1490 };
1491 
1492 template< typename T1, bool SO, typename T2 >
1493 struct MultTrait< CompressedMatrix<T1,SO>, RotationMatrix<T2> >
1494 {
1495  typedef DynamicMatrix< typename MultTrait<T1,T2>::Type, false > Type;
1496 };
1497 
1498 template< typename T1, typename T2 >
1499 struct MultTrait< RotationMatrix<T1>, RotationMatrix<T2> >
1500 {
1501  typedef RotationMatrix< typename MultTrait<T1,T2>::Type > Type;
1502 };
1504 //*************************************************************************************************
1505 
1506 
1507 
1508 
1509 //=================================================================================================
1510 //
1511 // MATHTRAIT SPECIALIZATIONS
1512 //
1513 //=================================================================================================
1514 
1515 //*************************************************************************************************
1517 template< typename T1, typename T2 >
1518 struct MathTrait< RotationMatrix<T1>, RotationMatrix<T2> >
1519 {
1520  typedef RotationMatrix< typename MathTrait<T1,T2>::HighType > HighType;
1521  typedef RotationMatrix< typename MathTrait<T1,T2>::LowType > LowType;
1522 };
1524 //*************************************************************************************************
1525 
1526 
1527 
1528 
1529 //=================================================================================================
1530 //
1531 // TYPE DEFINITIONS
1532 //
1533 //=================================================================================================
1534 
1535 //*************************************************************************************************
1540 //*************************************************************************************************
1541 
1542 } // namespace blaze
1543 
1544 #endif