22 #ifndef _BLAZE_MATH_QUATERNION_H_
23 #define _BLAZE_MATH_QUATERNION_H_
89 template<
typename Type >
102 explicit inline Quaternion( Type r, Type i, Type j, Type k );
104 template<
typename Axis >
107 explicit inline Quaternion( Type xangle, Type yangle, Type zangle );
109 template<
typename Other >
114 template<
typename Other >
135 inline Quaternion&
set( Type r, Type i, Type j, Type k );
137 inline Type
length()
const;
142 inline void rotateX( Type angle );
143 inline void rotateY( Type angle );
144 inline void rotateZ( Type angle );
152 template<
typename Other,
bool TF >
156 template<
typename Other >
160 template<
typename Other >
164 template<
typename Other >
210 template<
typename Type >
229 template<
typename Type >
232 v_[0] = r; v_[1] = i; v_[2] = j; v_[3] = k;
251 template<
typename Type >
252 template<
typename Axis >
264 const Type sina( std::sin( angle*Type(0.5) ) );
265 const Type cosa( std::cos( angle*Type(0.5) ) );
270 v_[1] = sina * axis[0];
271 v_[2] = sina * axis[1];
272 v_[3] = sina * axis[2];
287 template<
typename Type >
306 template<
typename Type >
307 template<
typename Other >
325 template<
typename Type >
341 template<
typename Type >
342 template<
typename Other >
369 template<
typename Type >
391 template<
typename Type >
392 template<
typename Other >
416 template<
typename Type >
441 template<
typename Type >
466 template<
typename Type >
470 v_[1] = v_[2] = v_[3] = Type(0);
480 template<
typename Type >
485 return std::sqrt( v_[0]*v_[0] + v_[1]*v_[1] + v_[2]*v_[2] + v_[3]*v_[3] );
495 template<
typename Type >
498 const Type len( std::sqrt( v_[0]*v_[0] + v_[1]*v_[1] + v_[2]*v_[2] + v_[3]*v_[3] ) );
503 const Type ilen( Type(1)/len );
520 template<
typename Type >
523 const Type len( std::sqrt( v_[0]*v_[0] + v_[1]*v_[1] + v_[2]*v_[2] + v_[3]*v_[3] ) );
528 const Type ilen( Type(1)/len );
530 return Quaternion( ilen*v_[0], ilen*v_[1], ilen*v_[2], ilen*v_[3] );
540 template<
typename Type >
556 template<
typename Type >
560 Type(2)*( v_[1]*v_[2] - v_[0]*v_[3] ),
561 Type(2)*( v_[1]*v_[3] + v_[0]*v_[2] ),
562 Type(2)*( v_[1]*v_[2] + v_[0]*v_[3] ),
563 Type(1) - Type(2)*v_[1]*v_[1] - Type(2)*v_[3]*v_[3],
564 Type(2)*( v_[2]*v_[3] - v_[0]*v_[1] ),
565 Type(2)*( v_[1]*v_[3] - v_[0]*v_[2] ),
566 Type(2)*( v_[2]*v_[3] + v_[0]*v_[1] ),
567 Type(1) - Type(2)*v_[1]*v_[1] - Type(2)*v_[2]*v_[2] );
578 template<
typename Type >
581 const Type sina( std::sin( angle*Type(0.5) ) );
582 const Type cosa( std::cos( angle*Type(0.5) ) );
585 cosa*v_[1] + sina*v_[0],
586 cosa*v_[2] - sina*v_[3],
587 cosa*v_[3] + sina*v_[2] );
589 this->operator=( q );
600 template<
typename Type >
603 const Type sina( std::sin( angle*Type(0.5) ) );
604 const Type cosa( std::cos( angle*Type(0.5) ) );
607 cosa*v_[1] + sina*v_[3],
608 cosa*v_[2] + sina*v_[0],
609 cosa*v_[3] - sina*v_[1] );
611 this->operator=( q );
622 template<
typename Type >
625 const Type sina( std::sin( angle*Type(0.5) ) );
626 const Type cosa( std::cos( angle*Type(0.5) ) );
629 cosa*v_[1] - sina*v_[2],
630 cosa*v_[2] + sina*v_[1],
631 cosa*v_[3] + sina*v_[0] );
633 this->operator=( q );
645 template<
typename Type >
674 template<
typename Type >
675 template<
typename Other
683 const MT w( v_[1]*v[0] + v_[2]*v[1] + v_[3]*v[2] );
684 const MT x( v_[0]*v[0] - v_[3]*v[1] + v_[2]*v[2] );
685 const MT y( v_[0]*v[1] - v_[1]*v[2] + v_[3]*v[0] );
686 const MT z( v_[0]*v[2] - v_[2]*v[0] + v_[1]*v[1] );
689 v_[0]*y + v_[2]*w + v_[3]*x - v_[1]*z,
690 v_[0]*z + v_[3]*w + v_[1]*y - v_[2]*x );
717 template<
typename Type >
718 template<
typename Other >
741 template<
typename Type >
742 template<
typename Other >
759 template<
typename Type >
760 template<
typename Other >
767 const High y ( u.
length() );
768 const High x ( v_[0] );
769 const High dot( u * axis );
771 return High(2) * std::atan2( y, ( dot <
real(0) )?( -x ):( x ) );
787 template<
typename T1,
typename T2 >
790 template<
typename T1,
typename T2 >
793 template<
typename Type >
794 std::ostream& operator<<( std::ostream& os, const Quaternion<Type>& q );
796 template<
typename Type >
799 template<
typename Type >
802 template<
typename Type >
805 template<
typename Type >
808 template<
typename Type >
811 template<
typename Type >
814 template<
typename Type >
817 template<
typename Type >
831 template<
typename T1
837 if( !
equal( lhs[0], rhs[0] ) ||
838 !
equal( lhs[1], rhs[1] ) ||
839 !
equal( lhs[2], rhs[2] ) ||
840 !
equal( lhs[2], rhs[2] ) )
855 template<
typename T1
859 return !( lhs == rhs );
872 template<
typename Type >
873 std::ostream& operator<<( std::ostream& os, const Quaternion<Type>& q )
875 return os <<
"<" << q[0] <<
"," << q[1] <<
"," << q[2] <<
"," << q[3] <<
">";
888 template<
typename Type >
893 char bracket1, bracket2, comma1, comma2, comma3;
894 Type r(0), i(0), j(0), k(0);
895 const std::istream::pos_type pos( is.tellg() );
896 const std::istream::fmtflags oldFlags( is.flags() );
902 if( !(is >> bracket1 >> r >> comma1 >> i >> comma2 >> j >> comma3 >> k >> bracket2) ||
903 bracket1 !=
'<' || comma1 !=
',' || comma2 !=
',' || comma3 !=
',' || bracket2 !=
'>' ) {
906 is.setstate( std::istream::failbit );
907 is.flags( oldFlags );
915 is.flags( oldFlags );
929 template<
typename Type >
946 template<
typename Type >
963 template<
typename Type >
985 template<
typename Type >
988 return ( q[0] == Type(1) ) && ( q[1] == Type(0) ) && ( q[2] == Type(0) ) && ( q[3] == Type(0) );
1002 template<
typename Type >
1020 template<
typename Type >
1037 template<
typename Type >
1056 template<
typename T1,
typename T2 >
1057 inline const Quaternion< typename MultTrait<T1,T2>::Type >
1058 operator*(
const Quaternion<T1>& lhs,
const Quaternion<T2>& rhs );
1072 template<
typename T1
1074 inline const Quaternion< typename MultTrait<T1,T2>::Type >
1079 const MT r( lhs[0]*rhs[0] - lhs[1]*rhs[1] - lhs[2]*rhs[2] - lhs[3]*rhs[3] );
1080 const MT i( lhs[0]*rhs[1] + lhs[1]*rhs[0] + lhs[2]*rhs[3] - lhs[3]*rhs[2] );
1081 const MT j( lhs[0]*rhs[2] + lhs[2]*rhs[0] + lhs[3]*rhs[1] - lhs[1]*rhs[3] );
1082 const MT k( lhs[0]*rhs[3] + lhs[3]*rhs[0] + lhs[1]*rhs[2] - lhs[2]*rhs[1] );
1084 const MT len2( r*r + i*i + j*j + k*k );
1086 if(
std::fabs( len2 - MT(1) ) < MT(1E-8) ) {
1090 const MT ilen( MT(1) / std::sqrt( len2 ) );
1107 template<
typename T1,
typename T2 >
1108 struct MultTrait< Quaternion<T1>, Quaternion<T2> >
1110 typedef Quaternion< typename MultTrait<T1,T2>::Type > MultType;
1126 template<
typename T1,
typename T2 >
1127 struct MathTrait< Quaternion<T1>, Quaternion<T2> >
1129 typedef Quaternion< typename MathTrait<T1,T2>::HighType > HighType;
1130 typedef Quaternion< typename MathTrait<T1,T2>::LowType > LowType;