33 #ifndef __VMML__QUATERNION__HPP__
34 #define __VMML__QUATERNION__HPP__
36 #include <vmmlib/vector.hpp>
37 #include <vmmlib/matrix.hpp>
38 #include <vmmlib/math.hpp>
39 #include <vmmlib/enable_if.hpp>
41 #include <vmmlib/exception.hpp>
42 #include <vmmlib/vmmlib_config.hpp>
55 #define QUATERNION_TRACE_EPSILON 1e-5
60 template <
typename T >
66 using super::operator();
74 using super::find_min;
75 using super::find_max;
76 using super::find_min_index;
77 using super::find_max_index;
78 using super::iter_set;
98 void set( T ww, T xx, T yy, T zz);
101 template<
typename input_iterator_t >
102 void set( input_iterator_t begin_, input_iterator_t end_ );
104 bool operator==(
const T& a )
const;
105 bool operator!=(
const T& a )
const;
107 bool operator==(
const quaternion& a )
const;
108 bool operator!=(
const quaternion& a )
const;
113 bool is_akin(
const quaternion& a,
114 const T& delta = std::numeric_limits< T >::epsilon() );
117 quaternion get_conjugate()
const;
120 T squared_abs()
const;
123 quaternion get_normalized()
const;
125 quaternion negate()
const;
126 quaternion operator-()
const;
128 quaternion& operator=(
const quaternion& other);
146 quaternion operator*( T a )
const;
147 quaternion operator/( T a )
const;
149 void operator*=( T a );
150 void operator/=( T a );
170 quaternion inverse();
172 void normal(
const quaternion& aa,
const quaternion& bb,
const quaternion& cc,
const quaternion& dd );
173 quaternion normal(
const quaternion& aa,
const quaternion& bb,
const quaternion& cc );
183 static quaternion slerp( T a,
const quaternion& p,
184 const quaternion& q,
const T epsilon = 1e-13 );
188 template<
size_t D >
void get_rotation_matrix(
matrix< D, D, T >& result )
const;
190 friend std::ostream& operator<< ( std::ostream& os,
const quaternion& q )
194 for( ; index < 3; ++index )
196 os << q.at( index ) <<
", ";
198 os << q.at( index ) <<
") ";
202 static const quaternion IDENTITY;
203 static const quaternion QUATERI;
204 static const quaternion QUATERJ;
205 static const quaternion QUATERK;
209 #ifndef VMMLIB_NO_TYPEDEFS
218 template <
typename T >
221 template <
typename T >
224 template <
typename T >
227 template <
typename T >
230 template <
typename T >
241 template <
typename T >
244 super::set( xyz, w_ );
250 template <
typename T >
253 super::set( xyz, static_cast< T >( 0.0 ) );
258 template<
typename T >
template<
size_t M >
259 quaternion< T >::quaternion(
const matrix< M, M, T >& rotation_matrix_,
260 typename enable_if< M >= 3 >::type* )
262 this->
template set< M >( rotation_matrix_ );
268 template <
typename T >
template<
size_t D >
269 void quaternion< T >::set(
const matrix< D, D, T >& M )
271 T trace = M( 0, 0 ) + M( 1, 1 ) + M( 2,2 ) + 1.0;
274 if( trace > QUATERNION_TRACE_EPSILON )
276 T s = 0.5 / sqrt( trace );
277 x() = M( 2, 1 ) - M( 1, 2 );
280 y() = M( 0, 2 ) - M( 2, 0 );
283 z() = M( 1, 0 ) - M( 0, 1 );
291 size_t largest = diag.find_max_index();
296 T s = 0.5 / sqrt( 1.0 + M( 0, 0 ) - M( 1, 1 ) - M( 2, 2 ) );
299 y() = M( 0,1 ) + M( 1,0 );
302 z() = M( 0,2 ) + M( 2,0 );
305 w() = M( 1,2 ) - M( 2,1 );
308 else if ( largest == 1 )
310 T s = 0.5 / sqrt( 1.0 + M( 1,1 ) - M( 0,0 ) - M( 2,2 ) );
311 x() = M( 0,1 ) + M( 1,0 );
316 z() = M( 1,2 ) + M( 2,1 );
319 w() = M( 0,2 ) - M( 2,0 );
323 else if ( largest == 2 )
325 T s = 0.5 / sqrt( 1.0 + M( 2,2 ) - M( 0,0 ) - M( 1,1 ) );
326 x() = M( 0,2 ) + M( 2,0 );
329 y() = M( 1,2 ) + M( 2,1 );
334 w() = M( 0,1 ) - M( 1,0 );
339 (*this) = super::ZERO;
347 template <
typename T >
348 void quaternion< T >::zero()
350 (*this) = super::ZERO;
355 template <
typename T >
356 void quaternion< T >::identity()
363 template <
typename T >
364 void quaternion< T >::set( T xx, T yy, T zz, T ww )
374 template<
typename T >
385 template <
typename T >
386 template<
typename input_iterator_t >
387 void quaternion< T >::set( input_iterator_t begin_, input_iterator_t end_ )
389 super::template set< input_iterator_t >( begin_, end_ );
394 template <
typename T >
395 bool quaternion< T >::operator==(
const T& a )
const
397 return ( w() == a && x() == 0 && y() == 0 && z() == 0 );
402 template <
typename T >
403 bool quaternion< T >::operator!=(
const T& a )
const
405 return ( w() != a || x() != 0 || y() != 0 || z() != 0 );
409 template <
typename T >
412 return this->operator==(
413 reinterpret_cast< const quaternion< T >&
>( a )
418 template <
typename T >
422 return ! this->operator==( a );
427 template <
typename T >
429 quaternion< T >::operator==(
const quaternion& a )
const
441 template <
typename T >
443 quaternion< T >::operator!=(
const quaternion& a )
const
445 return ! this->operator==( a );
450 template <
typename T >
452 quaternion< T >::is_akin(
const quaternion& a,
const T& delta )
454 if( fabsf( w() - a.w() ) > delta ||
455 fabsf( x() - a.x() ) > delta ||
456 fabsf( y() - a.y() ) > delta ||
457 fabsf( z() - a.z() ) > delta
465 template <
typename T >
466 void quaternion< T >::conjugate()
475 template <
typename T >
476 quaternion< T > quaternion< T >::get_conjugate()
const
478 return quaternion< T > ( -x(), -y(), -z(), w() );
483 template <
typename T >
485 quaternion< T >::abs()
const
487 return sqrt( squared_abs() );
492 template <
typename T >
493 T quaternion< T >::squared_abs()
const
495 return x() * x() + y() * y() + z() * z() + w() * w();
500 template <
typename T >
501 quaternion< T > quaternion< T >::inverse()
503 quaternion< T > q( *
this );
506 T tmp = squared_abs();
507 tmp =
static_cast< T
>( 1.0 ) / tmp;
513 template <
typename T >
514 T quaternion< T >::normalize()
520 this->operator*=( len );
526 template <
typename T >
528 quaternion< T >::get_normalized()
const
530 quaternion< T > q( *
this );
541 template <
typename T >
543 quaternion< T >::operator+(
const quaternion< T >& a )
const
545 return quaternion( x() + a.x(), y() + a.y(), z() + a.z(), w() + a.w() );
550 template <
typename T >
552 quaternion< T >::operator-(
const quaternion< T >& a )
const
554 return quaternion( x() - a.x(), y() - a.y(), z() - a.z(), w() - a.w() );
560 template <
typename T >
562 quaternion< T >::operator*(
const quaternion< T >& a )
const
564 quaternion< T > ret( *
this );
572 template <
typename T >
573 void quaternion< T >::operator*=(
const quaternion< T >& q )
576 quaternion< T > orig( *
this );
577 x() = orig.w() * a.x() + orig.x() * a.w() + orig.y() * a.z() - orig.z() * a.y();
578 y() = orig.w() * a.y() + orig.y() * a.w() + orig.z() * a.x() - orig.x() * a.z();
579 z() = orig.w() * a.z() + orig.z() * a.w() + orig.x() * a.y() - orig.y() * a.x();
580 w() = orig.w() * a.w() - orig.x() * a.x() - orig.y() * a.y() - orig.z() * a.z();
586 T* _array = super::array;
588 const T& a = _array[ 3 ];
589 const T& b = _array[ 0 ];
590 const T& c = _array[ 1 ];
591 const T& d = _array[ 2 ];
592 const T& _x = q.array[ 3 ];
593 const T& _y = q.array[ 0 ];
594 const T& _z = q.array[ 1 ];
595 const T& _w = q.array[ 2 ];
597 const T tmp_00 = (d - c) * (_z - _w);
598 const T tmp_01 = (a + b) * (_x + _y);
599 const T tmp_02 = (a - b) * (_z + _w);
600 const T tmp_03 = (c + d) * (_x - _y);
601 const T tmp_04 = (d - b) * (_y - _z);
602 const T tmp_05 = (d + b) * (_y + _z);
603 const T tmp_06 = (a + c) * (_x - _w);
604 const T tmp_07 = (a - c) * (_x + _w);
605 const T tmp_08 = tmp_05 + tmp_06 + tmp_07;
606 const T tmp_09 = 0.5 * (tmp_04 + tmp_08);
608 _array[ 3 ] = tmp_00 + tmp_09 - tmp_05;
609 _array[ 0 ] = tmp_01 + tmp_09 - tmp_08;
610 _array[ 1 ] = tmp_02 + tmp_09 - tmp_07;
611 _array[ 2 ] = tmp_03 + tmp_09 - tmp_06;
620 template <
typename T >
622 quaternion< T >::operator-()
const
624 return quaternion( -x(), -y(), -z(), -w() );
629 template <
typename T >
630 void quaternion< T >::operator+=(
const quaternion< T >& q )
632 array[ 0 ] += q.array[ 0 ];
633 array[ 1 ] += q.array[ 1 ];
634 array[ 2 ] += q.array[ 2 ];
635 array[ 3 ] += q.array[ 3 ];
640 template <
typename T >
641 void quaternion< T >::operator-=(
const quaternion< T >& q )
643 array[ 0 ] -= q.array[ 0 ];
644 array[ 1 ] -= q.array[ 1 ];
645 array[ 2 ] -= q.array[ 2 ];
646 array[ 3 ] -= q.array[ 3 ];
655 template <
typename T >
657 quaternion< T >::operator*(
const T a )
const
659 return quaternion( x() * a, y() * a, z() * a, w() * a );
664 template <
typename T >
666 quaternion< T >::operator/( T a )
const
670 VMMLIB_ERROR(
"Division by zero.", VMMLIB_HERE );
673 return quaternion( x() * a, y() * a, z() * a, w() * a );
678 template <
typename T >
679 void quaternion< T >::operator*=( T q )
689 template <
typename T >
690 void quaternion< T >::operator/=( T q )
694 VMMLIB_ERROR(
"Division by zero", VMMLIB_HERE );
697 this->operator*=( q );
703 template <
typename T >
707 return quaternion( x() + a.x(), y() + a.y(), z() + a.z(), w() );
712 template <
typename T >
716 return quaternion( w(), x() - a.x(), y() - a.y(), z() - a.z() );
721 template <
typename T >
725 return quaternion( -x() * a.x() - y() * a.y() - z() * a.z(),
726 w() * a.x() + y() * a.z() - z() * a.y(),
727 w() * a.y() + z() * a.x() - x() * a.z(),
728 w() * a.z() + x() * a.y() - y() * a.x() );
733 template <
typename T >
743 template <
typename T >
754 template <
typename T >
762 x() = _w * a.x() + _y * a.z() - _z * a.y();
763 y() = _w * a.y() + _z * a.x() - _x * a.z();
764 z() = _w * a.z() + _x * a.y() - _y * a.x();
765 w() = -_x * a.x() - _y * a.y() - _z * a.z();
771 template <
typename T >
772 vector< 3, T > quaternion< T >::cross(
const quaternion< T >& bb )
const
776 result.array[ 0 ] = y() * bb.z() - z() * bb.y();
777 result.array[ 1 ] = z() * bb.x() - x() * bb.z();
778 result.array[ 2 ] = x() * bb.y() - y() * bb.x();
785 template <
typename T >
786 T quaternion< T >::dot(
const quaternion< T >& q )
const
788 return w() * q.w() + x() * q.x() + y() * q.y() + z() * q.z();
793 template <
typename T >
795 dot(
const quaternion< T >& p,
const quaternion< T >& q )
797 return p.w() * q.w() + p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
802 template <
typename T >
803 void quaternion< T >::normal(
const quaternion< T >& aa,
804 const quaternion< T >& bb,
805 const quaternion< T >& cc,
806 const quaternion< T >& dd )
809 const quaternion< T > quat_t = bb - aa;
810 const quaternion< T > quat_u = cc - aa;
811 const quaternion< T > quat_v = dd - aa;
820 template <
typename T >
821 quaternion< T > quaternion< T >::normal(
const quaternion< T >& aa,
822 const quaternion< T >& bb,
823 const quaternion< T >& cc )
826 tmp.normal( *
this, aa, bb, cc );
834 template<
typename T >
835 quaternion< T > quaternion< T >::rotate( T theta,
vector< 3, T >& axis,
838 quaternion< T > p = a;
840 quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * axis.normalize() );
841 return q * p * q.invert();
846 template<
typename T >
847 quaternion< T > quaternion< T >::rotate_x( T theta,
const vector< 3, T >& a )
849 quaternion< T > p = a;
851 quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * QUATERI );
852 return q * p * q.invert();
857 template<
typename T >
858 quaternion< T > quaternion< T >::rotate_y( T theta,
const vector< 3, T >& a )
860 quaternion< T > p = a;
862 quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * QUATERJ );
863 return q * p * q.invert();
868 template<
typename T >
869 quaternion< T > quaternion< T >::rotate_z( T theta,
const vector< 3, T >& a )
871 quaternion< T > p = a;
873 quaternion< T > q = std::cos( alpha ) + ( std::sin( alpha ) * QUATERK );
874 return q * p * q.invert();
879 template <
typename T >
881 quaternion< T >::get_rotation_matrix()
const
883 matrix< 3, 3, T > result;
884 get_rotation_matrix< 3 >( result );
890 template <
typename T >
template<
size_t D >
891 void quaternion< T >::get_rotation_matrix( matrix< D, D, T >& M )
const
904 M( 0, 0 ) = w2 + x2 - y2 - z2;
905 M( 0, 1 ) = 2. * (xy - wz);
906 M( 0, 2 ) = 2. * (xz + wy);
907 M( 1, 0 ) = 2. * (xy + wz);
908 M( 1, 1 ) = w2 - x2 + y2 - z2;
909 M( 1, 2 ) = 2. * (yz - wx);
910 M( 2, 0 ) = 2. * (xz - wy);
911 M( 2, 1 ) = 2. * (yz + wx);
912 M( 2, 2 ) = w2 - x2 - y2 + z2;
916 template<
typename T >
917 quaternion< T > quaternion< T >::
918 slerp( T a,
const quaternion< T >& p,
const quaternion< T >& q,
const T epsilon )
920 quaternion< T > px = p.get_normalized();
921 quaternion< T > qx = q.get_normalized();
923 T cosine = px.dot( qx );
932 const T abs_cos =
static_cast< T
>( fabs(cosine) );
933 const T one_x =
static_cast< T
>( 1. - epsilon );
934 if( abs_cos < one_x )
937 T sine = sqrt( 1. - ( cosine * cosine ) );
938 T angle = atan2( sine, cosine );
939 T coeff1 = std::sin( ( 1.0 - a ) * angle) / sine;
940 T coeff2 = std::sin( a * angle ) / sine;
960 template <
typename T >
961 quaternion< T >& quaternion< T >::operator=(
const quaternion& other)
963 memcpy( array, other.array, 4 *
sizeof( T ) );
968 template <
typename T >
972 memcpy( array, other.array, 4 *
sizeof( T ) );