PBRT
|
00001 00002 // 00003 // Copyright (c) 2002, Industrial Light & Magic, a division of Lucas 00004 // Digital Ltd. LLC 00005 // 00006 // All rights reserved. 00007 // 00008 // Redistribution and use in source and binary forms, with or without 00009 // modification, are permitted provided that the following conditions are 00010 // met: 00011 // * Redistributions of source code must retain the above copyright 00012 // notice, this list of conditions and the following disclaimer. 00013 // * Redistributions in binary form must reproduce the above 00014 // copyright notice, this list of conditions and the following disclaimer 00015 // in the documentation and/or other materials provided with the 00016 // distribution. 00017 // * Neither the name of Industrial Light & Magic nor the names of 00018 // its contributors may be used to endorse or promote products derived 00019 // from this software without specific prior written permission. 00020 // 00021 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 00024 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 00025 // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 00026 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 00027 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 00028 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 00029 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00030 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00031 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00032 // 00034 00035 00036 00037 #ifndef INCLUDED_IMATHQUAT_H 00038 #define INCLUDED_IMATHQUAT_H 00039 00040 //---------------------------------------------------------------------- 00041 // 00042 // template class Quat<T> 00043 // 00044 // "Quaternions came from Hamilton ... and have been an unmixed 00045 // evil to those who have touched them in any way. Vector is a 00046 // useless survival ... and has never been of the slightest use 00047 // to any creature." 00048 // 00049 // - Lord Kelvin 00050 // 00051 // This class implements the quaternion numerical type -- you 00052 // will probably want to use this class to represent orientations 00053 // in R3 and to convert between various euler angle reps. You 00054 // should probably use Imath::Euler<> for that. 00055 // 00056 //---------------------------------------------------------------------- 00057 00058 #include "ImathExc.h" 00059 #include "ImathMatrix.h" 00060 00061 #include <iostream> 00062 00063 namespace Imath { 00064 00065 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 00066 // Disable MS VC++ warnings about conversion from double to float 00067 #pragma warning(disable:4244) 00068 #endif 00069 00070 template <class T> 00071 class Quat 00072 { 00073 public: 00074 00075 T r; // real part 00076 Vec3<T> v; // imaginary vector 00077 00078 00079 //----------------------------------------------------- 00080 // Constructors - default constructor is identity quat 00081 //----------------------------------------------------- 00082 00083 Quat (); 00084 00085 template <class S> 00086 Quat (const Quat<S> &q); 00087 00088 Quat (T s, T i, T j, T k); 00089 00090 Quat (T s, Vec3<T> d); 00091 00092 static Quat<T> identity (); 00093 00094 00095 //------------------------------------------------- 00096 // Basic Algebra - Operators and Methods 00097 // The operator return values are *NOT* normalized 00098 // 00099 // operator^ and euclideanInnnerProduct() both 00100 // implement the 4D dot product 00101 // 00102 // operator/ uses the inverse() quaternion 00103 // 00104 // operator~ is conjugate -- if (S+V) is quat then 00105 // the conjugate (S+V)* == (S-V) 00106 // 00107 // some operators (*,/,*=,/=) treat the quat as 00108 // a 4D vector when one of the operands is scalar 00109 //------------------------------------------------- 00110 00111 const Quat<T> & operator = (const Quat<T> &q); 00112 const Quat<T> & operator *= (const Quat<T> &q); 00113 const Quat<T> & operator *= (T t); 00114 const Quat<T> & operator /= (const Quat<T> &q); 00115 const Quat<T> & operator /= (T t); 00116 const Quat<T> & operator += (const Quat<T> &q); 00117 const Quat<T> & operator -= (const Quat<T> &q); 00118 T & operator [] (int index); // as 4D vector 00119 T operator [] (int index) const; 00120 00121 template <class S> bool operator == (const Quat<S> &q) const; 00122 template <class S> bool operator != (const Quat<S> &q) const; 00123 00124 Quat<T> & invert (); // this -> 1 / this 00125 Quat<T> inverse () const; 00126 Quat<T> & normalize (); // returns this 00127 Quat<T> normalized () const; 00128 T length () const; // in R4 00129 Vec3<T> rotateVector(const Vec3<T> &original) const; 00130 T euclideanInnerProduct(const Quat<T> &q) const; 00131 00132 //----------------------- 00133 // Rotation conversion 00134 //----------------------- 00135 00136 Quat<T> & setAxisAngle (const Vec3<T> &axis, T radians); 00137 00138 Quat<T> & setRotation (const Vec3<T> &fromDirection, 00139 const Vec3<T> &toDirection); 00140 00141 T angle () const; 00142 Vec3<T> axis () const; 00143 00144 Matrix33<T> toMatrix33 () const; 00145 Matrix44<T> toMatrix44 () const; 00146 00147 Quat<T> log () const; 00148 Quat<T> exp () const; 00149 00150 00151 private: 00152 00153 void setRotationInternal (const Vec3<T> &f0, 00154 const Vec3<T> &t0, 00155 Quat<T> &q); 00156 }; 00157 00158 00159 template<class T> 00160 Quat<T> slerp (const Quat<T> &q1, const Quat<T> &q2, T t); 00161 00162 template<class T> 00163 Quat<T> slerpShortestArc 00164 (const Quat<T> &q1, const Quat<T> &q2, T t); 00165 00166 00167 template<class T> 00168 Quat<T> squad (const Quat<T> &q1, const Quat<T> &q2, 00169 const Quat<T> &qa, const Quat<T> &qb, T t); 00170 00171 template<class T> 00172 void intermediate (const Quat<T> &q0, const Quat<T> &q1, 00173 const Quat<T> &q2, const Quat<T> &q3, 00174 Quat<T> &qa, Quat<T> &qb); 00175 00176 template<class T> 00177 Matrix33<T> operator * (const Matrix33<T> &M, const Quat<T> &q); 00178 00179 template<class T> 00180 Matrix33<T> operator * (const Quat<T> &q, const Matrix33<T> &M); 00181 00182 template<class T> 00183 std::ostream & operator << (std::ostream &o, const Quat<T> &q); 00184 00185 template<class T> 00186 Quat<T> operator * (const Quat<T> &q1, const Quat<T> &q2); 00187 00188 template<class T> 00189 Quat<T> operator / (const Quat<T> &q1, const Quat<T> &q2); 00190 00191 template<class T> 00192 Quat<T> operator / (const Quat<T> &q, T t); 00193 00194 template<class T> 00195 Quat<T> operator * (const Quat<T> &q, T t); 00196 00197 template<class T> 00198 Quat<T> operator * (T t, const Quat<T> &q); 00199 00200 template<class T> 00201 Quat<T> operator + (const Quat<T> &q1, const Quat<T> &q2); 00202 00203 template<class T> 00204 Quat<T> operator - (const Quat<T> &q1, const Quat<T> &q2); 00205 00206 template<class T> 00207 Quat<T> operator ~ (const Quat<T> &q); 00208 00209 template<class T> 00210 Quat<T> operator - (const Quat<T> &q); 00211 00212 template<class T> 00213 Vec3<T> operator * (const Vec3<T> &v, const Quat<T> &q); 00214 00215 00216 //-------------------- 00217 // Convenient typedefs 00218 //-------------------- 00219 00220 typedef Quat<float> Quatf; 00221 typedef Quat<double> Quatd; 00222 00223 00224 //--------------- 00225 // Implementation 00226 //--------------- 00227 00228 template<class T> 00229 inline 00230 Quat<T>::Quat (): r (1), v (0, 0, 0) 00231 { 00232 // empty 00233 } 00234 00235 00236 template<class T> 00237 template <class S> 00238 inline 00239 Quat<T>::Quat (const Quat<S> &q): r (q.r), v (q.v) 00240 { 00241 // empty 00242 } 00243 00244 00245 template<class T> 00246 inline 00247 Quat<T>::Quat (T s, T i, T j, T k): r (s), v (i, j, k) 00248 { 00249 // empty 00250 } 00251 00252 00253 template<class T> 00254 inline 00255 Quat<T>::Quat (T s, Vec3<T> d): r (s), v (d) 00256 { 00257 // empty 00258 } 00259 00260 00261 template<class T> 00262 inline Quat<T> 00263 Quat<T>::identity () 00264 { 00265 return Quat<T>(); 00266 } 00267 00268 template<class T> 00269 inline const Quat<T> & 00270 Quat<T>::operator = (const Quat<T> &q) 00271 { 00272 r = q.r; 00273 v = q.v; 00274 return *this; 00275 } 00276 00277 00278 template<class T> 00279 inline const Quat<T> & 00280 Quat<T>::operator *= (const Quat<T> &q) 00281 { 00282 T rtmp = r * q.r - (v ^ q.v); 00283 v = r * q.v + v * q.r + v % q.v; 00284 r = rtmp; 00285 return *this; 00286 } 00287 00288 00289 template<class T> 00290 inline const Quat<T> & 00291 Quat<T>::operator *= (T t) 00292 { 00293 r *= t; 00294 v *= t; 00295 return *this; 00296 } 00297 00298 00299 template<class T> 00300 inline const Quat<T> & 00301 Quat<T>::operator /= (const Quat<T> &q) 00302 { 00303 *this = *this * q.inverse(); 00304 return *this; 00305 } 00306 00307 00308 template<class T> 00309 inline const Quat<T> & 00310 Quat<T>::operator /= (T t) 00311 { 00312 r /= t; 00313 v /= t; 00314 return *this; 00315 } 00316 00317 00318 template<class T> 00319 inline const Quat<T> & 00320 Quat<T>::operator += (const Quat<T> &q) 00321 { 00322 r += q.r; 00323 v += q.v; 00324 return *this; 00325 } 00326 00327 00328 template<class T> 00329 inline const Quat<T> & 00330 Quat<T>::operator -= (const Quat<T> &q) 00331 { 00332 r -= q.r; 00333 v -= q.v; 00334 return *this; 00335 } 00336 00337 00338 template<class T> 00339 inline T & 00340 Quat<T>::operator [] (int index) 00341 { 00342 return index ? v[index - 1] : r; 00343 } 00344 00345 00346 template<class T> 00347 inline T 00348 Quat<T>::operator [] (int index) const 00349 { 00350 return index ? v[index - 1] : r; 00351 } 00352 00353 00354 template <class T> 00355 template <class S> 00356 inline bool 00357 Quat<T>::operator == (const Quat<S> &q) const 00358 { 00359 return r == q.r && v == q.v; 00360 } 00361 00362 00363 template <class T> 00364 template <class S> 00365 inline bool 00366 Quat<T>::operator != (const Quat<S> &q) const 00367 { 00368 return r != q.r || v != q.v; 00369 } 00370 00371 00372 template<class T> 00373 inline T 00374 operator ^ (const Quat<T>& q1 ,const Quat<T>& q2) 00375 { 00376 return q1.r * q2.r + (q1.v ^ q2.v); 00377 } 00378 00379 00380 template <class T> 00381 inline T 00382 Quat<T>::length () const 00383 { 00384 return Math<T>::sqrt (r * r + (v ^ v)); 00385 } 00386 00387 00388 template <class T> 00389 inline Quat<T> & 00390 Quat<T>::normalize () 00391 { 00392 if (T l = length()) 00393 { 00394 r /= l; 00395 v /= l; 00396 } 00397 else 00398 { 00399 r = 1; 00400 v = Vec3<T> (0); 00401 } 00402 00403 return *this; 00404 } 00405 00406 00407 template <class T> 00408 inline Quat<T> 00409 Quat<T>::normalized () const 00410 { 00411 if (T l = length()) 00412 return Quat (r / l, v / l); 00413 00414 return Quat(); 00415 } 00416 00417 00418 template<class T> 00419 inline Quat<T> 00420 Quat<T>::inverse () const 00421 { 00422 // 00423 // 1 Q* 00424 // - = ---- where Q* is conjugate (operator~) 00425 // Q Q* Q and (Q* Q) == Q ^ Q (4D dot) 00426 // 00427 00428 T qdot = *this ^ *this; 00429 return Quat (r / qdot, -v / qdot); 00430 } 00431 00432 00433 template<class T> 00434 inline Quat<T> & 00435 Quat<T>::invert () 00436 { 00437 T qdot = (*this) ^ (*this); 00438 r /= qdot; 00439 v = -v / qdot; 00440 return *this; 00441 } 00442 00443 00444 template<class T> 00445 inline Vec3<T> 00446 Quat<T>::rotateVector(const Vec3<T>& original) const 00447 { 00448 // 00449 // Given a vector p and a quaternion q (aka this), 00450 // calculate p' = qpq* 00451 // 00452 // Assumes unit quaternions (because non-unit 00453 // quaternions cannot be used to rotate vectors 00454 // anyway). 00455 // 00456 00457 Quat<T> vec (0, original); // temporarily promote grade of original 00458 Quat<T> inv (*this); 00459 inv.v *= -1; // unit multiplicative inverse 00460 Quat<T> result = *this * vec * inv; 00461 return result.v; 00462 } 00463 00464 00465 template<class T> 00466 inline T 00467 Quat<T>::euclideanInnerProduct (const Quat<T> &q) const 00468 { 00469 return r * q.r + v.x * q.v.x + v.y * q.v.y + v.z * q.v.z; 00470 } 00471 00472 00473 template<class T> 00474 T 00475 angle4D (const Quat<T> &q1, const Quat<T> &q2) 00476 { 00477 // 00478 // Compute the angle between two quaternions, 00479 // interpreting the quaternions as 4D vectors. 00480 // 00481 00482 Quat<T> d = q1 - q2; 00483 T lengthD = Math<T>::sqrt (d ^ d); 00484 00485 Quat<T> s = q1 + q2; 00486 T lengthS = Math<T>::sqrt (s ^ s); 00487 00488 return 2 * Math<T>::atan2 (lengthD, lengthS); 00489 } 00490 00491 00492 template<class T> 00493 Quat<T> 00494 slerp (const Quat<T> &q1, const Quat<T> &q2, T t) 00495 { 00496 // 00497 // Spherical linear interpolation. 00498 // Assumes q1 and q2 are normalized and that q1 != -q2. 00499 // 00500 // This method does *not* interpolate along the shortest 00501 // arc between q1 and q2. If you desire interpolation 00502 // along the shortest arc, and q1^q2 is negative, then 00503 // consider calling slerpShortestArc(), below, or flipping 00504 // the second quaternion explicitly. 00505 // 00506 // The implementation of squad() depends on a slerp() 00507 // that interpolates as is, without the automatic 00508 // flipping. 00509 // 00510 // Don Hatch explains the method we use here on his 00511 // web page, The Right Way to Calculate Stuff, at 00512 // http://www.plunk.org/~hatch/rightway.php 00513 // 00514 00515 T a = angle4D (q1, q2); 00516 T s = 1 - t; 00517 00518 Quat<T> q = sinx_over_x (s * a) / sinx_over_x (a) * s * q1 + 00519 sinx_over_x (t * a) / sinx_over_x (a) * t * q2; 00520 00521 return q.normalized(); 00522 } 00523 00524 00525 template<class T> 00526 Quat<T> 00527 slerpShortestArc (const Quat<T> &q1, const Quat<T> &q2, T t) 00528 { 00529 // 00530 // Spherical linear interpolation along the shortest 00531 // arc from q1 to either q2 or -q2, whichever is closer. 00532 // Assumes q1 and q2 are unit quaternions. 00533 // 00534 00535 if ((q1 ^ q2) >= 0) 00536 return slerp (q1, q2, t); 00537 else 00538 return slerp (q1, -q2, t); 00539 } 00540 00541 00542 template<class T> 00543 Quat<T> 00544 spline (const Quat<T> &q0, const Quat<T> &q1, 00545 const Quat<T> &q2, const Quat<T> &q3, 00546 T t) 00547 { 00548 // 00549 // Spherical Cubic Spline Interpolation - 00550 // from Advanced Animation and Rendering 00551 // Techniques by Watt and Watt, Page 366: 00552 // A spherical curve is constructed using three 00553 // spherical linear interpolations of a quadrangle 00554 // of unit quaternions: q1, qa, qb, q2. 00555 // Given a set of quaternion keys: q0, q1, q2, q3, 00556 // this routine does the interpolation between 00557 // q1 and q2 by constructing two intermediate 00558 // quaternions: qa and qb. The qa and qb are 00559 // computed by the intermediate function to 00560 // guarantee the continuity of tangents across 00561 // adjacent cubic segments. The qa represents in-tangent 00562 // for q1 and the qb represents the out-tangent for q2. 00563 // 00564 // The q1 q2 is the cubic segment being interpolated. 00565 // The q0 is from the previous adjacent segment and q3 is 00566 // from the next adjacent segment. The q0 and q3 are used 00567 // in computing qa and qb. 00568 // 00569 00570 Quat<T> qa = intermediate (q0, q1, q2); 00571 Quat<T> qb = intermediate (q1, q2, q3); 00572 Quat<T> result = squad (q1, qa, qb, q2, t); 00573 00574 return result; 00575 } 00576 00577 00578 template<class T> 00579 Quat<T> 00580 squad (const Quat<T> &q1, const Quat<T> &qa, 00581 const Quat<T> &qb, const Quat<T> &q2, 00582 T t) 00583 { 00584 // 00585 // Spherical Quadrangle Interpolation - 00586 // from Advanced Animation and Rendering 00587 // Techniques by Watt and Watt, Page 366: 00588 // It constructs a spherical cubic interpolation as 00589 // a series of three spherical linear interpolations 00590 // of a quadrangle of unit quaternions. 00591 // 00592 00593 Quat<T> r1 = slerp (q1, q2, t); 00594 Quat<T> r2 = slerp (qa, qb, t); 00595 Quat<T> result = slerp (r1, r2, 2 * t * (1 - t)); 00596 00597 return result; 00598 } 00599 00600 00601 template<class T> 00602 Quat<T> 00603 intermediate (const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2) 00604 { 00605 // 00606 // From advanced Animation and Rendering 00607 // Techniques by Watt and Watt, Page 366: 00608 // computing the inner quadrangle 00609 // points (qa and qb) to guarantee tangent 00610 // continuity. 00611 // 00612 00613 Quat<T> q1inv = q1.inverse(); 00614 Quat<T> c1 = q1inv * q2; 00615 Quat<T> c2 = q1inv * q0; 00616 Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log()); 00617 Quat<T> qa = q1 * c3.exp(); 00618 qa.normalize(); 00619 return qa; 00620 } 00621 00622 00623 template <class T> 00624 inline Quat<T> 00625 Quat<T>::log () const 00626 { 00627 // 00628 // For unit quaternion, from Advanced Animation and 00629 // Rendering Techniques by Watt and Watt, Page 366: 00630 // 00631 00632 T theta = Math<T>::acos (std::min (r, (T) 1.0)); 00633 00634 if (theta == 0) 00635 return Quat<T> (0, v); 00636 00637 T sintheta = Math<T>::sin (theta); 00638 00639 T k; 00640 if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta)) 00641 k = 1; 00642 else 00643 k = theta / sintheta; 00644 00645 return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k); 00646 } 00647 00648 00649 template <class T> 00650 inline Quat<T> 00651 Quat<T>::exp () const 00652 { 00653 // 00654 // For pure quaternion (zero scalar part): 00655 // from Advanced Animation and Rendering 00656 // Techniques by Watt and Watt, Page 366: 00657 // 00658 00659 T theta = v.length(); 00660 T sintheta = Math<T>::sin (theta); 00661 00662 T k; 00663 if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta)) 00664 k = 1; 00665 else 00666 k = sintheta / theta; 00667 00668 T costheta = Math<T>::cos (theta); 00669 00670 return Quat<T> (costheta, v.x * k, v.y * k, v.z * k); 00671 } 00672 00673 00674 template <class T> 00675 inline T 00676 Quat<T>::angle () const 00677 { 00678 return 2 * Math<T>::atan2 (v.length(), r); 00679 } 00680 00681 00682 template <class T> 00683 inline Vec3<T> 00684 Quat<T>::axis () const 00685 { 00686 return v.normalized(); 00687 } 00688 00689 00690 template <class T> 00691 inline Quat<T> & 00692 Quat<T>::setAxisAngle (const Vec3<T> &axis, T radians) 00693 { 00694 r = Math<T>::cos (radians / 2); 00695 v = axis.normalized() * Math<T>::sin (radians / 2); 00696 return *this; 00697 } 00698 00699 00700 template <class T> 00701 Quat<T> & 00702 Quat<T>::setRotation (const Vec3<T> &from, const Vec3<T> &to) 00703 { 00704 // 00705 // Create a quaternion that rotates vector from into vector to, 00706 // such that the rotation is around an axis that is the cross 00707 // product of from and to. 00708 // 00709 // This function calls function setRotationInternal(), which is 00710 // numerically accurate only for rotation angles that are not much 00711 // greater than pi/2. In order to achieve good accuracy for angles 00712 // greater than pi/2, we split large angles in half, and rotate in 00713 // two steps. 00714 // 00715 00716 // 00717 // Normalize from and to, yielding f0 and t0. 00718 // 00719 00720 Vec3<T> f0 = from.normalized(); 00721 Vec3<T> t0 = to.normalized(); 00722 00723 if ((f0 ^ t0) >= 0) 00724 { 00725 // 00726 // The rotation angle is less than or equal to pi/2. 00727 // 00728 00729 setRotationInternal (f0, t0, *this); 00730 } 00731 else 00732 { 00733 // 00734 // The angle is greater than pi/2. After computing h0, 00735 // which is halfway between f0 and t0, we rotate first 00736 // from f0 to h0, then from h0 to t0. 00737 // 00738 00739 Vec3<T> h0 = (f0 + t0).normalized(); 00740 00741 if ((h0 ^ h0) != 0) 00742 { 00743 setRotationInternal (f0, h0, *this); 00744 00745 Quat<T> q; 00746 setRotationInternal (h0, t0, q); 00747 00748 *this *= q; 00749 } 00750 else 00751 { 00752 // 00753 // f0 and t0 point in exactly opposite directions. 00754 // Pick an arbitrary axis that is orthogonal to f0, 00755 // and rotate by pi. 00756 // 00757 00758 r = T (0); 00759 00760 Vec3<T> f02 = f0 * f0; 00761 00762 if (f02.x <= f02.y && f02.x <= f02.z) 00763 v = (f0 % Vec3<T> (1, 0, 0)).normalized(); 00764 else if (f02.y <= f02.z) 00765 v = (f0 % Vec3<T> (0, 1, 0)).normalized(); 00766 else 00767 v = (f0 % Vec3<T> (0, 0, 1)).normalized(); 00768 } 00769 } 00770 00771 return *this; 00772 } 00773 00774 00775 template <class T> 00776 void 00777 Quat<T>::setRotationInternal (const Vec3<T> &f0, const Vec3<T> &t0, Quat<T> &q) 00778 { 00779 // 00780 // The following is equivalent to setAxisAngle(n,2*phi), 00781 // where the rotation axis, n, is orthogonal to the f0 and 00782 // t0 vectors, and 2*phi is the angle between f0 and t0. 00783 // 00784 // This function is called by setRotation(), above; it assumes 00785 // that f0 and t0 are normalized and that the angle between 00786 // them is not much greater than pi/2. This function becomes 00787 // numerically inaccurate if f0 and t0 point into nearly 00788 // opposite directions. 00789 // 00790 00791 // 00792 // Find a normalized vector, h0, that is halfway between f0 and t0. 00793 // The angle between f0 and h0 is phi. 00794 // 00795 00796 Vec3<T> h0 = (f0 + t0).normalized(); 00797 00798 // 00799 // Store the rotation axis and rotation angle. 00800 // 00801 00802 q.r = f0 ^ h0; // f0 ^ h0 == cos (phi) 00803 q.v = f0 % h0; // (f0 % h0).length() == sin (phi) 00804 } 00805 00806 00807 template<class T> 00808 Matrix33<T> 00809 Quat<T>::toMatrix33() const 00810 { 00811 return Matrix33<T> (1 - 2 * (v.y * v.y + v.z * v.z), 00812 2 * (v.x * v.y + v.z * r), 00813 2 * (v.z * v.x - v.y * r), 00814 00815 2 * (v.x * v.y - v.z * r), 00816 1 - 2 * (v.z * v.z + v.x * v.x), 00817 2 * (v.y * v.z + v.x * r), 00818 00819 2 * (v.z * v.x + v.y * r), 00820 2 * (v.y * v.z - v.x * r), 00821 1 - 2 * (v.y * v.y + v.x * v.x)); 00822 } 00823 00824 template<class T> 00825 Matrix44<T> 00826 Quat<T>::toMatrix44() const 00827 { 00828 return Matrix44<T> (1 - 2 * (v.y * v.y + v.z * v.z), 00829 2 * (v.x * v.y + v.z * r), 00830 2 * (v.z * v.x - v.y * r), 00831 0, 00832 2 * (v.x * v.y - v.z * r), 00833 1 - 2 * (v.z * v.z + v.x * v.x), 00834 2 * (v.y * v.z + v.x * r), 00835 0, 00836 2 * (v.z * v.x + v.y * r), 00837 2 * (v.y * v.z - v.x * r), 00838 1 - 2 * (v.y * v.y + v.x * v.x), 00839 0, 00840 0, 00841 0, 00842 0, 00843 1); 00844 } 00845 00846 00847 template<class T> 00848 inline Matrix33<T> 00849 operator * (const Matrix33<T> &M, const Quat<T> &q) 00850 { 00851 return M * q.toMatrix33(); 00852 } 00853 00854 00855 template<class T> 00856 inline Matrix33<T> 00857 operator * (const Quat<T> &q, const Matrix33<T> &M) 00858 { 00859 return q.toMatrix33() * M; 00860 } 00861 00862 00863 template<class T> 00864 std::ostream & 00865 operator << (std::ostream &o, const Quat<T> &q) 00866 { 00867 return o << "(" << q.r 00868 << " " << q.v.x 00869 << " " << q.v.y 00870 << " " << q.v.z 00871 << ")"; 00872 } 00873 00874 00875 template<class T> 00876 inline Quat<T> 00877 operator * (const Quat<T> &q1, const Quat<T> &q2) 00878 { 00879 return Quat<T> (q1.r * q2.r - (q1.v ^ q2.v), 00880 q1.r * q2.v + q1.v * q2.r + q1.v % q2.v); 00881 } 00882 00883 00884 template<class T> 00885 inline Quat<T> 00886 operator / (const Quat<T> &q1, const Quat<T> &q2) 00887 { 00888 return q1 * q2.inverse(); 00889 } 00890 00891 00892 template<class T> 00893 inline Quat<T> 00894 operator / (const Quat<T> &q, T t) 00895 { 00896 return Quat<T> (q.r / t, q.v / t); 00897 } 00898 00899 00900 template<class T> 00901 inline Quat<T> 00902 operator * (const Quat<T> &q, T t) 00903 { 00904 return Quat<T> (q.r * t, q.v * t); 00905 } 00906 00907 00908 template<class T> 00909 inline Quat<T> 00910 operator * (T t, const Quat<T> &q) 00911 { 00912 return Quat<T> (q.r * t, q.v * t); 00913 } 00914 00915 00916 template<class T> 00917 inline Quat<T> 00918 operator + (const Quat<T> &q1, const Quat<T> &q2) 00919 { 00920 return Quat<T> (q1.r + q2.r, q1.v + q2.v); 00921 } 00922 00923 00924 template<class T> 00925 inline Quat<T> 00926 operator - (const Quat<T> &q1, const Quat<T> &q2) 00927 { 00928 return Quat<T> (q1.r - q2.r, q1.v - q2.v); 00929 } 00930 00931 00932 template<class T> 00933 inline Quat<T> 00934 operator ~ (const Quat<T> &q) 00935 { 00936 return Quat<T> (q.r, -q.v); 00937 } 00938 00939 00940 template<class T> 00941 inline Quat<T> 00942 operator - (const Quat<T> &q) 00943 { 00944 return Quat<T> (-q.r, -q.v); 00945 } 00946 00947 00948 template<class T> 00949 inline Vec3<T> 00950 operator * (const Vec3<T> &v, const Quat<T> &q) 00951 { 00952 Vec3<T> a = q.v % v; 00953 Vec3<T> b = q.v % a; 00954 return v + T (2) * (q.r * a + b); 00955 } 00956 00957 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER 00958 #pragma warning(default:4244) 00959 #endif 00960 00961 } // namespace Imath 00962 00963 #endif