PBRT
/home/felix/UBC/projects/AdaptiveLightfieldSampling/pbrt_v2/src/3rdparty/ilmbase-1.0.2/ImathQuat.h
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