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