1/////////////////////////////////////////////////////////////////////////// 2// 3// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas 4// Digital Ltd. LLC 5// 6// All rights reserved. 7// 8// Redistribution and use in source and binary forms, with or without 9// modification, are permitted provided that the following conditions are 10// met: 11// * Redistributions of source code must retain the above copyright 12// notice, this list of conditions and the following disclaimer. 13// * Redistributions in binary form must reproduce the above 14// copyright notice, this list of conditions and the following disclaimer 15// in the documentation and/or other materials provided with the 16// distribution. 17// * Neither the name of Industrial Light & Magic nor the names of 18// its contributors may be used to endorse or promote products derived 19// from this software without specific prior written permission. 20// 21// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 24// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32// 33/////////////////////////////////////////////////////////////////////////// 34 35 36 37#ifndef INCLUDED_IMATHQUAT_H 38#define INCLUDED_IMATHQUAT_H 39 40//---------------------------------------------------------------------- 41// 42// template class Quat<T> 43// 44// "Quaternions came from Hamilton ... and have been an unmixed 45// evil to those who have touched them in any way. Vector is a 46// useless survival ... and has never been of the slightest use 47// to any creature." 48// 49// - Lord Kelvin 50// 51// This class implements the quaternion numerical type -- you 52// will probably want to use this class to represent orientations 53// in R3 and to convert between various euler angle reps. You 54// should probably use Imath::Euler<> for that. 55// 56//---------------------------------------------------------------------- 57 58#include "ImathExc.h" 59#include "ImathMatrix.h" 60 61#include <iostream> 62 63namespace Imath { 64 65#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 66// Disable MS VC++ warnings about conversion from double to float 67#pragma warning(disable:4244) 68#endif 69 70template <class T> 71class Quat; 72 73template<class T> 74Quat<T> slerp (const Quat<T> &q1,const Quat<T> &q2, T t); 75 76template<class T> 77Quat<T> squad (const Quat<T> &q1,const Quat<T> &q2, 78 const Quat<T> &qa,const Quat<T> &qb, T t); 79 80template<class T> 81void intermediate (const Quat<T> &q0, const Quat<T> &q1, 82 const Quat<T> &q2, const Quat<T> &q3, 83 Quat<T> &qa, Quat<T> &qb); 84 85template <class T> 86class Quat 87{ 88 public: 89 90 T r; // real part 91 Vec3<T> v; // imaginary vector 92 93 //----------------------------------------------------- 94 // Constructors - default constructor is identity quat 95 //----------------------------------------------------- 96 97 Quat() : r(1), v(0,0,0) {} 98 99 template <class S> 100 Quat( const Quat<S>& q) : r(q.r), v(q.v) {} 101 102 Quat( T s, T i, T j, T k ) : r(s), v(i,j,k) {} 103 104 Quat( T s, Vec3<T> d ) : r(s), v(d) {} 105 106 static Quat<T> identity() { return Quat<T>(); } 107 108 //------------------------------------------------ 109 // Basic Algebra - Operators and Methods 110 // The operator return values are *NOT* normalized 111 // 112 // operator^ is 4D dot product 113 // operator/ uses the inverse() quaternion 114 // operator~ is conjugate -- if (S+V) is quat then 115 // the conjugate (S+V)* == (S-V) 116 // 117 // some operators (*,/,*=,/=) treat the quat as 118 // a 4D vector when one of the operands is scalar 119 //------------------------------------------------ 120 121 const Quat<T>& operator= (const Quat<T>&); 122 const Quat<T>& operator*= (const Quat<T>&); 123 const Quat<T>& operator*= (T); 124 const Quat<T>& operator/= (const Quat<T>&); 125 const Quat<T>& operator/= (T); 126 const Quat<T>& operator+= (const Quat<T>&); 127 const Quat<T>& operator-= (const Quat<T>&); 128 T& operator[] (int index); // as 4D vector 129 T operator[] (int index) const; 130 131 template <class S> bool operator == (const Quat<S> &q) const; 132 template <class S> bool operator != (const Quat<S> &q) const; 133 134 Quat<T>& invert(); // this -> 1 / this 135 Quat<T> inverse() const; 136 Quat<T>& normalize(); // returns this 137 Quat<T> normalized() const; 138 T length() const; // in R4 139 140 //----------------------- 141 // Rotation conversion 142 //----------------------- 143 144 Quat<T>& setAxisAngle(const Vec3<T>& axis, T radians); 145 Quat<T>& setRotation(const Vec3<T>& fromDirection, 146 const Vec3<T>& toDirection); 147 148 T angle() const; 149 Vec3<T> axis() const; 150 151 Matrix33<T> toMatrix33() const; 152 Matrix44<T> toMatrix44() const; 153 154 Quat<T> log() const; 155 Quat<T> exp() const; 156 157 private: 158 159 void setRotationInternal (const Vec3<T>& f0, 160 const Vec3<T>& t0, 161 Quat<T> &q); 162}; 163 164 165//-------------------- 166// Convenient typedefs 167//-------------------- 168 169typedef Quat<float> Quatf; 170typedef Quat<double> Quatd; 171 172 173//--------------- 174// Implementation 175//--------------- 176 177template<class T> 178inline const Quat<T>& Quat<T>::operator= (const Quat<T>& q) 179{ 180 r = q.r; 181 v = q.v; 182 return *this; 183} 184 185template<class T> 186inline const Quat<T>& Quat<T>::operator*= (const Quat<T>& q) 187{ 188 T rtmp = r * q.r - (v ^ q.v); 189 v = r * q.v + v * q.r + v % q.v; 190 r = rtmp; 191 return *this; 192} 193 194template<class T> 195inline const Quat<T>& Quat<T>::operator*= (T t) 196{ 197 r *= t; 198 v *= t; 199 return *this; 200} 201 202template<class T> 203inline const Quat<T>& Quat<T>::operator/= (const Quat<T>& q) 204{ 205 *this = *this * q.inverse(); 206 return *this; 207} 208 209template<class T> 210inline const Quat<T>& Quat<T>::operator/= (T t) 211{ 212 r /= t; 213 v /= t; 214 return *this; 215} 216 217template<class T> 218inline const Quat<T>& Quat<T>::operator+= (const Quat<T>& q) 219{ 220 r += q.r; 221 v += q.v; 222 return *this; 223} 224 225template<class T> 226inline const Quat<T>& Quat<T>::operator-= (const Quat<T>& q) 227{ 228 r -= q.r; 229 v -= q.v; 230 return *this; 231} 232template<class T> 233inline T& Quat<T>::operator[] (int index) 234{ 235 return index ? v[index-1] : r; 236} 237 238template<class T> 239inline T Quat<T>::operator[] (int index) const 240{ 241 return index ? v[index-1] : r; 242} 243 244template <class T> 245template <class S> 246inline bool 247Quat<T>::operator == (const Quat<S> &q) const 248{ 249 return r == q.r && v == q.v; 250} 251 252template <class T> 253template <class S> 254inline bool 255Quat<T>::operator != (const Quat<S> &q) const 256{ 257 return r != q.r || v != q.v; 258} 259 260template<class T> 261inline T operator^ (const Quat<T>& q1,const Quat<T>& q2) 262{ 263 return q1.r * q2.r + (q1.v ^ q2.v); 264} 265 266template <class T> 267inline T Quat<T>::length() const 268{ 269 return Math<T>::sqrt( r * r + (v ^ v) ); 270} 271 272template <class T> 273inline Quat<T>& Quat<T>::normalize() 274{ 275 if ( T l = length() ) { r /= l; v /= l; } 276 else { r = 1; v = Vec3<T>(0); } 277 return *this; 278} 279 280template <class T> 281inline Quat<T> Quat<T>::normalized() const 282{ 283 if ( T l = length() ) return Quat( r / l, v / l ); 284 return Quat(); 285} 286 287template<class T> 288inline Quat<T> Quat<T>::inverse() const 289{ 290 // 1 Q* 291 // - = ---- where Q* is conjugate (operator~) 292 // Q Q* Q and (Q* Q) == Q ^ Q (4D dot) 293 294 T qdot = *this ^ *this; 295 return Quat( r / qdot, -v / qdot ); 296} 297 298template<class T> 299inline Quat<T>& Quat<T>::invert() 300{ 301 T qdot = (*this) ^ (*this); 302 r /= qdot; 303 v = -v / qdot; 304 return *this; 305} 306 307 308template<class T> 309T 310angle4D (const Quat<T> &q1, const Quat<T> &q2) 311{ 312 // 313 // Compute the angle between two quaternions, 314 // interpreting the quaternions as 4D vectors. 315 // 316 317 Quat<T> d = q1 - q2; 318 T lengthD = Math<T>::sqrt (d ^ d); 319 320 Quat<T> s = q1 + q2; 321 T lengthS = Math<T>::sqrt (s ^ s); 322 323 return 2 * Math<T>::atan2 (lengthD, lengthS); 324} 325 326 327template<class T> 328Quat<T> 329slerp(const Quat<T> &q1,const Quat<T> &q2, T t) 330{ 331 // 332 // Spherical linear interpolation. 333 // Assumes q1 and q2 are normalized and that q1 != -q2. 334 // 335 // This method does *not* interpolate along the shortest 336 // arc between q1 and q2. If you desire interpolation 337 // along the shortest arc, and q1^q2 is negative, then 338 // consider flipping the second quaternion explicitly. 339 // 340 // The implementation of squad() depends on a slerp() 341 // that interpolates as is, without the automatic 342 // flipping. 343 // 344 // Don Hatch explains the method we use here on his 345 // web page, The Right Way to Calculate Stuff, at 346 // http://www.plunk.org/~hatch/rightway.php 347 // 348 349 T a = angle4D (q1, q2); 350 T s = 1 - t; 351 352 Quat<T> q = sinx_over_x (s * a) / sinx_over_x (a) * s * q1 + 353 sinx_over_x (t * a) / sinx_over_x (a) * t * q2; 354 355 return q.normalized(); 356} 357 358 359template<class T> 360Quat<T> spline(const Quat<T> &q0, const Quat<T> &q1, 361 const Quat<T> &q2, const Quat<T> &q3, 362 T t) 363{ 364 // Spherical Cubic Spline Interpolation - 365 // from Advanced Animation and Rendering 366 // Techniques by Watt and Watt, Page 366: 367 // A spherical curve is constructed using three 368 // spherical linear interpolations of a quadrangle 369 // of unit quaternions: q1, qa, qb, q2. 370 // Given a set of quaternion keys: q0, q1, q2, q3, 371 // this routine does the interpolation between 372 // q1 and q2 by constructing two intermediate 373 // quaternions: qa and qb. The qa and qb are 374 // computed by the intermediate function to 375 // guarantee the continuity of tangents across 376 // adjacent cubic segments. The qa represents in-tangent 377 // for q1 and the qb represents the out-tangent for q2. 378 // 379 // The q1 q2 is the cubic segment being interpolated. 380 // The q0 is from the previous adjacent segment and q3 is 381 // from the next adjacent segment. The q0 and q3 are used 382 // in computing qa and qb. 383 // 384 385 Quat<T> qa = intermediate (q0, q1, q2); 386 Quat<T> qb = intermediate (q1, q2, q3); 387 Quat<T> result = squad(q1, qa, qb, q2, t); 388 389 return result; 390} 391 392template<class T> 393Quat<T> squad(const Quat<T> &q1, const Quat<T> &qa, 394 const Quat<T> &qb, const Quat<T> &q2, 395 T t) 396{ 397 // Spherical Quadrangle Interpolation - 398 // from Advanced Animation and Rendering 399 // Techniques by Watt and Watt, Page 366: 400 // It constructs a spherical cubic interpolation as 401 // a series of three spherical linear interpolations 402 // of a quadrangle of unit quaternions. 403 // 404 405 Quat<T> r1 = slerp(q1, q2, t); 406 Quat<T> r2 = slerp(qa, qb, t); 407 Quat<T> result = slerp(r1, r2, 2*t*(1-t)); 408 409 return result; 410} 411 412template<class T> 413Quat<T> intermediate(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2) 414{ 415 // From advanced Animation and Rendering 416 // Techniques by Watt and Watt, Page 366: 417 // computing the inner quadrangle 418 // points (qa and qb) to guarantee tangent 419 // continuity. 420 // 421 Quat<T> q1inv = q1.inverse(); 422 Quat<T> c1 = q1inv*q2; 423 Quat<T> c2 = q1inv*q0; 424 Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log()); 425 Quat<T> qa = q1 * c3.exp(); 426 qa.normalize(); 427 return qa; 428} 429 430template <class T> 431inline Quat<T> Quat<T>::log() const 432{ 433 // 434 // For unit quaternion, from Advanced Animation and 435 // Rendering Techniques by Watt and Watt, Page 366: 436 // 437 438 T theta = Math<T>::acos (std::min (r, (T) 1.0)); 439 440 if (theta == 0) 441 return Quat<T> (0, v); 442 443 T sintheta = Math<T>::sin (theta); 444 445 T k; 446 if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta)) 447 k = 1; 448 else 449 k = theta / sintheta; 450 451 return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k); 452} 453 454template <class T> 455inline Quat<T> Quat<T>::exp() const 456{ 457 // 458 // For pure quaternion (zero scalar part): 459 // from Advanced Animation and Rendering 460 // Techniques by Watt and Watt, Page 366: 461 // 462 463 T theta = v.length(); 464 T sintheta = Math<T>::sin (theta); 465 466 T k; 467 if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta)) 468 k = 1; 469 else 470 k = sintheta / theta; 471 472 T costheta = Math<T>::cos (theta); 473 474 return Quat<T> (costheta, v.x * k, v.y * k, v.z * k); 475} 476 477template <class T> 478inline T Quat<T>::angle() const 479{ 480 return 2.0*Math<T>::acos(r); 481} 482 483template <class T> 484inline Vec3<T> Quat<T>::axis() const 485{ 486 return v.normalized(); 487} 488 489template <class T> 490inline Quat<T>& Quat<T>::setAxisAngle(const Vec3<T>& axis, T radians) 491{ 492 r = Math<T>::cos(radians/2); 493 v = axis.normalized() * Math<T>::sin(radians/2); 494 return *this; 495} 496 497 498template <class T> 499Quat<T>& 500Quat<T>::setRotation(const Vec3<T>& from, const Vec3<T>& to) 501{ 502 // 503 // Create a quaternion that rotates vector from into vector to, 504 // such that the rotation is around an axis that is the cross 505 // product of from and to. 506 // 507 // This function calls function setRotationInternal(), which is 508 // numerically accurate only for rotation angles that are not much 509 // greater than pi/2. In order to achieve good accuracy for angles 510 // greater than pi/2, we split large angles in half, and rotate in 511 // two steps. 512 // 513 514 // 515 // Normalize from and to, yielding f0 and t0. 516 // 517 518 Vec3<T> f0 = from.normalized(); 519 Vec3<T> t0 = to.normalized(); 520 521 if ((f0 ^ t0) >= 0) 522 { 523 // 524 // The rotation angle is less than or equal to pi/2. 525 // 526 527 setRotationInternal (f0, t0, *this); 528 } 529 else 530 { 531 // 532 // The angle is greater than pi/2. After computing h0, 533 // which is halfway between f0 and t0, we rotate first 534 // from f0 to h0, then from h0 to t0. 535 // 536 537 Vec3<T> h0 = (f0 + t0).normalized(); 538 539 if ((h0 ^ h0) != 0) 540 { 541 setRotationInternal (f0, h0, *this); 542 543 Quat<T> q; 544 setRotationInternal (h0, t0, q); 545 546 *this *= q; 547 } 548 else 549 { 550 // 551 // f0 and t0 point in exactly opposite directions. 552 // Pick an arbitrary axis that is orthogonal to f0, 553 // and rotate by pi. 554 // 555 556 r = T (0); 557 558 Vec3<T> f02 = f0 * f0; 559 560 if (f02.x <= f02.y && f02.x <= f02.z) 561 v = (f0 % Vec3<T> (1, 0, 0)).normalized(); 562 else if (f02.y <= f02.z) 563 v = (f0 % Vec3<T> (0, 1, 0)).normalized(); 564 else 565 v = (f0 % Vec3<T> (0, 0, 1)).normalized(); 566 } 567 } 568 569 return *this; 570} 571 572 573template <class T> 574void 575Quat<T>::setRotationInternal (const Vec3<T>& f0, const Vec3<T>& t0, Quat<T> &q) 576{ 577 // 578 // The following is equivalent to setAxisAngle(n,2*phi), 579 // where the rotation axis, is orthogonal to the f0 and 580 // t0 vectors, and 2*phi is the angle between f0 and t0. 581 // 582 // This function is called by setRotation(), above; it assumes 583 // that f0 and t0 are normalized and that the angle between 584 // them is not much greater than pi/2. This function becomes 585 // numerically inaccurate if f0 and t0 point into nearly 586 // opposite directions. 587 // 588 589 // 590 // Find a normalized vector, h0, that is half way between f0 and t0. 591 // The angle between f0 and h0 is phi. 592 // 593 594 Vec3<T> h0 = (f0 + t0).normalized(); 595 596 // 597 // Store the rotation axis and rotation angle. 598 // 599 600 q.r = f0 ^ h0; // f0 ^ h0 == cos (phi) 601 q.v = f0 % h0; // (f0 % h0).length() == sin (phi) 602} 603 604 605template<class T> 606Matrix33<T> Quat<T>::toMatrix33() const 607{ 608 return Matrix33<T>(1. - 2.0 * (v.y * v.y + v.z * v.z), 609 2.0 * (v.x * v.y + v.z * r), 610 2.0 * (v.z * v.x - v.y * r), 611 612 2.0 * (v.x * v.y - v.z * r), 613 1. - 2.0 * (v.z * v.z + v.x * v.x), 614 2.0 * (v.y * v.z + v.x * r), 615 616 2.0 * (v.z * v.x + v.y * r), 617 2.0 * (v.y * v.z - v.x * r), 618 1. - 2.0 * (v.y * v.y + v.x * v.x)); 619} 620 621template<class T> 622Matrix44<T> Quat<T>::toMatrix44() const 623{ 624 return Matrix44<T>(1. - 2.0 * (v.y * v.y + v.z * v.z), 625 2.0 * (v.x * v.y + v.z * r), 626 2.0 * (v.z * v.x - v.y * r), 627 0., 628 2.0 * (v.x * v.y - v.z * r), 629 1. - 2.0 * (v.z * v.z + v.x * v.x), 630 2.0 * (v.y * v.z + v.x * r), 631 0., 632 2.0 * (v.z * v.x + v.y * r), 633 2.0 * (v.y * v.z - v.x * r), 634 1. - 2.0 * (v.y * v.y + v.x * v.x), 635 0., 636 0., 637 0., 638 0., 639 1.0 ); 640} 641 642 643template<class T> 644inline Matrix33<T> operator* (const Matrix33<T> &M, const Quat<T> &q) 645{ 646 return M * q.toMatrix33(); 647} 648 649template<class T> 650inline Matrix33<T> operator* (const Quat<T> &q, const Matrix33<T> &M) 651{ 652 return q.toMatrix33() * M; 653} 654 655template<class T> 656std::ostream& operator<< (std::ostream &o, const Quat<T> &q) 657{ 658 return o << "(" << q.r 659 << " " << q.v.x 660 << " " << q.v.y 661 << " " << q.v.z 662 << ")"; 663 664} 665 666template<class T> 667inline Quat<T> operator* (const Quat<T>& q1, const Quat<T>& q2) 668{ 669 // (S1+V1) (S2+V2) = S1 S2 - V1.V2 + S1 V2 + V1 S2 + V1 x V2 670 return Quat<T>( q1.r * q2.r - (q1.v ^ q2.v), 671 q1.r * q2.v + q1.v * q2.r + q1.v % q2.v ); 672} 673 674template<class T> 675inline Quat<T> operator/ (const Quat<T>& q1, const Quat<T>& q2) 676{ 677 return q1 * q2.inverse(); 678} 679 680template<class T> 681inline Quat<T> operator/ (const Quat<T>& q,T t) 682{ 683 return Quat<T>(q.r/t,q.v/t); 684} 685 686template<class T> 687inline Quat<T> operator* (const Quat<T>& q,T t) 688{ 689 return Quat<T>(q.r*t,q.v*t); 690} 691 692template<class T> 693inline Quat<T> operator* (T t, const Quat<T>& q) 694{ 695 return Quat<T>(q.r*t,q.v*t); 696} 697 698template<class T> 699inline Quat<T> operator+ (const Quat<T>& q1, const Quat<T>& q2) 700{ 701 return Quat<T>( q1.r + q2.r, q1.v + q2.v ); 702} 703 704template<class T> 705inline Quat<T> operator- (const Quat<T>& q1, const Quat<T>& q2) 706{ 707 return Quat<T>( q1.r - q2.r, q1.v - q2.v ); 708} 709 710template<class T> 711inline Quat<T> operator~ (const Quat<T>& q) 712{ 713 return Quat<T>( q.r, -q.v ); // conjugate: (S+V)* = S-V 714} 715 716template<class T> 717inline Quat<T> operator- (const Quat<T>& q) 718{ 719 return Quat<T>( -q.r, -q.v ); 720} 721 722template<class T> 723inline Vec3<T> operator* (const Vec3<T>& v, const Quat<T>& q) 724{ 725 Vec3<T> a = q.v % v; 726 Vec3<T> b = q.v % a; 727 return v + T (2) * (q.r * a + b); 728} 729 730#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 731#pragma warning(default:4244) 732#endif 733 734} // namespace Imath 735 736#endif 737