31 template<
typename T,
typename Y>
33 typedef typename T::TYPE
F;
34 static F getW(
const Y &
v ) {
return static_cast<F>( v.w ); }
35 static F getX(
const Y &
v ) {
return static_cast<F>( v.x ); }
36 static F getY(
const Y &
v ) {
return static_cast<F>( v.y ); }
37 static F getZ(
const Y &
v ) {
return static_cast<F>( v.z ); }
51 template<
typename FromT>
59 Quaternion( T xRotation, T yRotation, T zRotation ) {
set( xRotation, yRotation, zRotation ); }
71 T invLen =
static_cast<T
>( 1.0 ) /
math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
100 return w * quat.
w +
v.dot( quat.
v );
105 return (T)std::sqrt(
w*
w +
v.lengthSquared() );
110 return w *
w +
v.lengthSquared();
115 T norm =
w *
w +
v.x *
v.x +
v.y *
v.y +
v.z *
v.z;
121 T normRecip =
static_cast<T
>( 1.0f ) / norm;
122 return Quaternion<T>( normRecip *
w, -normRecip *
v.x, -normRecip *
v.y, -normRecip *
v.z );
132 w =
static_cast<T
>( 1.0 );
146 result.
w =
static_cast<T
>( 1.0 );
165 if ( abs( sintheta ) < 1 && abs( theta ) >= 3.402823466e+38F * abs( sintheta ) )
168 k = theta / sintheta;
182 if( abs( theta ) < 1 && abs( sintheta ) >= 3.402823466e+38F * abs( theta ) )
185 k = sintheta / theta;
194 T qdot = this->
dot( *
this );
200 T qdot = this->
dot( *
this );
201 set( -
v / qdot,
w / qdot );
215 set( from.
dot( to ), axis.
x, axis.
y, axis.
z );
218 w +=
static_cast<T
>( 1.0 );
221 if ( from.
z * from.
z > from.
x * from.
x ) {
222 set( static_cast<T>( 0.0 ), static_cast<T>( 0.0 ), from.
z, -from.
y );
225 set( static_cast<T>( 0.0 ), from.
y, -from.
x, static_cast<T>( 0.0 ) );
237 if( f0.
dot( t0 ) >= 0 ) {
238 setRotationInternal (f0, t0, *
this);
247 if( h0.dot( h0 ) != 0 ) {
248 setRotationInternal( f0, h0, *
this );
250 setRotationInternal( h0, t0, q );
252 set( q.w*
w - q.v.x*
v.x - q.v.y*
v.y - q.v.z*
v.z,
253 q.w*
v.x + q.v.x*
w + q.v.y*
v.z - q.v.z*
v.y,
254 q.w*
v.y + q.v.y*
w + q.v.z*
v.x - q.v.x*
v.z,
255 q.w*
v.z + q.v.z*
w + q.v.x*
v.y - q.v.y*
v.x );
266 Vec3<T> f02 = f0 * f0;
268 if( ( f02.x <= f02.y ) && ( f02.x <= f02.z ) )
269 v = f0.
cross( Vec3<T>( 1, 0, 0 ) ).normalized();
270 else if( f02.y <= f02.z )
271 v = f0.cross( Vec3<T>( 0, 1, 0 ) ).normalized();
273 v = f0.cross( Vec3<T>( 0, 0, 1 ) ).normalized();
278 static void setRotationInternal(
const Vec3<T> &f0,
const Vec3<T> &t0, Quaternion<T> &
q )
298 q.v = f0.cross( h0 );
309 void set( T xRotation, T yRotation, T zRotation )
311 zRotation *= T( 0.5 );
312 yRotation *= T( 0.5 );
313 xRotation *= T( 0.5 );
326 w = Cx*Cy*Cz - Sx*Sy*Sz;
327 v.x = Sx*Cy*Cz + Cx*Sy*Sz;
328 v.y = Cx*Sy*Cz - Sx*Cy*Sz;
329 v.z = Cx*Cy*Sz + Sx*Sy*Cx;
336 T invLen =
static_cast<T
>( 1.0 ) /
math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
338 axis->
x =
v.x * invLen;
339 axis->
y =
v.y * invLen;
340 axis->
z =
v.z * invLen;
346 T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
361 mV[0] = T( 1 ) - ( yy + zz );
366 mV[4] = T( 1 ) - ( xx + zz );
371 mV[8] = T( 1 ) - ( xx + yy );
379 T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
394 mV[0] = T( 1 ) - ( yy + zz );
400 mV[5] = T( 1 ) - ( xx + zz );
406 mV[10] = T( 1 ) - ( xx + yy );
422 float cosTheta =
dot( end );
430 result += *
this * (
static_cast<T
>( 1.0 ) - t );
434 result += *
this * ( t -
static_cast<T
>( 1.0 ) );
468 T cosTheta = this->
dot( end );
469 T startInterp, endInterp;
474 if( ( static_cast<T>( 1.0 ) - cosTheta ) >
EPSILON ) {
477 T recipSinTheta =
static_cast<T
>( 1.0 ) /
math<T>::sin( theta );
479 startInterp =
math<T>::sin( ( static_cast<T>( 1.0 ) - t ) * theta ) * recipSinTheta;
485 startInterp =
static_cast<T
>( 1.0 ) - t;
492 if( ( static_cast<T>( 1.0 ) + cosTheta ) >
EPSILON ) {
495 T recipSinTheta =
static_cast<T
>( 1.0 ) /
math<T>::sin( theta );
497 startInterp =
math<T>::sin( ( t - static_cast<T>( 1.0 ) ) * theta ) * recipSinTheta;
503 startInterp = t -
static_cast<T
>( 1.0 );
508 return *
this * startInterp + end * endInterp;
520 if( this->
dot( q2 ) >= 0 )
526 if( qa.
dot( qb ) >= 0 )
531 if( r1.
dot( r2 ) >= 0 )
541 return r1.
slerp( 2 * t * (1-t), r2 );
577 if ( trace > (T)0.0 )
581 T recip = (T)0.5 / s;
582 v.x = ( m.
at(2,1) - m.
at(1,2) ) * recip;
583 v.y = ( m.
at(0,2) - m.
at(2,0) ) * recip;
584 v.z = ( m.
at(1,0) - m.
at(0,1) ) * recip;
589 if( m.
at(1,1) > m.
at(0,0) )
591 if( m.
at(2,2) > m.
at(i,i) )
593 unsigned int j = ( i + 1 ) % 3;
594 unsigned int k = ( j + 1 ) % 3;
596 (*this)[i] = (T)0.5 * s;
597 T recip = (T)0.5 / s;
598 w = ( m.
at(k,j) - m.
at(j,k) ) * recip;
599 (*this)[j] = ( m.
at(j,i) + m.
at(i,j) ) * recip;
600 (*this)[k] = ( m.
at(k,i) + m.
at(i,k) ) * recip;
608 if ( trace > (T)0.0 )
612 T recip = (T)0.5 / s;
613 v.x = ( m.
at(2,1) - m.
at(1,2) ) * recip;
614 v.y = ( m.
at(0,2) - m.
at(2,0) ) * recip;
615 v.z = ( m.
at(1,0) - m.
at(0,1) ) * recip;
620 if( m.
at(1,1) > m.
at(0,0) )
622 if( m.
at(2,2) > m.
at(i,i) )
624 unsigned int j = ( i + 1 ) % 3;
625 unsigned int k = ( j + 1 ) % 3;
627 (*this)[i] = (T)0.5 * s;
628 T recip = (T)0.5 / s;
629 w = ( m.
at(k,j) - m.
at(j,k) ) * recip;
630 (*this)[j] = ( m.
at(j,i) + m.
at(i,j) ) * recip;
631 (*this)[k] = ( m.
at(k,i) + m.
at(i,k) ) * recip;
643 template<
typename FromT>
647 w =
static_cast<T
>( rhs.
w );
662 rhs.
w*
v.x + rhs.
v.x*
w + rhs.
v.y*
v.z - rhs.
v.z*
v.y,
663 rhs.
w*
v.y + rhs.
v.y*
w + rhs.
v.z*
v.x - rhs.
v.x*
v.z,
664 rhs.
w*
v.z + rhs.
v.z*
w + rhs.
v.x*
v.y - rhs.
v.y*
v.x );
675 T vMult = T( 2 ) * (
v.x * vec.
x +
v.y * vec.
y +
v.z * vec.
z );
676 T crossMult = T( 2 ) *
w;
677 T pMult = crossMult *
w - T( 1 );
679 return Vec3<T>( pMult * vec.
x + vMult *
v.x + crossMult * (
v.y * vec.
z -
v.z * vec.
y ),
680 pMult * vec.
y + vMult *
v.y + crossMult * (
v.z * vec.
x -
v.x * vec.
z ),
681 pMult * vec.
z + vMult *
v.z + crossMult * (
v.x * vec.
y -
v.y * vec.
x ) );
722 return ( std::fabs(lhs.
w - rhs.
w) <
EPSILON ) && lhs.
v == rhs.
v;
727 return ! (*
this == rhs);
731 inline const T&
operator[](
unsigned int i )
const {
return (&
v.x)[i]; }
739 friend std::ostream& operator <<( std::ostream &oss, const Quaternion<T> &
q )
741 oss << q.getAxis() <<
" @ " << q.getAngle() * ( (T)180 /
M_PI ) <<
"deg";
765 T vMult = T( 2 ) * ( q.
v.x * vec.
x + q.
v.y * vec.
y + q.
v.z * vec.
z );
766 T crossMult = T( 2 ) * q.
w;
767 T pMult = crossMult * q.
w - T( 1 );
769 return Vec3<T>( pMult * vec.
x + vMult * q.
v.x + crossMult * ( q.
v.y * vec.
z - q.
v.z * vec.
y ),
770 pMult * vec.
y + vMult * q.
v.y + crossMult * ( q.
v.z * vec.
x - q.
v.x * vec.
z ),
771 pMult * vec.
z + vMult * q.
v.z + crossMult * ( q.
v.x * vec.
y - q.
v.y * vec.
x ) );
static T sqrt(T x)
Definition: CinderMath.h:63
Quaternion< double > Quatd
Definition: Quaternion.h:775
Quaternion< T > slerp(T t, const Quaternion< T > &end) const
Definition: Quaternion.h:465
void set(T xRotation, T yRotation, T zRotation)
Definition: Quaternion.h:309
const T & operator[](unsigned int i) const
Definition: Quaternion.h:731
GLenum GLint GLint y
Definition: GLee.h:987
Quaternion(T xRotation, T yRotation, T zRotation)
Definition: Quaternion.h:59
Definition: CinderMath.h:40
T dot(const Quaternion< T > &quat) const
Definition: Quaternion.h:98
static T cos(T x)
Definition: CinderMath.h:46
Quaternion(const Vec3< T > &from, const Vec3< T > &to)
Definition: Quaternion.h:57
T trace() const
Definition: Matrix33.h:664
T sinx_over_x(T x)
Definition: CinderMath.h:191
T dot(const Vec3< T > &rhs) const
Definition: Vector.h:418
void set(const Vec3< T > &from, const Vec3< T > &to)
Definition: Quaternion.h:211
T getAngle() const
Definition: Quaternion.h:77
static T sin(T x)
Definition: CinderMath.h:47
Quaternion< T > & operator=(const Quaternion< T > &rhs)
Definition: Quaternion.h:636
T::TYPE F
Definition: Quaternion.h:33
T & at(int row, int col)
Definition: Matrix33.h:501
T getPitch() const
Definition: Quaternion.h:83
void set(const Vec3< T > &axis, T radians)
Definition: Quaternion.h:302
#define EPSILON
Definition: CinderMath.h:125
T z
Definition: Vector.h:321
Vec3< T > getAxis() const
Definition: Quaternion.h:68
T x
Definition: Vector.h:321
static Quaternion< T > identity()
Definition: Quaternion.h:733
Quaternion< T > exp() const
Definition: Quaternion.h:176
T y
Definition: Vector.h:321
bool operator!=(const Quaternion< T > &rhs) const
Definition: Quaternion.h:725
Quaternion< T > & operator=(const Quaternion< FromT > &rhs)
Definition: Quaternion.h:644
Quaternion< T > lerp(T t, const Quaternion< T > &end) const
Definition: Quaternion.h:419
Quaternion(T aW, T x, T y, T z)
Definition: Quaternion.h:54
void invert()
Definition: Quaternion.h:198
ColorT< T > operator*(Y s, const ColorT< T > &c)
Definition: Color.h:391
T trace(bool fullTrace=false) const
Definition: Matrix44.h:850
Quaternion< T > squadShortestEnforced(T t, const Quaternion< T > &qa, const Quaternion< T > &qb, const Quaternion< T > &q2) const
Definition: Quaternion.h:517
Quaternion(const Y &v)
Definition: Quaternion.h:63
#define min(a, b)
Definition: AppImplMsw.cpp:36
void getAxisAngle(Vec3< T > *axis, T *radians) const
Definition: Quaternion.h:332
T & operator[](unsigned int i)
Definition: Quaternion.h:730
Definition: Quaternion.h:41
bool operator==(const Quaternion< T > &rhs) const
Definition: Quaternion.h:719
const Vec3< T > operator*(const Vec3< T > &vec) const
Definition: Quaternion.h:673
Quaternion< float > Quatf
Definition: Quaternion.h:774
Vec3< T > v
Definition: Quaternion.h:47
Quaternion(const Matrix44< T > &m)
Definition: Quaternion.h:61
Quaternion< T > spline(T t, const Quaternion< T > &q1, const Quaternion< T > &q2, const Quaternion< T > &q3) const
Definition: Quaternion.h:563
static T atan2(T y, T x)
Definition: CinderMath.h:45
Quaternion< T > log() const
Definition: Quaternion.h:155
GLdouble GLdouble GLdouble GLdouble q
Definition: GLee.h:1522
static T acos(T x)
Definition: CinderMath.h:42
T w
Definition: Quaternion.h:48
GLenum GLint x
Definition: GLee.h:987
const Quaternion< T > operator+(const Quaternion< T > &rhs) const
Definition: Quaternion.h:651
T getRoll() const
Definition: Quaternion.h:93
T & at(int row, int col)
Definition: Matrix44.h:643
Quaternion< T > normalized() const
Definition: Quaternion.h:137
const GLdouble * v
Definition: GLee.h:1384
Quaternion< T > squad(T t, const Quaternion< T > &qa, const Quaternion< T > &qb, const Quaternion< T > &q2) const
Definition: Quaternion.h:537
void normalize()
Definition: Quaternion.h:125
GLdouble GLdouble z
Definition: GLee.h:1911
T value_type
Definition: Quaternion.h:45
Matrix33< T > toMatrix33() const
Definition: Quaternion.h:343
GLuint GLuint end
Definition: GLee.h:963
static F getY(const Y &v)
Definition: Quaternion.h:36
T TYPE
Definition: Quaternion.h:44
void set(T aW, T x, T y, T z)
Definition: Quaternion.h:204
Quaternion< T > & operator-=(const Quaternion< T > &rhs)
Definition: Quaternion.h:697
#define M_PI
Definition: CinderMath.h:121
static F getX(const Y &v)
Definition: Quaternion.h:35
Definition: Matrix44.h:41
const Quaternion< T > operator*(const Quaternion< T > &rhs) const
Definition: Quaternion.h:659
Quaternion< T > inverted() const
Definition: Quaternion.h:192
Vec3< T > cross(const Vec3< T > &rhs) const
Definition: Vector.h:423
void set(const Matrix33< T > &m)
Definition: Quaternion.h:573
GLboolean GLboolean GLboolean GLboolean a
Definition: GLee.h:2964
T lengthSquared() const
Definition: Quaternion.h:108
Quaternion(const Quaternion< FromT > &q)
Definition: Quaternion.h:52
Quaternion< T > inverse() const
Definition: Quaternion.h:113
GLenum GLsizei len
Definition: GLee.h:4413
Quaternion()
Definition: Quaternion.h:50
Quaternion< T > & operator+=(const Quaternion< T > &rhs)
Definition: Quaternion.h:690
GLubyte GLubyte GLubyte GLubyte w
Definition: GLee.h:2685
void set(const Matrix44< T > &m)
Definition: Quaternion.h:604
Quaternion< T > & operator*=(T rhs)
Definition: Quaternion.h:712
static F getZ(const Y &v)
Definition: Quaternion.h:37
T getYaw() const
Definition: Quaternion.h:88
const Quaternion< T > operator*(T rhs) const
Definition: Quaternion.h:667
const GLfloat * m
Definition: GLee.h:13493
GLdouble GLdouble t
Definition: GLee.h:1426
const Quaternion< T > operator-(const Quaternion< T > &rhs) const
Definition: Quaternion.h:684
GLdouble s
Definition: GLee.h:1378
Quaternion(const Matrix33< T > &m)
Definition: Quaternion.h:60
Quaternion< T > slerpShortestUnenforced(T t, const Quaternion< T > &end) const
Definition: Quaternion.h:448
T length() const
Definition: Quaternion.h:103
static F getW(const Y &v)
Definition: Quaternion.h:34
Definition: Quaternion.h:32
Matrix44< T > toMatrix44() const
Definition: Quaternion.h:376
Definition: Matrix33.h:37
Quaternion< T > & operator*=(const Quaternion< T > &rhs)
Definition: Quaternion.h:704
Vec3< T > normalized() const
Definition: Vector.h:492
Quaternion(const Vec3< T > &axis, T radians)
Definition: Quaternion.h:56