simgear/scene/sky/clouds3d/quatimpl.hpp

   1 //------------------------------------------------------------------------------
   2 // File : quatimpl.hpp
   3 //------------------------------------------------------------------------------
   4 // GLVU : Copyright 1997 - 2002
   5 //        The University of North Carolina at Chapel Hill
   6 //------------------------------------------------------------------------------
   7 // Permission to use, copy, modify, distribute and sell this software and its
   8 // documentation for any purpose is hereby granted without fee, provided that
   9 // the above copyright notice appear in all copies and that both that copyright
  10 // notice and this permission notice appear in supporting documentation.
  11 // Binaries may be compiled with this software without any royalties or
  12 // restrictions.
  13 //
  14 // The University of North Carolina at Chapel Hill makes no representations
  15 // about the suitability of this software for any purpose. It is provided
  16 // "as is" without express or implied warranty.
  17
  18 /***********************************************************************
  19
  20   quatimpl.hpp
  21
  22   A quaternion template class
  23
  24   -------------------------------------------------------------------
  25
  26   Feb 1998, Paul Rademacher (rademach@cs.unc.edu)
  27   Oct 2000, Bill Baxter
  28
  29   Modification History:
  30   - See main header file, quat.hpp
  31
  32 ************************************************************************/
  33
  34 #include "quat.hpp"
  35 #include <math.h>
  36 #include <assert.h>
  37
  38
  39 //============================================================================
  40 // CONSTRUCTORS
  41 //============================================================================
  42
  43 template <class Type>
  44 Quat<Type>::Quat( void )
  45 {
  46   // do nothing so default construction is fast
  47 }
  48
  49 template <class Type>
  50 Quat<Type>::Quat( Type _s, Type x, Type y, Type z )
  51 {
  52   s = _s;
  53   v.Set( x, y, z );
  54 }
  55 template <class Type>
  56 Quat<Type>::Quat( Type x, Type y, Type z )
  57 {
  58   s = 0.0;
  59   v.Set( x, y, z );
  60 }
  61
  62 template <class Type>
  63 Quat<Type>::Quat( const qvec& _v, Type _s )
  64 {
  65   Set( _v, _s );
  66 }
  67
  68 template <class Type>
  69 Quat<Type>::Quat( Type _s, const qvec& _v )
  70 {
  71   Set( _v, _s );
  72 }
  73
  74
  75 template <class Type>
  76 Quat<Type>::Quat( const Type *d )
  77 {
  78   s = *d++;
  79   v.Set(d);
  80 }
  81
  82 template <class Type>
  83 Quat<Type>::Quat( const Quat &q )
  84 {
  85   s = q.s;
  86   v = q.v;
  87 }
  88
  89 //============================================================================
  90 // SETTERS
  91 //============================================================================
  92
  93 template <class Type>
  94 void Quat<Type>::Set( Type _s, Type x, Type y, Type z )
  95 {
  96   s = _s;
  97   v.Set(x,y,z);
  98 }
  99 template <class Type>
 100 void Quat<Type>::Set( Type x, Type y, Type z )
 101 {
 102   s = 0.0;
 103   v.Set(x,y,z);
 104 }
 105
 106 template <class Type>
 107 void Quat<Type>::Set( const qvec& _v, Type _s )
 108 {
 109   s = _s;
 110   v = _v;
 111 }
 112 template <class Type>
 113 void Quat<Type>::Set( Type _s, const qvec& _v )
 114 {
 115   s = _s;
 116   v = _v;
 117 }
 118
 119
 120 //============================================================================
 121 // OPERATORS
 122 //============================================================================
 123
 124 template <class Type>
 125 Quat<Type>& Quat<Type>::operator = (const Quat& q)
 126 {
 127   v = q.v;  s = q.s; return *this;
 128 }
 129
 130 template <class Type>
 131 Quat<Type>& Quat<Type>::operator += ( const Quat &q )
 132 {
 133   v += q.v; s += q.s; return *this;
 134 }
 135
 136 template <class Type>
 137 Quat<Type>& Quat<Type>::operator -= ( const Quat &q )
 138 {
 139   v -= q.v; s -= q.s; return *this;
 140 }
 141
 142 template <class Type>
 143 Quat<Type> &Quat<Type>::operator *= ( const Type d )
 144 {
 145   v *= d; s *= d; return *this;
 146 }
 147
 148 template <class Type>
 149 Quat<Type> &Quat<Type>::operator *= ( const Quat& q )
 150 {
 151 #if 0
 152   // Quaternion multiplication with
 153   // temporary object construction minimized (hopefully)
 154   Type ns = s*q.s - v*q.v;
 155   qvec nv(v^q.v);
 156   v *= q.s;
 157   v += nv;
 158   nv.Set(s*q.v);
 159   v += nv;
 160   s = ns;
 161   return *this;
 162 #else
 163   // optimized (12 mults, and no compiler-generated temp objects)
 164   Type A, B, C, D, E, F, G, H;
 165
 166   A = (s   + v.x)*(q.s   + q.v.x);
 167   B = (v.z - v.y)*(q.v.y - q.v.z);
 168   C = (s   - v.x)*(q.v.y + q.v.z);
 169   D = (v.y + v.z)*(q.s   - q.v.x);
 170   E = (v.x + v.z)*(q.v.x + q.v.y);
 171   F = (v.x - v.z)*(q.v.x - q.v.y);
 172   G = (s   + v.y)*(q.s   - q.v.z);
 173   H = (s   - v.y)*(q.s   + q.v.z);
 174
 175   v.x = A - (E + F + G + H) * Type(0.5);
 176   v.y = C + (E - F + G - H) * Type(0.5);
 177   v.z = D + (E - F - G + H) * Type(0.5);
 178   s = B + (-E - F + G + H) * Type(0.5);
 179
 180   return *this;
 181 #endif
 182 }
 183
 184 template <class Type>
 185 Quat<Type> &Quat<Type>::operator /= ( const Type d )
 186 {
 187   Type r = Type(1.0)/d;
 188   v *= r;
 189   s *= r;
 190   return *this;
 191 }
 192
 193 template <class Type>
 194 Type &Quat<Type>::operator [] ( int i)
 195 {
 196   switch (i) {
 197     case 0: return s;
 198     case 1: return v.x;
 199     case 2: return v.y;
 200     case 3: return v.z;
 201   }
 202   assert(false);
 203   return s;
 204 }
 205
 206 //============================================================================
 207 // SPECIAL FUNCTIONS
 208 //============================================================================
 209 template <class Type>
 210 inline Type Quat<Type>::Length( void ) const
 211 {
 212   return Type( sqrt( v*v + s*s ) );
 213 }
 214
 215 template <class Type>
 216 inline Type Quat<Type>::LengthSqr( void ) const
 217 {
 218   return Norm();
 219 }
 220
 221 template <class Type>
 222 inline Type Quat<Type>::Norm( void ) const
 223 {
 224   return v*v + s*s;
 225 }
 226
 227 template <class Type>
 228 inline Quat<Type>& Quat<Type>::Normalize( void )
 229 {
 230   *this *= Type(1.0) / Type(sqrt(v*v + s*s));
 231   return *this;
 232 }
 233
 234 template <class Type>
 235 inline Quat<Type>& Quat<Type>::Invert( void )
 236 {
 237   Type scale = Type(1.0)/Norm();
 238   v *= -scale;
 239   s *= scale;
 240   return *this;
 241 }
 242
 243 template <class Type>
 244 inline Quat<Type>& Quat<Type>::Conjugate( void )
 245 {
 246   v.x = -v.x;
 247   v.y = -v.y;
 248   v.z = -v.z;
 249   return *this;
 250 }
 251
 252
 253 //----------------------------------------------------------------------------
 254 // Xform
 255 //----------------------------------------------------------------------------
 256 // Transform a vector by this quaternion using q * v * q^-1
 257 //----------------------------------------------------------------------------
 258 template <class Type>
 259 Quat<Type>::qvec Quat<Type>::Xform( const qvec &vec ) const
 260 {
 261   /* copy vector into temp quaternion for multiply      */
 262   Quat  vecQuat(vec);
 263   /* invert multiplier  */
 264   Quat  inverse(*this);
 265   inverse.Invert();
 266
 267   /* do q * vec * q(inv)        */
 268   Quat  tempVecQuat(*this * vecQuat);
 269   tempVecQuat *= inverse;
 270
 271   /* return vector part */
 272   return tempVecQuat.v;
 273 }
 274
 275 //----------------------------------------------------------------------------
 276 // Log
 277 //----------------------------------------------------------------------------
 278 // Natural log of quat
 279 //----------------------------------------------------------------------------
 280 template <class Type>
 281 Quat<Type> &Quat<Type>::Log(void)
 282 {
 283   Type theta, scale;
 284
 285   scale = v.Length();
 286   theta = ATan2(scale, s);
 287
 288   if (scale > 0.0)
 289     scale = theta/scale;
 290
 291   v *= scale;
 292   s = 0.0;
 293   return *this;
 294 }
 295
 296 //----------------------------------------------------------------------------
 297 // Exp
 298 //----------------------------------------------------------------------------
 299 // e to the quat: e^quat
 300 // -- assuming scalar part 0
 301 //----------------------------------------------------------------------------
 302 template <class Type>
 303 Quat<Type> &Quat<Type>::Exp(void)
 304 {
 305   Type scale;
 306   Type theta = v.Length();
 307
 308   if (theta > FUDGE()) {
 309     scale = Sin(theta)/theta ;
 310     v *= scale;
 311   }
 312
 313   s = Cos(theta) ;
 314   return *this;
 315 }
 316
 317
 318 //----------------------------------------------------------------------------
 319 // SetAngle (radians)
 320 //----------------------------------------------------------------------------
 321 template <class Type>
 322 void Quat<Type>::SetAngle( Type f )
 323 {
 324   qvec axis(GetAxis());
 325   f *= Type(0.5);
 326   s = Cos( f );
 327   v = axis * Sin( f );
 328 }
 329
 330 //----------------------------------------------------------------------------
 331 // ScaleAngle
 332 //----------------------------------------------------------------------------
 333 template <class Type>
 334 inline void  Quat<Type>::ScaleAngle( Type f )
 335 {
 336   SetAngle( f * GetAngle() );
 337 }
 338
 339 //----------------------------------------------------------------------------
 340 // GetAngle (radians)
 341 //----------------------------------------------------------------------------
 342 // get rot angle in radians.  Assumes s is between -1 and 1, which will always
 343 // be the case for unit quaternions.
 344 //----------------------------------------------------------------------------
 345 template <class Type>
 346 inline Type Quat<Type>::GetAngle( void ) const
 347 {
 348   return ( Type(2.0) * ACos( s ) );
 349 }
 350
 351 //----------------------------------------------------------------------------
 352 // GetAxis
 353 //----------------------------------------------------------------------------
 354 template <class Type>
 355 Quat<Type>::qvec Quat<Type>::GetAxis( void ) const
 356 {
 357   Type scale;
 358
 359   scale = Sin( acos( s ) ) ;
 360   if ( scale < FUDGE() && scale > -FUDGE() )
 361     return qvec( 0.0, 0.0, 0.0 );
 362   else
 363     return  v / scale;
 364 }
 365
 366 //----------------------------------------------------------------------------
 367 // Print
 368 //----------------------------------------------------------------------------
 369 template <class Type>
 370 inline void Quat<Type>::Print( ) const
 371 {
 372   printf( "(%3.2f, <%3.2f %3.2f %3.2f>)\n", s, v.x, v.y, v.z );
 373 }
 374
 375
 376 //============================================================================
 377 // CONVERSIONS
 378 //============================================================================
 379
 380 template <class Type>
 381 Mat44<Type>& Quat<Type>::ToMat( Mat44<Type>& dest ) const
 382 {
 383   Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
 384   qvec  a, c, b, d;
 385
 386   t  = Type(2.0) / (v*v + s*s);
 387   const Type ONE(1.0);
 388
 389   xs = v.x*t;   ys = v.y*t;   zs = v.z*t;
 390   wx = s*xs;    wy = s*ys;    wz = s*zs;
 391   xx = v.x*xs;  xy = v.x*ys;  xz = v.x*zs;
 392   yy = v.y*ys;  yz = v.y*zs;  zz = v.z*zs;
 393
 394   dest.Set( ONE-(yy+zz), xy-wz,       xz+wy,       0.0,
 395             xy+wz,       ONE-(xx+zz), yz-wx,       0.0,
 396             xz-wy,       yz+wx,       ONE-(xx+yy), 0.0,
 397             0.0,         0.0,         0.0,         ONE );
 398
 399   return dest;
 400 }
 401
 402 template <class Type>
 403 Mat33<Type>&  Quat<Type>::ToMat( Mat33<Type>& dest ) const
 404 {
 405   Type t, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
 406   qvec  a, c, b, d;
 407
 408   t  = Type(2.0) / Norm();
 409   const Type ONE(1.0);
 410
 411   xs = v.x*t;   ys = v.y*t;   zs = v.z*t;
 412   wx = s*xs;    wy = s*ys;    wz = s*zs;
 413   xx = v.x*xs;  xy = v.x*ys;  xz = v.x*zs;
 414   yy = v.y*ys;  yz = v.y*zs;  zz = v.z*zs;
 415
 416   dest.Set( ONE-(yy+zz), xy-wz,       xz+wy,
 417             xy+wz,       ONE-(xx+zz), yz-wx,
 418             xz-wy,       yz+wx,       ONE-(xx+yy) );
 419
 420   return dest;
 421 }
 422
 423 //----------------------------------------------------------------------------
 424 // FromMat
 425 //----------------------------------------------------------------------------
 426 // Convert rotation matrix to quaternion
 427 // Results will be bad if matrix is not (very close to) orthonormal
 428 // Modified from gamasutra.com article:
 429 // http://www.gamasutra.com/features/programming/19980703/quaternions_07.htm
 430 //----------------------------------------------------------------------------
 431 template <class Type>
 432 Quat<Type>&  Quat<Type>::FromMat( const Mat44<Type>& m )
 433 {
 434   Type tr = m.Trace();
 435
 436   // check the diagonal
 437   if (tr > 0.0) {
 438     Type scale = Type( sqrt (tr) );
 439     s = scale * Type(0.5);
 440     scale = Type(0.5) / scale;
 441     v.x = (m(1,2) - m(2,1)) * scale;
 442     v.y = (m(2,0) - m(0,2)) * scale;
 443     v.z = (m(0,1) - m(1,0)) * scale;
 444   } else {
 445     // diagonal is negative or zero
 446     int i, j, k;
 447     i = 0;
 448     if (m(1,1) > m(0,0)) i = 1;
 449     if (m(2,2) > m(i,i)) i = 2;
 450     int nxt[3] = {1, 2, 0};
 451     j = nxt[i];
 452     k = nxt[j];
 453
 454     Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
 455
 456     v[i] = scale * Type(0.5);
 457
 458     if (scale != 0.0) scale = Type(0.5) / scale;
 459
 460     s = (m(j,k) - m(k,j)) * scale;
 461     v[j] = (m(i,j) + m(j,i)) * scale;
 462     v[k] = (m(i,k) + m(k,i)) * scale;
 463   }
 464   return *this;
 465 }
 466
 467 template <class Type>
 468 Quat<Type>&  Quat<Type>::FromMat( const Mat33<Type>& m )
 469 {
 470   Type tr = m.Trace();
 471
 472   // check the diagonal
 473   if (tr > 0.0) {
 474     Type scale = Type( sqrt (tr + Type(1.0)) );
 475     s = scale * Type(0.5);
 476     scale = Type(0.5) / scale;
 477     v.x = (m(1,2) - m(2,1)) * scale;
 478     v.y = (m(2,0) - m(0,2)) * scale;
 479     v.z = (m(0,1) - m(1,0)) * scale;
 480   } else {
 481     // diagonal is negative or zero
 482     int i, j, k;
 483     i = 0;
 484     if (m(1,1) > m(0,0)) i = 1;
 485     if (m(2,2) > m(i,i)) i = 2;
 486     int nxt[3] = {1, 2, 0};
 487     j = nxt[i];
 488     k = nxt[j];
 489
 490     Type scale = Type( sqrt (Type(1.0) + m(i,i) - (m(j,j) + m(k,k)) ) );
 491
 492     v[i] = scale * Type(0.5);
 493
 494     if (scale != 0.0) scale = Type(0.5) / scale;
 495
 496     s = (m(j,k) - m(k,j)) * scale;
 497     v[j] = (m(i,j) + m(j,i)) * scale;
 498     v[k] = (m(i,k) + m(k,i)) * scale;
 499   }
 500   return *this;
 501 }
 502
 503 //----------------------------------------------------------------------------
 504 // ToAngleAxis (radians)
 505 //----------------------------------------------------------------------------
 506 // Convert to angle & axis representation
 507 //----------------------------------------------------------------------------
 508 template <class Type>
 509 void Quat<Type>::ToAngleAxis( Type &angle, qvec &axis ) const
 510 {
 511   Type cinv = ACos( s );
 512   angle = Type(2.0) * cinv;
 513
 514   Type scale;
 515
 516   scale = Sin( cinv );
 517   if ( scale < FUDGE() && scale > -FUDGE() )
 518     axis = qvec::ZERO;
 519   else {
 520     axis = v;
 521     axis /= scale;
 522   }
 523 }
 524
 525 //----------------------------------------------------------------------------
 526 // FromAngleAxis (radians)
 527 //----------------------------------------------------------------------------
 528 // Convert to quat from angle & axis representation
 529 //----------------------------------------------------------------------------
 530 template <class Type>
 531 Quat<Type>& Quat<Type>::FromAngleAxis( Type angle, const qvec &axis )
 532 {
 533   /* normalize vector */
 534   Type length = axis.Length();
 535
 536   /* if zero vector passed in, just set to identity quaternion */
 537   if ( length < FUDGE() )
 538   {
 539     *this = IDENTITY();
 540     return *this;
 541   }
 542   length = Type(1.0)/length;
 543   angle *= 0.5;
 544   v = axis;
 545   v *= length;
 546   v *= Sin(angle);
 547
 548   s = Cos(angle);
 549   return *this;
 550 }
 551
 552 //----------------------------------------------------------------------------
 553 // FromTwoVecs
 554 //----------------------------------------------------------------------------
 555 // Return the quat that rotates vector a into vector b
 556 //----------------------------------------------------------------------------
 557 template <class Type>
 558 Quat<Type>& Quat<Type>::FromTwoVecs(const qvec &a, const qvec& b)
 559 {
 560   qvec u1(a);
 561   qvec u2(b);
 562   double theta ;                                     /* angle of rotation about axis */
 563   double theta_complement ;
 564   double crossProductMagnitude ;
 565
 566
 567   // Normalize both vectors and take cross product to get rotation axis.
 568   u1.Normalize();
 569   u2.Normalize();
 570   qvec axis( u1 ^ u2 );
 571
 572
 573   // | u1 X u2 | = |u1||u2|sin(theta)
 574   //
 575   // Since u1 and u2 are normalized,
 576   //
 577   //  theta = arcsin(|axis|)
 578   crossProductMagnitude = axis.Length();
 579
 580   // Occasionally, even though the vectors are normalized, the
 581   // magnitude will be calculated to be slightly greater than one.  If
 582   // this happens, just set it to 1 or asin() will barf.
 583   if( crossProductMagnitude > Type(1.0) )
 584     crossProductMagnitude = Type(1.0) ;
 585
 586   // Take arcsin of magnitude of rotation axis to compute rotation
 587   // angle.  Since crossProductMagnitude=[0,1], we will have
 588   // theta=[0,pi/2].
 589   theta = ASin( crossProductMagnitude ) ;
 590   theta_complement = Type(3.14159265358979323846) - theta ;
 591
 592   // If cos(theta) < 0, use complement of theta as rotation angle.
 593   if( u1 * u2 < 0.0 )
 594   {
 595     double tmp = theta;
 596     theta = theta_complement ;
 597     theta_complement = tmp;
 598   }
 599
 600   // if angle is 0, just return identity quaternion
 601   if( theta < FUDGE() )
 602   {
 603     *this = IDENTITY();
 604   }
 605   else
 606   {
 607     if( theta_complement < FUDGE() )
 608     {
 609       // The two vectors are opposed.  Find some arbitrary axis vector.
 610       // First try cross product with x-axis if u1 not parallel to x-axis.
 611       if( (u1.y*u1.y + u1.z*u1.z) >= FUDGE() )
 612       {
 613         axis.Set( 0.0, u1.z, -u1.y ) ;
 614       }
 615       else
 616       {
 617         // u1 is parallel to to x-axis.  Use z-axis as axis of rotation.
 618         axis.Set(0.0, 0.0, 1.0);
 619       }
 620     }
 621
 622     axis.Normalize();
 623     FromAngleAxis(Type(theta), axis);
 624     Normalize();
 625   }
 626   return *this;
 627 }
 628
 629 //----------------------------------------------------------------------------
 630 // FromEuler
 631 //----------------------------------------------------------------------------
 632 // converts 3 euler angles (in radians) to a quaternion
 633 //
 634 // angles are in radians; Assumes roll is rotation about X, pitch is
 635 // rotation about Y, yaw is about Z.  (So thinking of
 636 // Z as up) Assumes order of yaw, pitch, roll applied as follows:
 637 //
 638 //          p' = roll( pitch( yaw(p) ) )
 639 //
 640 // Where yaw, pitch, and roll are defined in the BODY coordinate sys.
 641 // In other words these are ZYX-relative (or XYZ-fixed) Euler Angles.
 642 //
 643 // For a complete Euler angle implementation that handles all 24 angle
 644 // sets, see "Euler Angle Conversion" by Ken Shoemake, in "Graphics
 645 // Gems IV", Academic Press, 1994
 646 //----------------------------------------------------------------------------
 647 template <class Type>
 648 Quat<Type>& Quat<Type>::FromEuler(Type yaw, Type pitch, Type roll)
 649 {
 650   Type  cosYaw, sinYaw, cosPitch, sinPitch, cosRoll, sinRoll;
 651   Type  half_roll, half_pitch, half_yaw;
 652
 653   /* put angles into radians and divide by two, since all angles in formula
 654    *  are (angle/2)
 655    */
 656   const Type HALF(0.5);
 657   half_yaw   = yaw   * HALF;
 658   half_pitch = pitch * HALF;
 659   half_roll  = roll  * HALF;
 660
 661   cosYaw = Cos(half_yaw);
 662   sinYaw = Sin(half_yaw);
 663
 664   cosPitch = Cos(half_pitch);
 665   sinPitch = Sin(half_pitch);
 666
 667   cosRoll = Cos(half_roll);
 668   sinRoll = Sin(half_roll);
 669
 670   Type cpcy = cosPitch * cosYaw;
 671   Type spsy = sinPitch * sinYaw;
 672
 673   v.x = sinRoll * cpcy              - cosRoll * spsy;
 674   v.y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
 675   v.z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
 676   s   = cosRoll * cpcy              + sinRoll * spsy;
 677
 678   return *this;
 679 }
 680
 681 //----------------------------------------------------------------------------
 682 // ToEuler
 683 //----------------------------------------------------------------------------
 684 // converts a quaternion to  3 euler angles (in radians)
 685 //
 686 // See FromEuler for details of which set of Euler Angles are returned
 687 //----------------------------------------------------------------------------
 688 template <class Type>
 689 void Quat<Type>::ToEuler(Type& yaw, Type& pitch, Type& roll) const
 690 {
 691   // This is probably wrong
 692   Mat33<Type> M;
 693   ToMat(M);
 694   const int i = 0, j = 1, k = 2;
 695   double cy = sqrt(M(i,i)*M(i,i) + M(i,j)*M(i,j));
 696   if (cy > FUDGE()) {
 697     roll = ATan2(M(j,k), M(k,k));
 698     pitch = ATan2(-M(i,k), cy);
 699     yaw = ATan2(M(i,j), M(i,i));
 700   } else {
 701     roll = ATan2(-M(k,j), M(j,j));
 702     pitch = ATan2(-M(i,k), cy);
 703     yaw = 0;
 704   }
 705 }
 706
 707 //============================================================================
 708 // QUAT FRIENDS
 709 //============================================================================
 710
 711
 712 template <class Type>
 713 Quat<Type> operator + (const Quat<Type> &a, const Quat<Type> &b)
 714 {
 715   return Quat<Type>( a.s+b.s, a.v+b.v );
 716 }
 717
 718 template <class Type>
 719 Quat<Type> operator - (const Quat<Type> &a, const Quat<Type> &b)
 720 {
 721   return Quat<Type>( a.s-b.s, a.v-b.v );
 722 }
 723
 724 template <class Type>
 725 Quat<Type> operator - (const Quat<Type> &a )
 726 {
 727   return Quat<Type>( -a.s, -a.v );
 728 }
 729
 730 template <class Type>
 731 Quat<Type> operator * ( const Quat<Type> &a, const Quat<Type> &b)
 732 {
 733 #if 0
 734   // 16 mults
 735   return Quat<Type>( a.s*b.s - a.v*b.v, a.s*b.v + b.s*a.v + a.v^b.v );
 736 #else
 737   // optimized (12 mults, and no compiler-generated temp objects)
 738   Type A, B, C, D, E, F, G, H;
 739
 740   A = (a.s   + a.v.x)*(b.s   + b.v.x);
 741   B = (a.v.z - a.v.y)*(b.v.y - b.v.z);
 742   C = (a.s   - a.v.x)*(b.v.y + b.v.z);
 743   D = (a.v.y + a.v.z)*(b.s   - b.v.x);
 744   E = (a.v.x + a.v.z)*(b.v.x + b.v.y);
 745   F = (a.v.x - a.v.z)*(b.v.x - b.v.y);
 746   G = (a.s   + a.v.y)*(b.s   - b.v.z);
 747   H = (a.s   - a.v.y)*(b.s   + b.v.z);
 748
 749   return Quat<Type>(
 750     B + (-E - F + G + H) * Type(0.5),
 751     A - (E + F + G + H) * Type(0.5),
 752     C + (E - F + G - H) * Type(0.5),
 753     D + (E - F - G + H) * Type(0.5));
 754 #endif
 755 }
 756
 757 template <class Type>
 758 Quat<Type> operator * ( const Quat<Type> &a, const Type t)
 759 {
 760   return Quat<Type>( a.v * t, a.s * t );
 761 }
 762
 763 template <class Type>
 764 Quat<Type> operator * ( const Type t, const Quat<Type> &a )
 765 {
 766   return Quat<Type>( a.v * t, a.s * t );
 767 }
 768 template <class Type>
 769 Quat<Type> operator / ( const Quat<Type> &a, const Type t )
 770 {
 771   return Quat<Type>( a.v / t, a.s / t );
 772 }
 773
 774 template <class Type>
 775 bool operator == (const Quat<Type> &a, const Quat<Type> &b)
 776 {
 777   return (a.s == b.s && a.v == b.v);
 778 }
 779 template <class Type>
 780 bool operator != (const Quat<Type> &a, const Quat<Type> &b)
 781 {
 782   return (a.s != b.s || a.v != b.v);
 783 }
 784
 785
 786 //============================================================================
 787 // UTILS
 788 //============================================================================
 789 template <class Type>
 790 inline Type Quat<Type>::DEG2RAD(Type d)
 791 {
 792   return d * Type(0.0174532925199432957692369076848861);
 793 }
 794 template <class Type>
 795 inline Type Quat<Type>::RAD2DEG(Type d)
 796 {
 797   return d * Type(57.2957795130823208767981548141052);
 798 }
 799
 800 template <class Type>
 801 inline Type Quat<Type>::Sin(double d)  { return Type(sin(d)); }
 802 template <class Type>
 803 inline Type Quat<Type>::Cos(double d)  { return Type(cos(d)); }
 804 template <class Type>
 805 inline Type Quat<Type>::ACos(double d) { return Type(acos(d)); }
 806 template <class Type>
 807 inline Type Quat<Type>::ASin(double d) { return Type(asin(d)); }
 808 template <class Type>
 809 inline Type Quat<Type>::ATan(double d) { return Type(atan(d)); }
 810 template <class Type>
 811 inline Type Quat<Type>::ATan2(double n, double d) {return Type(atan2(n,d));}
 812
 813 template <class Type>
 814 inline Quat<Type> Quat<Type>::ZERO() {return Quat(0,0,0,0); }
 815 template <class Type>
 816 inline Quat<Type> Quat<Type>::IDENTITY() {return Quat(1,0,0,0); }
 817
 818 template<class Type>
 819 inline Type Quat<Type>::FUDGE()    { return 1e-6; }
 820 template<>
 821 inline double Quat<double>::FUDGE() { return 1e-10; }
 822
 823 //----------------------------------------------------------------------------
 824 // QuatSlerp
 825 //----------------------------------------------------------------------------
 826 template <class Type>
 827 Quat<Type>& QuatSlerp(
 828   Quat<Type> &dest,
 829   const Quat<Type> &from, const Quat<Type> &to, Type t )
 830 {
 831 #if 0
 832   // compact mathematical version
 833   // exp(t*log(to*from^-1))*from
 834   Quat<Type> fminv(from);
 835   Quat<Type> tofrom(to*fminv.Invert());
 836   Quat<Type> slerp = t*tofrom.Log();
 837   slerp.Exp();
 838   slerp *= from;
 839   return slerp;
 840 #endif
 841   Quat<Type> to1;
 842   double omega, cosom, sinom, scale0, scale1;
 843
 844   /* calculate cosine */
 845   cosom = from.v * to.v + from.s + to.s;
 846
 847   /* Adjust signs (if necessary) so we take shorter path */
 848   if ( cosom < 0.0 ) {
 849     cosom = -cosom;
 850     to1 = -to;
 851   }
 852   else
 853   {
 854     to1 = to;
 855   }
 856
 857   /* Calculate coefficients */
 858   if ((1.0 - cosom) > Quat<Type>::FUDGE ) {
 859     /* standard case (slerp) */
 860     omega = acos( cosom );
 861     sinom = sin( omega );
 862     scale0 = sin((1.0 - t) * omega) / sinom;
 863     scale1 = sin(t * omega) / sinom;
 864   }
 865   else {
 866     /* 'from' and 'to' are very close - just do linear interpolation */
 867     scale0 = 1.0 - t;
 868     scale1 = t;
 869   }
 870
 871   dest = from;
 872   dest *= Type(scale0);
 873   dest += Type(scale1) * to1;
 874   return dest;
 875 }
 876
 877 // This version creates more temporary objects
 878 template <class Type>
 879 inline Quat<Type> QuatSlerp(
 880   const Quat<Type>& from, const Quat<Type>& to, Type t )
 881 {
 882   Quat<Type> q;
 883   return QuatSlerp(q, from, to, t);
 884 }
 885
 886