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