Cinder  0.8.6
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
Quaternion.h
Go to the documentation of this file.
1 /*
2  Copyright (c) 2010, The Barbarian Group
3  All rights reserved.
4 
5  Redistribution and use in source and binary forms, with or without modification, are permitted provided that
6  the following conditions are met:
7 
8  * Redistributions of source code must retain the above copyright notice, this list of conditions and
9  the following disclaimer.
10  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
11  the following disclaimer in the documentation and/or other materials provided with the distribution.
12 
13  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
14  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
15  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
16  ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
17  TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
18  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
19  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
20  POSSIBILITY OF SUCH DAMAGE.
21 */
22 
23 #pragma once
24 
25 #include "cinder/CinderMath.h"
26 #include "cinder/Vector.h"
27 #include "cinder/Matrix.h"
28 
29 namespace cinder {
30 
31 template<typename T, typename Y>
32 struct QUATCONV {
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 ); }
38 };
39 
40 template<typename T>
41 class Quaternion
42 {
43 public:
44  typedef T TYPE;
45  typedef T value_type;
46 
47  Vec3<T> v; // axisOfRotation.normalized() * sin( angleOfRotation / 2 )
48  T w; // cos( angleOfRotation / 2 )
49 
50  Quaternion(): w( 1 ), v( 0, 0, 0 ){} // default constructor is identity quat
51  template<typename FromT>
52  Quaternion( const Quaternion<FromT>& q ) : w( static_cast<T>( q.w ) ), v( q.v ) {}
53 
54  Quaternion( T aW, T x, T y, T z ): w( aW ), v( x, y, z ) {}
55  // construct from axis-angle
56  Quaternion( const Vec3<T> &axis, T radians ) { set( axis, radians ); }
57  Quaternion( const Vec3<T> &from, const Vec3<T> &to ) { set( from, to ); }
58  // create from Euler angles in radians expressed in ZYX rotation order
59  Quaternion( T xRotation, T yRotation, T zRotation ) { set( xRotation, yRotation, zRotation ); }
60  Quaternion( const Matrix33<T> &m ) { set( m ); }
61  Quaternion( const Matrix44<T> &m ) { set( m ); }
62  template<typename Y>
63  explicit Quaternion( const Y &v )
64  : w( QUATCONV<Quaternion<typename Quaternion::TYPE>,Y>::getW( v ) ), v( QUATCONV<typename Quaternion::TYPE,Y>::getX( v ), QUATCONV<typename Quaternion::TYPE,Y>::getY( v ), QUATCONV<typename Quaternion::TYPE,Y>::getZ( v ) )
65  {}
66 
67  // get axis-angle representation's axis
68  Vec3<T> getAxis() const
69  {
70  T cos_angle = w;
71  T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
72 
73  return v * invLen;
74  }
75 
76  // get axis-angle representation's angle in radians
77  T getAngle() const
78  {
79  T cos_angle = w;
80  return math<T>::acos( cos_angle ) * 2;
81  }
82 
83  T getPitch() const
84  {
85  return math<T>::atan2( (T)2 * ( v.y * v.z + w * v.x ), w * w - v.x * v.x - v.y * v.y + v.z * v.z );
86  }
87 
88  T getYaw() const
89  {
90  return math<T>::sin( (T)-2 * ( v.x * v.z - w * v.y ) );
91  }
92 
93  T getRoll() const
94  {
95  return math<T>::atan2( (T)2 * ( v.x * v.y + w * v.z), w * w + v.x * v.x - v.y * v.y - v.z * v.z );
96  }
97 
98  T dot( const Quaternion<T> &quat ) const
99  {
100  return w * quat.w + v.dot( quat.v );
101  }
102 
103  T length() const
104  {
105  return (T)std::sqrt( w*w + v.lengthSquared() );
106  }
107 
108  T lengthSquared() const
109  {
110  return w * w + v.lengthSquared();
111  }
112 
114  {
115  T norm = w * w + v.x * v.x + v.y * v.y + v.z * v.z;
116  // if we're the zero quaternion, just return identity
117  /*if( ! ( math<T>::abs( norm ) < EPSILON_VALUE ) ) {
118  return identity();
119  }*/
120 
121  T normRecip = static_cast<T>( 1.0f ) / norm;
122  return Quaternion<T>( normRecip * w, -normRecip * v.x, -normRecip * v.y, -normRecip * v.z );
123  }
124 
125  void normalize()
126  {
127  if( T len = length() ) {
128  w /= len;
129  v /= len;
130  }
131  else {
132  w = static_cast<T>( 1.0 );
133  v = Vec3<T>( 0, 0, 0 );
134  }
135  }
136 
138  {
139  Quaternion<T> result = *this;
140 
141  if( T len = length() ) {
142  result.w /= len;
143  result.v /= len;
144  }
145  else {
146  result.w = static_cast<T>( 1.0 );
147  result.v = Vec3<T>( 0, 0, 0 );
148  }
149 
150  return result;
151  }
152 
153  // For unit quaternion, from Advanced Animation and
154  // Rendering Techniques by Watt and Watt, Page 366:
156  {
157  T theta = math<T>::acos( std::min( w, (T) 1.0 ) );
158 
159  if( theta == 0 )
160  return Quaternion<T>( v, 0 );
161 
162  T sintheta = math<T>::sin( theta );
163 
164  T k;
165  if ( abs( sintheta ) < 1 && abs( theta ) >= 3.402823466e+38F * abs( sintheta ) )
166  k = 1;
167  else
168  k = theta / sintheta;
169 
170  return Quaternion<T>( (T)0, v.x * k, v.y * k, v.z * k );
171  }
172 
173  // For pure quaternion (zero scalar part):
174  // from Advanced Animation and Rendering
175  // Techniques by Watt and Watt, Page 366:
177  {
178  T theta = v.length();
179  T sintheta = math<T>::sin( theta );
180 
181  T k;
182  if( abs( theta ) < 1 && abs( sintheta ) >= 3.402823466e+38F * abs( theta ) )
183  k = 1;
184  else
185  k = sintheta / theta;
186 
187  T costheta = math<T>::cos( theta );
188 
189  return Quaternion<T>( costheta, v.x * k, v.y * k, v.z * k );
190  }
191 
193  {
194  T qdot = this->dot( *this );
195  return Quaternion( -v / qdot, w / qdot );
196  }
197 
198  void invert()
199  {
200  T qdot = this->dot( *this );
201  set( -v / qdot, w / qdot );
202  }
203 
204  void set( T aW, T x, T y, T z )
205  {
206  w = aW;
207  v = Vec3<T>( x, y, z );
208  }
209 
210 #if 1
211  void set( const Vec3<T> &from, const Vec3<T> &to )
212  {
213  Vec3<T> axis = from.cross( to );
214 
215  set( from.dot( to ), axis.x, axis.y, axis.z );
216  normalize();
217 
218  w += static_cast<T>( 1.0 );
219 
220  if( w <= EPSILON ) {
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 );
223  }
224  else {
225  set( static_cast<T>( 0.0 ), from.y, -from.x, static_cast<T>( 0.0 ) );
226  }
227  }
228 
229  normalize();
230  }
231 #else
232  void set( const Vec3<T> &from, const Vec3<T> &to )
233  {
234  Vec3<T> f0 = from.normalized();
235  Vec3<T> t0 = to.normalized();
236 
237  if( f0.dot( t0 ) >= 0 ) { // The rotation angle is less than or equal to pi/2.
238  setRotationInternal (f0, t0, *this);
239  }
240  else {
241  // The angle is greater than pi/2. After computing h0,
242  // which is halfway between f0 and t0, we rotate first
243  // from f0 to h0, then from h0 to t0.
244 
245  Vec3<T> h0 = (f0 + t0).normalized();
246 
247  if( h0.dot( h0 ) != 0 ) {
248  setRotationInternal( f0, h0, *this );
249  Quaternion<T> q;
250  setRotationInternal( h0, t0, q );
251 
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 );
256 
257  //*this *= q;
258  }
259  else {
260  // f0 and t0 point in exactly opposite directions.
261  // Pick an arbitrary axis that is orthogonal to f0,
262  // and rotate by pi.
263 
264  w = T( 0 );
265 
266  Vec3<T> f02 = f0 * f0;
267 
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();
272  else
273  v = f0.cross( Vec3<T>( 0, 0, 1 ) ).normalized();
274  }
275  }
276  }
277 
278  static void setRotationInternal( const Vec3<T> &f0, const Vec3<T> &t0, Quaternion<T> &q )
279  {
280  //
281  // The following is equivalent to setAxisAngle(n,2*phi),
282  // where the rotation axis, is orthogonal to the f0 and
283  // t0 vectors, and 2*phi is the angle between f0 and t0.
284  //
285  // This function is called by setRotation(), above; it assumes
286  // that f0 and t0 are normalized and that the angle between
287  // them is not much greater than pi/2. This function becomes
288  // numerically inaccurate if f0 and t0 point into nearly
289  // opposite directions.
290  //
291 
292  // Find a normalized vector, h0, that is half way between f0 and t0.
293  // The angle between f0 and h0 is phi.
294  Vec3<T> h0 = ( f0 + t0 ).normalized();
295 
296  // Store the rotation axis and rotation angle.
297  q.w = f0.dot( h0 ); // f0 ^ h0 == cos (phi)
298  q.v = f0.cross( h0 ); // (f0 % h0).length() == sin (phi)
299  }
300 #endif
301 
302  void set( const Vec3<T> &axis, T radians )
303  {
304  w = math<T>::cos( radians / 2 );
305  v = axis.normalized() * math<T>::sin( radians / 2 );
306  }
307 
308  // assumes ZYX rotation order and radians
309  void set( T xRotation, T yRotation, T zRotation )
310  {
311  zRotation *= T( 0.5 );
312  yRotation *= T( 0.5 );
313  xRotation *= T( 0.5 );
314 
315  // get sines and cosines of half angles
316  T Cx = math<T>::cos( xRotation );
317  T Sx = math<T>::sin( xRotation );
318 
319  T Cy = math<T>::cos( yRotation );
320  T Sy = math<T>::sin( yRotation );
321 
322  T Cz = math<T>::cos( zRotation );
323  T Sz = math<T>::sin( zRotation );
324 
325  // multiply it out
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;
330  }
331 
332  void getAxisAngle( Vec3<T> *axis, T *radians ) const
333  {
334  T cos_angle = w;
335  *radians = math<T>::acos( cos_angle ) * 2;
336  T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle );
337 
338  axis->x = v.x * invLen;
339  axis->y = v.y * invLen;
340  axis->z = v.z * invLen;
341  }
342 
344  {
345  Matrix33<T> mV;
346  T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
347 
348  xs = v.x + v.x;
349  ys = v.y + v.y;
350  zs = v.z + v.z;
351  wx = w * xs;
352  wy = w * ys;
353  wz = w * zs;
354  xx = v.x * xs;
355  xy = v.x * ys;
356  xz = v.x * zs;
357  yy = v.y * ys;
358  yz = v.y * zs;
359  zz = v.z * zs;
360 
361  mV[0] = T( 1 ) - ( yy + zz );
362  mV[3] = xy - wz;
363  mV[6] = xz + wy;
364 
365  mV[1] = xy + wz;
366  mV[4] = T( 1 ) - ( xx + zz );
367  mV[7] = yz - wx;
368 
369  mV[2] = xz - wy;
370  mV[5] = yz + wx;
371  mV[8] = T( 1 ) - ( xx + yy );
372 
373  return mV;
374  }
375 
377  {
378  Matrix44<T> mV;
379  T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
380 
381  xs = v.x + v.x;
382  ys = v.y + v.y;
383  zs = v.z + v.z;
384  wx = w * xs;
385  wy = w * ys;
386  wz = w * zs;
387  xx = v.x * xs;
388  xy = v.x * ys;
389  xz = v.x * zs;
390  yy = v.y * ys;
391  yz = v.y * zs;
392  zz = v.z * zs;
393 
394  mV[0] = T( 1 ) - ( yy + zz );
395  mV[4] = xy - wz;
396  mV[8] = xz + wy;
397  mV[12] = 0;
398 
399  mV[1] = xy + wz;
400  mV[5] = T( 1 ) - ( xx + zz );
401  mV[9] = yz - wx;
402  mV[13] = 0;
403 
404  mV[2] = xz - wy;
405  mV[6] = yz + wx;
406  mV[10] = T( 1 ) - ( xx + yy );
407  mV[14] = 0;
408 
409  mV[3] = 0;
410  mV[7] = 0;
411  mV[11] = 0;
412  mV[15] = T( 1 );
413 
414  return mV;
415  }
416 
417  operator Matrix44<T>() const { return toMatrix44(); }
418 
419  Quaternion<T> lerp( T t, const Quaternion<T> &end ) const
420  {
421  // get cos of "angle" between quaternions
422  float cosTheta = dot( end );
423 
424  // initialize result
425  Quaternion<T> result = end * t;
426 
427  // if "angle" between quaternions is less than 90 degrees
428  if( cosTheta >= EPSILON ) {
429  // use standard interpolation
430  result += *this * ( static_cast<T>( 1.0 ) - t );
431  }
432  else {
433  // otherwise, take the shorter path
434  result += *this * ( t - static_cast<T>( 1.0 ) );
435  }
436 
437  return result;
438  }
439 
440  // This method does *not* interpolate along the shortest
441  // arc between q1 and q2. If you desire interpolation
442  // along the shortest arc, and q1.dot( q2 ) is negative, then
443  // consider flipping the second quaternion explicitly.
444  //
445  // NOTE: the difference between this and slerp isn't clear, but we're using
446  // the Don Hatch / ilmbase squad code which explicity requires this impl. of slerp
447  // so I'm leaving it for now
449  {
450  Quaternion<T> d = *this - end;
451  T lengthD = math<T>::sqrt( this->dot( end ) );
452 
453  Quaternion<T> st = *this + end;
454  T lengthS = math<T>::sqrt( st.dot( st ) );
455 
456  T a = 2 * math<T>::atan2( lengthD, lengthS );;
457  T s = 1 - t;
458 
459  Quaternion<T> q( *this * ( sinx_over_x( s * a ) / sinx_over_x( a ) * s ) +
460  end * ( sinx_over_x( t * a ) / sinx_over_x( a ) * t ) );
461 
462  return q.normalized();
463  }
464 
465  Quaternion<T> slerp( T t, const Quaternion<T> &end ) const
466  {
467  // get cosine of "angle" between quaternions
468  T cosTheta = this->dot( end );
469  T startInterp, endInterp;
470 
471  // if "angle" between quaternions is less than 90 degrees
472  if( cosTheta >= EPSILON ) {
473  // if angle is greater than zero
474  if( ( static_cast<T>( 1.0 ) - cosTheta ) > EPSILON ) {
475  // use standard slerp
476  T theta = math<T>::acos( cosTheta );
477  T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta );
478 
479  startInterp = math<T>::sin( ( static_cast<T>( 1.0 ) - t ) * theta ) * recipSinTheta;
480  endInterp = math<T>::sin( t * theta ) * recipSinTheta;
481  }
482  // angle is close to zero
483  else {
484  // use linear interpolation
485  startInterp = static_cast<T>( 1.0 ) - t;
486  endInterp = t;
487  }
488  }
489  // otherwise, take the shorter route
490  else {
491  // if angle is less than 180 degrees
492  if( ( static_cast<T>( 1.0 ) + cosTheta ) > EPSILON ) {
493  // use slerp w/negation of start quaternion
494  T theta = math<T>::acos( -cosTheta );
495  T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta );
496 
497  startInterp = math<T>::sin( ( t - static_cast<T>( 1.0 ) ) * theta ) * recipSinTheta;
498  endInterp = math<T>::sin( t * theta ) * recipSinTheta;
499  }
500  // angle is close to 180 degrees
501  else {
502  // use lerp w/negation of start quaternion
503  startInterp = t - static_cast<T>( 1.0 );
504  endInterp = t;
505  }
506  }
507 
508  return *this * startInterp + end * endInterp;
509  }
510 
511  // Spherical Quadrangle Interpolation -
512  // from Advanced Animation and Rendering
513  // Techniques by Watt and Watt, Page 366:
514  // It constructs a spherical cubic interpolation as
515  // a series of three spherical linear interpolations
516  // of a quadrangle of unit quaternions.
517  Quaternion<T> squadShortestEnforced( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const
518  {
519  Quaternion<T> r1;
520  if( this->dot( q2 ) >= 0 )
521  r1 = this->slerpShortestUnenforced( t, q2 );
522  else
523  r1 = this->slerpShortestUnenforced( t, q2.inverted() );
524 
525  Quaternion<T> r2;
526  if( qa.dot( qb ) >= 0 )
527  r2 = qa.slerpShortestUnenforced( t, qb );
528  else
529  r2 = qa.slerpShortestUnenforced( t, qb.inverted() );
530 
531  if( r1.dot( r2 ) >= 0 )
532  return r1.slerpShortestUnenforced( 2 * t * (1-t), r2 );
533  else
534  return r1.slerpShortestUnenforced( 2 * t * (1-t), r2.inverted() );
535  }
536 
537  Quaternion<T> squad( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const
538  {
539  Quaternion<T> r1 = this->slerp( t, q2 );
540  Quaternion<T> r2 = qa.slerp( t, qb );
541  return r1.slerp( 2 * t * (1-t), r2 );
542  }
543 
544  // Spherical Cubic Spline Interpolation -
545  // from Advanced Animation and Rendering
546  // Techniques by Watt and Watt, Page 366:
547  // A spherical curve is constructed using three
548  // spherical linear interpolations of a quadrangle
549  // of unit quaternions: q1, qa, qb, q2.
550  // Given a set of quaternion keys: q0, q1, q2, q3,
551  // this routine does the interpolation between
552  // q1 and q2 by constructing two intermediate
553  // quaternions: qa and qb. The qa and qb are
554  // computed by the intermediate function to
555  // guarantee the continuity of tangents across
556  // adjacent cubic segments. The qa represents in-tangent
557  // for q1 and the qb represents the out-tangent for q2.
558  //
559  // The q1 q2 is the cubic segment being interpolated.
560  // The q0 is from the previous adjacent segment and q3 is
561  // from the next adjacent segment. The q0 and q3 are used
562  // in computing qa and qb.
564  const Quaternion<T> &q2, const Quaternion<T> &q3 ) const
565  {
566  Quaternion<T> qa = splineIntermediate( *this, q1, q2 );
567  Quaternion<T> qb = splineIntermediate( q1, q2, q3 );
568  Quaternion<T> result = q1.squadShortestEnforced( t, qa, qb, q2 );
569 
570  return result;
571  }
572 
573  void set( const Matrix33<T> &m )
574  {
575  //T trace = m.m[0] + m.m[4] + m.m[8];
576  T trace = m.trace();
577  if ( trace > (T)0.0 )
578  {
579  T s = math<T>::sqrt( trace + (T)1.0 );
580  w = s * (T)0.5;
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;
585  }
586  else
587  {
588  unsigned int i = 0;
589  if( m.at(1,1) > m.at(0,0) )
590  i = 1;
591  if( m.at(2,2) > m.at(i,i) )
592  i = 2;
593  unsigned int j = ( i + 1 ) % 3;
594  unsigned int k = ( j + 1 ) % 3;
595  T s = math<T>::sqrt( m.at(i,i) - m.at(j,j) - m.at(k,k) + (T)1.0 );
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;
601  }
602  }
603 
604  void set( const Matrix44<T> &m )
605  {
606  //T trace = m.m[0] + m.m[5] + m.m[10];
607  T trace = m.trace();
608  if ( trace > (T)0.0 )
609  {
610  T s = math<T>::sqrt( trace + (T)1.0 );
611  w = s * (T)0.5;
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;
616  }
617  else
618  {
619  unsigned int i = 0;
620  if( m.at(1,1) > m.at(0,0) )
621  i = 1;
622  if( m.at(2,2) > m.at(i,i) )
623  i = 2;
624  unsigned int j = ( i + 1 ) % 3;
625  unsigned int k = ( j + 1 ) % 3;
626  T s = math<T>::sqrt( m.at(i,i) - m.at(j,j) - m.at(k,k) + (T)1.0 );
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;
632  }
633  }
634 
635  // Operators
637  {
638  v = rhs.v;
639  w = rhs.w;
640  return *this;
641  }
642 
643  template<typename FromT>
645  {
646  v = rhs.v;
647  w = static_cast<T>( rhs.w );
648  return *this;
649  }
650 
651  const Quaternion<T> operator+( const Quaternion<T> &rhs ) const
652  {
653  const Quaternion<T>& lhs = *this;
654  return Quaternion<T>( lhs.w + rhs.w, lhs.v.x + rhs.v.x, lhs.v.y + rhs.v.y, lhs.v.z + rhs.v.z );
655  }
656 
657  // post-multiply operator, similar to matrices, but different from Shoemake
658  // Concatenates 'rhs' onto 'this'
659  const Quaternion<T> operator*( const Quaternion<T> &rhs ) const
660  {
661  return Quaternion<T>( rhs.w*w - rhs.v.x*v.x - rhs.v.y*v.y - rhs.v.z*v.z,
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 );
665  }
666 
667  const Quaternion<T> operator*( T rhs ) const
668  {
669  return Quaternion<T>( w * rhs, v.x * rhs, v.y * rhs, v.z * rhs );
670  }
671 
672  // transform a vector by the quaternion
673  const Vec3<T> operator*( const Vec3<T> &vec ) const
674  {
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 );
678 
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 ) );
682  }
683 
684  const Quaternion<T> operator-( const Quaternion<T> &rhs ) const
685  {
686  const Quaternion<T>& lhs = *this;
687  return Quaternion<T>( lhs.w - rhs.w, lhs.v.x - rhs.v.x, lhs.v.y - rhs.v.y, lhs.v.z - rhs.v.z );
688  }
689 
691  {
692  w += rhs.w;
693  v += rhs.v;
694  return *this;
695  }
696 
698  {
699  w -= rhs.w;
700  v -= rhs.v;
701  return *this;
702  }
703 
705  {
706  Quaternion q = (*this) * rhs;
707  v = q.v;
708  w = q.w;
709  return *this;
710  }
711 
713  {
714  w *= rhs;
715  v *= rhs;
716  return *this;
717  }
718 
719  bool operator==( const Quaternion<T> &rhs ) const
720  {
721  const Quaternion<T>& lhs = *this;
722  return ( std::fabs(lhs.w - rhs.w) < EPSILON ) && lhs.v == rhs.v;
723  }
724 
725  bool operator!=( const Quaternion<T> &rhs ) const
726  {
727  return ! (*this == rhs);
728  }
729 
730  inline T& operator[]( unsigned int i ) { return (&v.x)[i]; }
731  inline const T& operator[]( unsigned int i ) const { return (&v.x)[i]; }
732 
734  {
735  return Quaternion();
736  }
737 
738  // Output
739  friend std::ostream& operator <<( std::ostream &oss, const Quaternion<T> &q )
740  {
741  oss << q.getAxis() << " @ " << q.getAngle() * ( (T)180 / M_PI ) << "deg";
742  return oss;
743  }
744 
745  private:
746  // From advanced Animation and Rendering
747  // Techniques by Watt and Watt, Page 366:
748  // computing the inner quadrangle
749  // points (qa and qb) to guarantee tangent
750  // continuity.
751  static Quaternion<T> splineIntermediate( const Quaternion<T> &q0, const Quaternion<T> &q1, const Quaternion<T> &q2 )
752  {
753  Quaternion<T> q1inv = q1.inverted();
754  Quaternion<T> c1 = q1inv * q2;
755  Quaternion<T> c2 = q1inv * q0;
756  Quaternion<T> c3 = ( c2.log() + c1.log() ) * (T)-0.25;
757  Quaternion<T> qa = q1 * c3.exp();
758  return qa.normalized();
759  }
760 };
761 
762 template<typename T>
763 inline Vec3<T> operator*( const Vec3<T> &vec, const Quaternion<T> &q )
764 {
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 );
768 
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 ) );
772 }
773 
776 
777 } // namespace cinder
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
Definition: Vector.h:65
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