22 #ifndef _BLAZE_MATH_ROTATIONMATRIX_H_
23 #define _BLAZE_MATH_ROTATIONMATRIX_H_
140 template<
typename Type >
141 class RotationMatrix :
public DenseMatrix< RotationMatrix<Type>, false >
156 template<
typename Axis >
161 template<
typename Other >
176 inline Type
operator()(
size_t i,
size_t j )
const;
184 inline size_t rows()
const;
197 template<
typename Other >
inline bool isAliased(
const Other* alias )
const;
204 template<
typename Other >
208 template<
typename Other >
245 Type yx, Type yy, Type yz,
246 Type zx, Type zy, Type zz );
266 template<
typename Other >
friend class Quaternion;
268 template<
typename Other >
271 template<
typename Other >
274 template<
typename T1,
typename T2 >
305 template<
typename Type >
308 v_[0] = v_[4] = v_[8] = Type(1);
309 v_[1] = v_[2] = v_[3] = v_[5] = v_[6] = v_[7] = Type(0);
331 template<
typename Type >
332 template<
typename Axis >
339 const Type sina( std::sin(angle) );
340 const Type cosa( std::cos(angle) );
341 const Type tmp( Type(1)-cosa );
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;
365 template<
typename Type >
386 template<
typename Type >
387 template<
typename Other >
416 template<
typename Type >
418 Type yx, Type yy, Type yz,
419 Type zx, Type zy, Type zz )
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;
444 template<
typename Type >
471 template<
typename Type >
472 template<
typename Other >
501 template<
typename Type >
517 template<
typename Type >
533 template<
typename Type >
534 template<
typename Other >
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] );
548 return this->operator=( tmp );
566 template<
typename Type >
579 template<
typename Type >
600 template<
typename Type >
603 v_[0] = v_[4] = v_[8] = Type(1);
604 v_[1] = v_[2] = v_[3] = v_[5] = v_[6] = v_[7] = Type(0);
614 template<
typename Type >
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];
632 template<
typename Type >
650 template<
typename Type >
668 template<
typename Type >
698 template<
typename Type >
699 template<
typename Other >
702 return static_cast<const void*
>( this ) == static_cast<const void*>( alias );
728 template<
typename Type >
729 template<
typename Other >
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] );
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] );
792 template<
typename Type >
793 template<
typename Other >
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] );
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] );
835 template<
typename Type >
838 const Type cy( std::sqrt( v_[0]*v_[0] + v_[3]*v_[3] ) );
841 return StaticVector<Type,3UL>( std::atan2( v_[7], v_[8] ), std::atan2( -v_[6], cy ), std::atan2( v_[3], v_[0] ) );
856 template<
typename Type >
859 static const unsigned int eulSafe[4] = { 0, 1, 2, 0 };
860 static const unsigned int eulNext[4] = { 1, 2, 0, 1 };
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] );
874 const Type sy( std::sqrt( v_[i*3+j]*v_[i*3+j] + v_[i*3+k]*v_[i*3+k] ) );
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] );
881 ea[0] = std::atan2( -v_[j*3+k], v_[j*3+j] );
882 ea[1] = std::atan2( sy, v_[i*3+i] );
889 const Type cy( std::sqrt( v_[i*3+i]*v_[i*3+i] + v_[j*3+i]*v_[j*3+i] ) );
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] );
896 ea[0] = std::atan2( -v_[j*3+k], v_[j*3+j] );
897 ea[1] = std::atan2( -v_[k*3+i], cy );
932 template<
typename T1,
typename T2 >
935 template<
typename T1,
typename T2 >
938 template<
typename Type >
939 std::ostream& operator<<( std::ostream& os, const RotationMatrix<Type>& m );
941 template<
typename Type >
944 template<
typename Type >
947 template<
typename Type >
950 template<
typename Type >
953 template<
typename Type >
956 template<
typename Type >
959 template<
typename Type >
962 template<
typename Type >
965 template<
typename Type >
968 template<
typename Type >
982 template<
typename T1
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] ) )
1011 template<
typename T1
1015 return !( lhs == rhs );
1028 template<
typename Type >
1029 std::ostream& operator<<( std::ostream& os, const RotationMatrix<Type>& m )
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";
1045 template<
typename Type >
1067 template<
typename Type >
1088 template<
typename Type >
1106 template<
typename Type >
1123 template<
typename Type >
1138 template<
typename Type >
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) );
1165 template<
typename Type >
1192 template<
typename Type >
1210 template<
typename Type >
1227 template<
typename Type >
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 );
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 );
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 );
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 );
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 );
1282 template<
typename T1
1284 inline const StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
false >
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] );
1308 template<
typename T1
1310 inline const StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
true >
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] );
1334 template<
typename T1
1336 inline const StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL,
false >
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] );
1366 template<
typename T1
1368 inline const StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL,
false >
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] );
1397 template<
typename T1
1399 inline const RotationMatrix< typename MultTrait<T1,T2>::Type >
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] );
1426 template<
typename T1,
typename T2 >
1427 struct MultTrait< RotationMatrix<T1>, StaticVector<T2,3UL,false> >
1429 typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
false > Type;
1432 template<
typename T1,
typename T2 >
1433 struct MultTrait< StaticVector<T1,3UL,true>, RotationMatrix<T2> >
1435 typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
true > Type;
1438 template<
typename T1,
typename T2 >
1439 struct MultTrait< RotationMatrix<T1>, DynamicVector<T2,false> >
1441 typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
false > Type;
1444 template<
typename T1,
typename T2 >
1445 struct MultTrait< DynamicVector<T1,true>, RotationMatrix<T2> >
1447 typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
true > Type;
1450 template<
typename T1,
typename T2 >
1451 struct MultTrait< RotationMatrix<T1>, CompressedVector<T2,false> >
1453 typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
false > Type;
1456 template<
typename T1,
typename T2 >
1457 struct MultTrait< CompressedVector<T1,true>, RotationMatrix<T2> >
1459 typedef StaticVector< typename MultTrait<T1,T2>::Type, 3UL,
true > Type;
1462 template<
typename T1,
typename T2 >
1463 struct MultTrait< RotationMatrix<T1>, StaticMatrix<T2,3UL,3UL,false> >
1465 typedef StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL,
false > Type;
1468 template<
typename T1,
typename T2 >
1469 struct MultTrait< StaticMatrix<T1,3UL,3UL,false>, RotationMatrix<T2> >
1471 typedef StaticMatrix< typename MultTrait<T1,T2>::Type, 3UL, 3UL,
false > Type;
1474 template<
typename T1,
typename T2,
bool SO >
1475 struct MultTrait< RotationMatrix<T1>, DynamicMatrix<T2,SO> >
1477 typedef DynamicMatrix< typename MultTrait<T1,T2>::Type,
false > Type;
1480 template<
typename T1,
bool SO,
typename T2 >
1481 struct MultTrait< DynamicMatrix<T1,SO>, RotationMatrix<T2> >
1483 typedef DynamicMatrix< typename MultTrait<T1,T2>::Type,
false > Type;
1486 template<
typename T1,
typename T2,
bool SO >
1487 struct MultTrait< RotationMatrix<T1>, CompressedMatrix<T2,SO> >
1489 typedef DynamicMatrix< typename MultTrait<T1,T2>::Type,
false > Type;
1492 template<
typename T1,
bool SO,
typename T2 >
1493 struct MultTrait< CompressedMatrix<T1,SO>, RotationMatrix<T2> >
1495 typedef DynamicMatrix< typename MultTrait<T1,T2>::Type,
false > Type;
1498 template<
typename T1,
typename T2 >
1499 struct MultTrait< RotationMatrix<T1>, RotationMatrix<T2> >
1501 typedef RotationMatrix< typename MultTrait<T1,T2>::Type > Type;
1517 template<
typename T1,
typename T2 >
1518 struct MathTrait< RotationMatrix<T1>, RotationMatrix<T2> >
1520 typedef RotationMatrix< typename MathTrait<T1,T2>::HighType > HighType;
1521 typedef RotationMatrix< typename MathTrait<T1,T2>::LowType > LowType;