00001 /* 00002 Copyright (c) 2010, The Barbarian Group 00003 All rights reserved. 00004 00005 Redistribution and use in source and binary forms, with or without modification, are permitted provided that 00006 the following conditions are met: 00007 00008 * Redistributions of source code must retain the above copyright notice, this list of conditions and 00009 the following disclaimer. 00010 * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and 00011 the following disclaimer in the documentation and/or other materials provided with the distribution. 00012 00013 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED 00014 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 00015 PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 00016 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00017 TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 00018 HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 00019 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00020 POSSIBILITY OF SUCH DAMAGE. 00021 */ 00022 00023 #pragma once 00024 00025 #include "cinder/CinderMath.h" 00026 #include "cinder/Vector.h" 00027 #include "cinder/Matrix.h" 00028 00029 namespace cinder { 00030 00031 template<typename T, typename Y> 00032 struct QUATCONV { 00033 typedef typename T::TYPE F; 00034 static F getW( const Y &v ) { return static_cast<F>( v.w ); } 00035 static F getX( const Y &v ) { return static_cast<F>( v.x ); } 00036 static F getY( const Y &v ) { return static_cast<F>( v.y ); } 00037 static F getZ( const Y &v ) { return static_cast<F>( v.z ); } 00038 }; 00039 00040 template<typename T> 00041 class Quaternion 00042 { 00043 public: 00044 typedef T TYPE; 00045 00046 Vec3<T> v; // axisOfRotation.normalized() * sin( angleOfRotation / 2 ) 00047 T w; // cos( angleOfRotation / 2 ) 00048 00049 Quaternion(): w( 1 ), v( 0, 0, 0 ){} // default constructor is identity quat 00050 template<typename FromT> 00051 Quaternion( const Quaternion<FromT>& q ) : w( static_cast<T>( q.w ) ), v( q.v ) {} 00052 00053 Quaternion( T aW, T x, T y, T z ): w( aW ), v( x, y, z ) {} 00054 // construct from axis-angle 00055 Quaternion( const Vec3<T> &axis, T radians ) { set( axis, radians ); } 00056 Quaternion( const Vec3<T> &from, const Vec3<T> &to ) { set( from, to ); } 00057 // create from euler angles in radians expressed in ZYX rotation order 00058 Quaternion( T xRotation, T yRotation, T zRotation ) { set( xRotation, yRotation, zRotation ); } 00059 Quaternion( const Matrix44<T> &m ) { set( m ); } 00060 template<typename Y> 00061 explicit Quaternion( const Y &v ) 00062 : 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 ) ) 00063 {} 00064 00065 // get axis-angle representation's axis 00066 Vec3<T> getAxis() const 00067 { 00068 T cos_angle = w; 00069 T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle ); 00070 00071 return v * invLen; 00072 } 00073 00074 // get axis-angle representation's angle in radians 00075 T getAngle() const 00076 { 00077 T cos_angle = w; 00078 return math<T>::acos( cos_angle ) * 2; 00079 } 00080 00081 T getPitch() const 00082 { 00083 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 ); 00084 } 00085 00086 T getYaw() const 00087 { 00088 return math<T>::sin( (T)-2 * ( v.x * v.z - w * v.y ) ); 00089 } 00090 00091 T getRoll() const 00092 { 00093 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 ); 00094 } 00095 00096 T dot( const Quaternion<T> &quat ) const 00097 { 00098 return w * quat.w + v.dot( quat.v ); 00099 } 00100 00101 T length() const 00102 { 00103 return (T)std::sqrt( w*w + v.lengthSquared() ); 00104 } 00105 00106 T lengthSquared() const 00107 { 00108 return w * w + v.lengthSquared(); 00109 } 00110 00111 Quaternion<T> inverse() const 00112 { 00113 T norm = w * w + v.x * v.x + v.y * v.y + v.z * v.z; 00114 // if we're the zero quaternion, just return identity 00115 /*if( ! ( math<T>::abs( norm ) < EPSILON_VALUE ) ) { 00116 return identity(); 00117 }*/ 00118 00119 T normRecip = static_cast<T>( 1.0f ) / norm; 00120 return Quaternion<T>( normRecip * w, -normRecip * v.x, -normRecip * v.y, -normRecip * v.z ); 00121 } 00122 00123 void normalize() 00124 { 00125 if( T len = length() ) { 00126 w /= len; 00127 v /= len; 00128 } 00129 else { 00130 w = static_cast<T>( 1.0 ); 00131 v = Vec3<T>( 0, 0, 0 ); 00132 } 00133 } 00134 00135 Quaternion<T> normalized() const 00136 { 00137 Quaternion<T> result = *this; 00138 00139 if( T len = length() ) { 00140 result.w /= len; 00141 result.v /= len; 00142 } 00143 else { 00144 result.w = static_cast<T>( 1.0 ); 00145 result.v = Vec3<T>( 0, 0, 0 ); 00146 } 00147 00148 return result; 00149 } 00150 00151 // For unit quaternion, from Advanced Animation and 00152 // Rendering Techniques by Watt and Watt, Page 366: 00153 Quaternion<T> log() const 00154 { 00155 T theta = math<T>::acos( std::min( w, (T) 1.0 ) ); 00156 00157 if( theta == 0 ) 00158 return Quaternion<T>( v, 0 ); 00159 00160 T sintheta = math<T>::sin( theta ); 00161 00162 T k; 00163 if ( abs( sintheta ) < 1 && abs( theta ) >= 3.402823466e+38F * abs( sintheta ) ) 00164 k = 1; 00165 else 00166 k = theta / sintheta; 00167 00168 return Quaternion<T>( (T)0, v.x * k, v.y * k, v.z * k ); 00169 } 00170 00171 // For pure quaternion (zero scalar part): 00172 // from Advanced Animation and Rendering 00173 // Techniques by Watt and Watt, Page 366: 00174 Quaternion<T> exp() const 00175 { 00176 T theta = v.length(); 00177 T sintheta = math<T>::sin( theta ); 00178 00179 T k; 00180 if( abs( theta ) < 1 && abs( sintheta ) >= 3.402823466e+38F * abs( theta ) ) 00181 k = 1; 00182 else 00183 k = sintheta / theta; 00184 00185 T costheta = math<T>::cos( theta ); 00186 00187 return Quaternion<T>( costheta, v.x * k, v.y * k, v.z * k ); 00188 } 00189 00190 Quaternion<T> inverted() const 00191 { 00192 T qdot = this->dot( *this ); 00193 return Quaternion( -v / qdot, w / qdot ); 00194 } 00195 00196 void invert() 00197 { 00198 T qdot = this->dot( *this ); 00199 set( -v / qdot, w / qdot ); 00200 } 00201 00202 void set( T aW, T x, T y, T z ) 00203 { 00204 w = aW; 00205 v = Vec3<T>( x, y, z ); 00206 } 00207 00208 #if 1 00209 void set( const Vec3<T> &from, const Vec3<T> &to ) 00210 { 00211 Vec3<T> axis = from.cross( to ); 00212 00213 set( from.dot( to ), axis.x, axis.y, axis.z ); 00214 normalize(); 00215 00216 w += static_cast<T>( 1.0 ); 00217 00218 if( w <= EPSILON ) { 00219 if ( from.z * from.z > from.x * from.x ) { 00220 set( static_cast<T>( 0.0 ), static_cast<T>( 0.0 ), from.z, -from.y ); 00221 } 00222 else { 00223 set( static_cast<T>( 0.0 ), from.y, -from.x, static_cast<T>( 0.0 ) ); 00224 } 00225 } 00226 00227 normalize(); 00228 } 00229 #else 00230 void set( const Vec3<T> &from, const Vec3<T> &to ) 00231 { 00232 Vec3<T> f0 = from.normalized(); 00233 Vec3<T> t0 = to.normalized(); 00234 00235 if( f0.dot( t0 ) >= 0 ) { // The rotation angle is less than or equal to pi/2. 00236 setRotationInternal (f0, t0, *this); 00237 } 00238 else { 00239 // The angle is greater than pi/2. After computing h0, 00240 // which is halfway between f0 and t0, we rotate first 00241 // from f0 to h0, then from h0 to t0. 00242 00243 Vec3<T> h0 = (f0 + t0).normalized(); 00244 00245 if( h0.dot( h0 ) != 0 ) { 00246 setRotationInternal( f0, h0, *this ); 00247 Quaternion<T> q; 00248 setRotationInternal( h0, t0, q ); 00249 00250 set( q.w*w - q.v.x*v.x - q.v.y*v.y - q.v.z*v.z, 00251 q.w*v.x + q.v.x*w + q.v.y*v.z - q.v.z*v.y, 00252 q.w*v.y + q.v.y*w + q.v.z*v.x - q.v.x*v.z, 00253 q.w*v.z + q.v.z*w + q.v.x*v.y - q.v.y*v.x ); 00254 00255 //*this *= q; 00256 } 00257 else { 00258 // f0 and t0 point in exactly opposite directions. 00259 // Pick an arbitrary axis that is orthogonal to f0, 00260 // and rotate by pi. 00261 00262 w = T( 0 ); 00263 00264 Vec3<T> f02 = f0 * f0; 00265 00266 if( ( f02.x <= f02.y ) && ( f02.x <= f02.z ) ) 00267 v = f0.cross( Vec3<T>( 1, 0, 0 ) ).normalized(); 00268 else if( f02.y <= f02.z ) 00269 v = f0.cross( Vec3<T>( 0, 1, 0 ) ).normalized(); 00270 else 00271 v = f0.cross( Vec3<T>( 0, 0, 1 ) ).normalized(); 00272 } 00273 } 00274 } 00275 00276 static void setRotationInternal( const Vec3<T> &f0, const Vec3<T> &t0, Quaternion<T> &q ) 00277 { 00278 // 00279 // The following is equivalent to setAxisAngle(n,2*phi), 00280 // where the rotation axis, is orthogonal to the f0 and 00281 // t0 vectors, and 2*phi is the angle between f0 and t0. 00282 // 00283 // This function is called by setRotation(), above; it assumes 00284 // that f0 and t0 are normalized and that the angle between 00285 // them is not much greater than pi/2. This function becomes 00286 // numerically inaccurate if f0 and t0 point into nearly 00287 // opposite directions. 00288 // 00289 00290 // Find a normalized vector, h0, that is half way between f0 and t0. 00291 // The angle between f0 and h0 is phi. 00292 Vec3<T> h0 = ( f0 + t0 ).normalized(); 00293 00294 // Store the rotation axis and rotation angle. 00295 q.w = f0.dot( h0 ); // f0 ^ h0 == cos (phi) 00296 q.v = f0.cross( h0 ); // (f0 % h0).length() == sin (phi) 00297 } 00298 #endif 00299 00300 void set( const Vec3<T> &axis, T radians ) 00301 { 00302 w = math<T>::cos( radians / 2 ); 00303 v = axis.normalized() * math<T>::sin( radians / 2 ); 00304 } 00305 00306 // assumes ZYX rotation order and radians 00307 void set( T xRotation, T yRotation, T zRotation ) 00308 { 00309 zRotation *= T( 0.5 ); 00310 yRotation *= T( 0.5 ); 00311 xRotation *= T( 0.5 ); 00312 00313 // get sines and cosines of half angles 00314 T Cx = math<T>::cos( xRotation ); 00315 T Sx = math<T>::sin( xRotation ); 00316 00317 T Cy = math<T>::cos( yRotation ); 00318 T Sy = math<T>::sin( yRotation ); 00319 00320 T Cz = math<T>::cos( zRotation ); 00321 T Sz = math<T>::sin( zRotation ); 00322 00323 // multiply it out 00324 w = Cx*Cy*Cz - Sx*Sy*Sz; 00325 v.x = Sx*Cy*Cz + Cx*Sy*Sz; 00326 v.y = Cx*Sy*Cz - Sx*Cy*Sz; 00327 v.z = Cx*Cy*Sz + Sx*Sy*Cx; 00328 } 00329 00330 void getAxisAngle( Vec3<T> *axis, T *radians ) const 00331 { 00332 T cos_angle = w; 00333 *radians = math<T>::acos( cos_angle ) * 2; 00334 T invLen = static_cast<T>( 1.0 ) / math<T>::sqrt( static_cast<T>( 1.0 ) - cos_angle * cos_angle ); 00335 00336 axis->x = v.x * invLen; 00337 axis->y = v.y * invLen; 00338 axis->z = v.z * invLen; 00339 } 00340 00341 Matrix44<T> toMatrix44() const 00342 { 00343 Matrix44<T> mV; 00344 T xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; 00345 00346 xs = v.x + v.x; 00347 ys = v.y + v.y; 00348 zs = v.z + v.z; 00349 wx = w * xs; 00350 wy = w * ys; 00351 wz = w * zs; 00352 xx = v.x * xs; 00353 xy = v.x * ys; 00354 xz = v.x * zs; 00355 yy = v.y * ys; 00356 yz = v.y * zs; 00357 zz = v.z * zs; 00358 00359 mV[0] = T( 1 ) - ( yy + zz ); 00360 mV[4] = xy - wz; 00361 mV[8] = xz + wy; 00362 mV[12] = 0; 00363 00364 mV[1] = xy + wz; 00365 mV[5] = T( 1 ) - ( xx + zz ); 00366 mV[9] = yz - wx; 00367 mV[13] = 0; 00368 00369 mV[2] = xz - wy; 00370 mV[6] = yz + wx; 00371 mV[10] = T( 1 ) - ( xx + yy ); 00372 mV[14] = 0; 00373 00374 mV[3] = 0; 00375 mV[7] = 0; 00376 mV[11] = 0; 00377 mV[15] = T( 1 ); 00378 00379 return mV; 00380 } 00381 00382 operator Matrix44<T>() const { return toMatrix44(); } 00383 00384 Quaternion<T> lerp( T t, const Quaternion<T> &end ) const 00385 { 00386 // get cos of "angle" between quaternions 00387 float cosTheta = dot( end ); 00388 00389 // initialize result 00390 Quaternion<T> result = t * end; 00391 00392 // if "angle" between quaternions is less than 90 degrees 00393 if( cosTheta >= EPSILON ) { 00394 // use standard interpolation 00395 result += *this * ( static_cast<T>( 1.0 ) - t ); 00396 } 00397 else { 00398 // otherwise, take the shorter path 00399 result += *this * ( t - static_cast<T>( 1.0 ) ); 00400 } 00401 00402 return result; 00403 } 00404 00405 // This method does *not* interpolate along the shortest 00406 // arc between q1 and q2. If you desire interpolation 00407 // along the shortest arc, and q1.dot( q2 ) is negative, then 00408 // consider flipping the second quaternion explicitly. 00409 // 00410 // NOTE: the difference between this and slerp isn't clear, but we're using 00411 // the Don Hatch / ilmbase squad code which explicity requires this impl. of slerp 00412 // so I'm leaving it for now 00413 Quaternion<T> slerpShortestUnenforced( T t, const Quaternion<T> &end ) const 00414 { 00415 Quaternion<T> d = *this - end; 00416 T lengthD = math<T>::sqrt( this->dot( end ) ); 00417 00418 Quaternion<T> st = *this + end; 00419 T lengthS = math<T>::sqrt( st.dot( st ) ); 00420 00421 T a = 2 * math<T>::atan2( lengthD, lengthS );; 00422 T s = 1 - t; 00423 00424 Quaternion<T> q( *this * ( sinx_over_x( s * a ) / sinx_over_x( a ) * s ) + 00425 end * ( sinx_over_x( t * a ) / sinx_over_x( a ) * t ) ); 00426 00427 return q.normalized(); 00428 } 00429 00430 Quaternion<T> slerp( T t, const Quaternion<T> &end ) const 00431 { 00432 // get cosine of "angle" between quaternions 00433 T cosTheta = this->dot( end ); 00434 T startInterp, endInterp; 00435 00436 // if "angle" between quaternions is less than 90 degrees 00437 if( cosTheta >= EPSILON ) { 00438 // if angle is greater than zero 00439 if( ( static_cast<T>( 1.0 ) - cosTheta ) > EPSILON ) { 00440 // use standard slerp 00441 T theta = math<T>::acos( cosTheta ); 00442 T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta ); 00443 00444 startInterp = math<T>::sin( ( static_cast<T>( 1.0 ) - t ) * theta ) * recipSinTheta; 00445 endInterp = math<T>::sin( t * theta ) * recipSinTheta; 00446 } 00447 // angle is close to zero 00448 else { 00449 // use linear interpolation 00450 startInterp = static_cast<T>( 1.0 ) - t; 00451 endInterp = t; 00452 } 00453 } 00454 // otherwise, take the shorter route 00455 else { 00456 // if angle is less than 180 degrees 00457 if( ( static_cast<T>( 1.0 ) + cosTheta ) > EPSILON ) { 00458 // use slerp w/negation of start quaternion 00459 T theta = math<T>::acos( -cosTheta ); 00460 T recipSinTheta = static_cast<T>( 1.0 ) / math<T>::sin( theta ); 00461 00462 startInterp = math<T>::sin( ( t - static_cast<T>( 1.0 ) ) * theta ) * recipSinTheta; 00463 endInterp = math<T>::sin( t * theta ) * recipSinTheta; 00464 } 00465 // angle is close to 180 degrees 00466 else { 00467 // use lerp w/negation of start quaternion 00468 startInterp = t - static_cast<T>( 1.0 ); 00469 endInterp = t; 00470 } 00471 } 00472 00473 return *this * startInterp + end * endInterp; 00474 } 00475 00476 // Spherical Quadrangle Interpolation - 00477 // from Advanced Animation and Rendering 00478 // Techniques by Watt and Watt, Page 366: 00479 // It constructs a spherical cubic interpolation as 00480 // a series of three spherical linear interpolations 00481 // of a quadrangle of unit quaternions. 00482 Quaternion<T> squadShortestEnforced( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const 00483 { 00484 Quaternion<T> r1; 00485 if( this->dot( q2 ) >= 0 ) 00486 r1 = this->slerpShortestUnenforced( t, q2 ); 00487 else 00488 r1 = this->slerpShortestUnenforced( t, q2.inverted() ); 00489 00490 Quaternion<T> r2; 00491 if( qa.dot( qb ) >= 0 ) 00492 r2 = qa.slerpShortestUnenforced( t, qb ); 00493 else 00494 r2 = qa.slerpShortestUnenforced( t, qb.inverted() ); 00495 00496 if( r1.dot( r2 ) >= 0 ) 00497 return r1.slerpShortestUnenforced( 2 * t * (1-t), r2 ); 00498 else 00499 return r1.slerpShortestUnenforced( 2 * t * (1-t), r2.inverted() ); 00500 } 00501 00502 Quaternion<T> squad( T t, const Quaternion<T> &qa, const Quaternion<T> &qb, const Quaternion<T> &q2 ) const 00503 { 00504 Quaternion<T> r1 = this->slerp( t, q2 ); 00505 Quaternion<T> r2 = qa.slerp( t, qb ); 00506 return r1.slerp( 2 * t * (1-t), r2 ); 00507 } 00508 00509 // Spherical Cubic Spline Interpolation - 00510 // from Advanced Animation and Rendering 00511 // Techniques by Watt and Watt, Page 366: 00512 // A spherical curve is constructed using three 00513 // spherical linear interpolations of a quadrangle 00514 // of unit quaternions: q1, qa, qb, q2. 00515 // Given a set of quaternion keys: q0, q1, q2, q3, 00516 // this routine does the interpolation between 00517 // q1 and q2 by constructing two intermediate 00518 // quaternions: qa and qb. The qa and qb are 00519 // computed by the intermediate function to 00520 // guarantee the continuity of tangents across 00521 // adjacent cubic segments. The qa represents in-tangent 00522 // for q1 and the qb represents the out-tangent for q2. 00523 // 00524 // The q1 q2 is the cubic segment being interpolated. 00525 // The q0 is from the previous adjacent segment and q3 is 00526 // from the next adjacent segment. The q0 and q3 are used 00527 // in computing qa and qb. 00528 Quaternion<T> spline( T t, const Quaternion<T> &q1, 00529 const Quaternion<T> &q2, const Quaternion<T> &q3 ) const 00530 { 00531 Quaternion<T> qa = splineIntermediate( *this, q1, q2 ); 00532 Quaternion<T> qb = splineIntermediate( q1, q2, q3 ); 00533 Quaternion<T> result = q1.squadShortestEnforced( t, qa, qb, q2 ); 00534 00535 return result; 00536 } 00537 00538 void set( const Matrix44<T> &m ) 00539 { 00540 T trace = m.m[0] + m.m[5] + m.m[10]; 00541 if ( trace > (T)0.0 ) 00542 { 00543 T s = math<T>::sqrt( trace + (T)1.0 ); 00544 w = s * (T)0.5; 00545 T recip = (T)0.5 / s; 00546 v.x = ( m.at(2,1) - m.at(1,2) ) * recip; 00547 v.y = ( m.at(0,2) - m.at(2,0) ) * recip; 00548 v.z = ( m.at(1,0) - m.at(0,1) ) * recip; 00549 } 00550 else 00551 { 00552 unsigned int i = 0; 00553 if( m.at(1,1) > m.at(0,0) ) 00554 i = 1; 00555 if( m.at(2,2) > m.at(i,i) ) 00556 i = 2; 00557 unsigned int j = ( i + 1 ) % 3; 00558 unsigned int k = ( j + 1 ) % 3; 00559 T s = math<T>::sqrt( m.at(i,i) - m.at(j,j) - m.at(k,k) + (T)1.0 ); 00560 (*this)[i] = (T)0.5 * s; 00561 T recip = (T)0.5 / s; 00562 w = ( m.at(k,j) - m.at(j,k) ) * recip; 00563 (*this)[j] = ( m.at(j,i) + m.at(i,j) ) * recip; 00564 (*this)[k] = ( m.at(k,i) + m.at(i,k) ) * recip; 00565 } 00566 } 00567 00568 // Operators 00569 Quaternion<T>& operator=( const Quaternion<T> &rhs ) 00570 { 00571 v = rhs.v; 00572 w = rhs.w; 00573 return *this; 00574 } 00575 00576 template<typename FromT> 00577 Quaternion<T>& operator=( const Quaternion<FromT> &rhs ) 00578 { 00579 v = rhs.v; 00580 w = static_cast<T>( rhs.w ); 00581 return *this; 00582 } 00583 00584 const Quaternion<T> operator+( const Quaternion<T> &rhs ) const 00585 { 00586 const Quaternion<T>& lhs = *this; 00587 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 ); 00588 } 00589 00590 // post-multiply operator, similar to matrices, but different from Shoemake 00591 // Concatenates 'rhs' onto 'this' 00592 const Quaternion<T> operator*( const Quaternion<T> &rhs ) const 00593 { 00594 return Quaternion<T>( rhs.w*w - rhs.v.x*v.x - rhs.v.y*v.y - rhs.v.z*v.z, 00595 rhs.w*v.x + rhs.v.x*w + rhs.v.y*v.z - rhs.v.z*v.y, 00596 rhs.w*v.y + rhs.v.y*w + rhs.v.z*v.x - rhs.v.x*v.z, 00597 rhs.w*v.z + rhs.v.z*w + rhs.v.x*v.y - rhs.v.y*v.x ); 00598 } 00599 00600 const Quaternion<T> operator*( T rhs ) const 00601 { 00602 return Quaternion<T>( w * rhs, v.x * rhs, v.y * rhs, v.z * rhs ); 00603 } 00604 00605 // transform a vector by the quaternion 00606 const Vec3<T> operator*( const Vec3<T> &vec ) 00607 { 00608 T vMult = T( 2 ) * ( v.x * vec.x + v.y * vec.y + v.z * vec.z ); 00609 T crossMult = T( 2 ) * w; 00610 T pMult = crossMult * w - T( 1 ); 00611 00612 return Vec3<T>( pMult * vec.x + vMult * v.x + crossMult * ( v.y * vec.z - v.z * vec.y ), 00613 pMult * vec.y + vMult * v.y + crossMult * ( v.z * vec.x - v.x * vec.z ), 00614 pMult * vec.z + vMult * v.z + crossMult * ( v.x * vec.y - v.y * vec.x ) ); 00615 } 00616 00617 const Quaternion<T> operator-( const Quaternion<T> &rhs ) const 00618 { 00619 const Quaternion<T>& lhs = *this; 00620 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 ); 00621 } 00622 00623 Quaternion<T>& operator+=( const Quaternion<T> &rhs ) 00624 { 00625 w += rhs.w; 00626 v += rhs.v; 00627 return *this; 00628 } 00629 00630 Quaternion<T>& operator-=( const Quaternion<T>& rhs ) 00631 { 00632 w -= rhs.w; 00633 v -= rhs.v; 00634 return *this; 00635 } 00636 00637 Quaternion<T>& operator*=( const Quaternion<T> &rhs ) 00638 { 00639 Quaternion q = (*this) * rhs; 00640 v = q.v; 00641 w = q.w; 00642 return *this; 00643 } 00644 00645 Quaternion<T>& operator*=( T rhs ) 00646 { 00647 w *= rhs; 00648 v *= rhs; 00649 return *this; 00650 } 00651 00652 bool operator==( const Quaternion<T> &rhs ) const 00653 { 00654 const Quaternion<T>& lhs = *this; 00655 return ( std::fabs(lhs.w - rhs.w) < EPSILON ) && lhs.v == rhs.v; 00656 } 00657 00658 bool operator!=( const Quaternion<T> &rhs ) const 00659 { 00660 return ! (*this == rhs); 00661 } 00662 00663 inline T& operator[]( unsigned int i ) { return (&v.x)[i]; } 00664 inline const T& operator[]( unsigned int i ) const { return (&v.x)[i]; } 00665 00666 static Quaternion<T> identity() 00667 { 00668 return Quaternion(); 00669 } 00670 00671 // Output 00672 friend std::ostream& operator <<( std::ostream &oss, const Quaternion<T> &q ) 00673 { 00674 oss << q.getAxis() << " @ " << q.getAngle() * ( (T)180 / M_PI ) << "deg"; 00675 return oss; 00676 } 00677 00678 private: 00679 // From advanced Animation and Rendering 00680 // Techniques by Watt and Watt, Page 366: 00681 // computing the inner quadrangle 00682 // points (qa and qb) to guarantee tangent 00683 // continuity. 00684 static Quaternion<T> splineIntermediate( const Quaternion<T> &q0, const Quaternion<T> &q1, const Quaternion<T> &q2 ) 00685 { 00686 Quaternion<T> q1inv = q1.inverted(); 00687 Quaternion<T> c1 = q1inv * q2; 00688 Quaternion<T> c2 = q1inv * q0; 00689 Quaternion<T> c3 = ( c2.log() + c1.log() ) * (T)-0.25; 00690 Quaternion<T> qa = q1 * c3.exp(); 00691 return qa.normalized(); 00692 } 00693 }; 00694 00695 template<typename T> 00696 inline Vec3<T> operator*( const Vec3<T> &vec, const Quaternion<T> &q ) 00697 { 00698 T vMult = T( 2 ) * ( q.v.x * vec.x + q.v.y * vec.y + q.v.z * vec.z ); 00699 T crossMult = T( 2 ) * q.w; 00700 T pMult = crossMult * q.w - T( 1 ); 00701 00702 return Vec3<T>( pMult * vec.x + vMult * q.v.x + crossMult * ( q.v.y * vec.z - q.v.z * vec.y ), 00703 pMult * vec.y + vMult * q.v.y + crossMult * ( q.v.z * vec.x - q.v.x * vec.z ), 00704 pMult * vec.z + vMult * q.v.z + crossMult * ( q.v.x * vec.y - q.v.y * vec.x ) ); 00705 } 00706 00707 typedef Quaternion<float> Quatf; 00708 typedef Quaternion<double> Quatd; 00709 00710 } // namespace cinder