PBRT
/home/felix/UBC/projects/AdaptiveLightfieldSampling/pbrt_v2/src/3rdparty/ilmbase-1.0.2/ImathEuler.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_IMATHEULER_H
00038 #define INCLUDED_IMATHEULER_H
00039 
00040 //----------------------------------------------------------------------
00041 //
00042 //      template class Euler<T>
00043 //
00044 //      This class represents euler angle orientations. The class
00045 //      inherits from Vec3 to it can be freely cast. The additional
00046 //      information is the euler priorities rep. This class is
00047 //      essentially a rip off of Ken Shoemake's GemsIV code. It has
00048 //      been modified minimally to make it more understandable, but
00049 //      hardly enough to make it easy to grok completely.
00050 //
00051 //      There are 24 possible combonations of Euler angle
00052 //      representations of which 12 are common in CG and you will
00053 //      probably only use 6 of these which in this scheme are the
00054 //      non-relative-non-repeating types. 
00055 //
00056 //      The representations can be partitioned according to two
00057 //      criteria:
00058 //
00059 //         1) Are the angles measured relative to a set of fixed axis
00060 //            or relative to each other (the latter being what happens
00061 //            when rotation matrices are multiplied together and is
00062 //            almost ubiquitous in the cg community)
00063 //
00064 //         2) Is one of the rotations repeated (ala XYX rotation)
00065 //
00066 //      When you construct a given representation from scratch you
00067 //      must order the angles according to their priorities. So, the
00068 //      easiest is a softimage or aerospace (yaw/pitch/roll) ordering
00069 //      of ZYX. 
00070 //
00071 //          float x_rot = 1;
00072 //          float y_rot = 2;
00073 //          float z_rot = 3;
00074 //
00075 //          Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX);
00076 //              -or-
00077 //          Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX );
00078 //
00079 //      If instead, the order was YXZ for instance you would have to
00080 //      do this:
00081 //
00082 //          float x_rot = 1;
00083 //          float y_rot = 2;
00084 //          float z_rot = 3;
00085 //
00086 //          Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
00087 //              -or-
00088 //          Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ );
00089 //
00090 //      Notice how the order you put the angles into the three slots
00091 //      should correspond to the enum (YXZ) ordering. The input angle
00092 //      vector is called the "ijk" vector -- not an "xyz" vector. The
00093 //      ijk vector order is the same as the enum. If you treat the
00094 //      Euler<> as a Vec<> (which it inherts from) you will find the
00095 //      angles are ordered in the same way, i.e.:
00096 //
00097 //          V3f v = angles;
00098 //          // v.x == y_rot, v.y == x_rot, v.z == z_rot
00099 //
00100 //      If you just want the x, y, and z angles stored in a vector in
00101 //      that order, you can do this:
00102 //
00103 //          V3f v = angles.toXYZVector()
00104 //          // v.x == x_rot, v.y == y_rot, v.z == z_rot
00105 //
00106 //      If you want to set the Euler with an XYZVector use the
00107 //      optional layout argument:
00108 //
00109 //          Eulerf angles(x_rot, y_rot, z_rot, 
00110 //                        Eulerf::YXZ,
00111 //                        Eulerf::XYZLayout);
00112 //
00113 //      This is the same as:
00114 //
00115 //          Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
00116 //          
00117 //      Note that this won't do anything intelligent if you have a
00118 //      repeated axis in the euler angles (e.g. XYX)
00119 //
00120 //      If you need to use the "relative" versions of these, you will
00121 //      need to use the "r" enums. 
00122 //
00123 //      The units of the rotation angles are assumed to be radians.
00124 //
00125 //----------------------------------------------------------------------
00126 
00127 
00128 #include "ImathMath.h"
00129 #include "ImathVec.h"
00130 #include "ImathQuat.h"
00131 #include "ImathMatrix.h"
00132 #include "ImathLimits.h"
00133 #include <iostream>
00134 
00135 namespace Imath {
00136 
00137 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
00138 // Disable MS VC++ warnings about conversion from double to float
00139 #pragma warning(disable:4244)
00140 #endif
00141 
00142 template <class T>
00143 class Euler : public Vec3<T>
00144 {
00145   public:
00146  
00147     using Vec3<T>::x;
00148     using Vec3<T>::y;
00149     using Vec3<T>::z;
00150 
00151     enum Order
00152     {
00153         //
00154         //  All 24 possible orderings
00155         //
00156 
00157         XYZ     = 0x0101,       // "usual" orderings
00158         XZY     = 0x0001,
00159         YZX     = 0x1101,
00160         YXZ     = 0x1001,
00161         ZXY     = 0x2101,
00162         ZYX     = 0x2001,
00163         
00164         XZX     = 0x0011,       // first axis repeated
00165         XYX     = 0x0111,
00166         YXY     = 0x1011,
00167         YZY     = 0x1111,
00168         ZYZ     = 0x2011,
00169         ZXZ     = 0x2111,
00170 
00171         XYZr    = 0x2000,       // relative orderings -- not common
00172         XZYr    = 0x2100,
00173         YZXr    = 0x1000,
00174         YXZr    = 0x1100,
00175         ZXYr    = 0x0000,
00176         ZYXr    = 0x0100,
00177         
00178         XZXr    = 0x2110,       // relative first axis repeated 
00179         XYXr    = 0x2010,
00180         YXYr    = 0x1110,
00181         YZYr    = 0x1010,
00182         ZYZr    = 0x0110,
00183         ZXZr    = 0x0010,
00184         //          ||||
00185         //          VVVV
00186         //  Legend: ABCD
00187         //  A -> Initial Axis (0==x, 1==y, 2==z)
00188         //  B -> Parity Even (1==true)
00189         //  C -> Initial Repeated (1==true)
00190         //  D -> Frame Static (1==true)
00191         //
00192 
00193         Legal   =   XYZ | XZY | YZX | YXZ | ZXY | ZYX |
00194                     XZX | XYX | YXY | YZY | ZYZ | ZXZ |
00195                     XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr|
00196                     XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr,
00197 
00198         Min     = 0x0000,
00199         Max     = 0x2111,
00200         Default = XYZ
00201     };
00202 
00203     enum Axis { X = 0, Y = 1, Z = 2 };
00204 
00205     enum InputLayout { XYZLayout, IJKLayout };
00206 
00207     //--------------------------------------------------------------------
00208     //  Constructors -- all default to ZYX non-relative ala softimage
00209     //                  (where there is no argument to specify it)
00210     //
00211     // The Euler-from-matrix constructors assume that the matrix does
00212     // not include shear or non-uniform scaling, but the constructors
00213     // do not examine the matrix to verify this assumption.  If necessary,
00214     // you can adjust the matrix by calling the removeScalingAndShear()
00215     // function, defined in ImathMatrixAlgo.h.
00216     //--------------------------------------------------------------------
00217 
00218     Euler();
00219     Euler(const Euler&);
00220     Euler(Order p);
00221     Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
00222     Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
00223     Euler(const Euler<T> &euler, Order newp);
00224     Euler(const Matrix33<T> &, Order o = Default);
00225     Euler(const Matrix44<T> &, Order o = Default);
00226 
00227     //---------------------------------
00228     //  Algebraic functions/ Operators
00229     //---------------------------------
00230 
00231     const Euler<T>&     operator=  (const Euler<T>&);
00232     const Euler<T>&     operator=  (const Vec3<T>&);
00233 
00234     //--------------------------------------------------------
00235     //  Set the euler value
00236     //  This does NOT convert the angles, but setXYZVector() 
00237     //  does reorder the input vector.
00238     //--------------------------------------------------------
00239 
00240     static bool         legal(Order);
00241 
00242     void                setXYZVector(const Vec3<T> &);
00243 
00244     Order               order() const;
00245     void                setOrder(Order);
00246 
00247     void                set(Axis initial,
00248                             bool relative,
00249                             bool parityEven,
00250                             bool firstRepeats);
00251 
00252     //------------------------------------------------------------
00253     //  Conversions, toXYZVector() reorders the angles so that
00254     //  the X rotation comes first, followed by the Y and Z
00255     //  in cases like XYX ordering, the repeated angle will be
00256     //  in the "z" component
00257     //
00258     // The Euler-from-matrix extract() functions assume that the
00259     // matrix does not include shear or non-uniform scaling, but
00260     // the extract() functions do not examine the matrix to verify
00261     // this assumption.  If necessary, you can adjust the matrix
00262     // by calling the removeScalingAndShear() function, defined
00263     // in ImathMatrixAlgo.h.
00264     //------------------------------------------------------------
00265 
00266     void                extract(const Matrix33<T>&);
00267     void                extract(const Matrix44<T>&);
00268     void                extract(const Quat<T>&);
00269 
00270     Matrix33<T>         toMatrix33() const;
00271     Matrix44<T>         toMatrix44() const;
00272     Quat<T>             toQuat() const;
00273     Vec3<T>             toXYZVector() const;
00274 
00275     //---------------------------------------------------
00276     //  Use this function to unpack angles from ijk form
00277     //---------------------------------------------------
00278 
00279     void                angleOrder(int &i, int &j, int &k) const;
00280 
00281     //---------------------------------------------------
00282     //  Use this function to determine mapping from xyz to ijk
00283     // - reshuffles the xyz to match the order
00284     //---------------------------------------------------
00285     
00286     void                angleMapping(int &i, int &j, int &k) const;
00287 
00288     //----------------------------------------------------------------------
00289     //
00290     //  Utility methods for getting continuous rotations. None of these
00291     //  methods change the orientation given by its inputs (or at least
00292     //  that is the intent).
00293     //
00294     //    angleMod() converts an angle to its equivalent in [-PI, PI]
00295     //
00296     //    simpleXYZRotation() adjusts xyzRot so that its components differ
00297     //                        from targetXyzRot by no more than +-PI
00298     //
00299     //    nearestRotation() adjusts xyzRot so that its components differ
00300     //                      from targetXyzRot by as little as possible.
00301     //                      Note that xyz here really means ijk, because
00302     //                      the order must be provided.
00303     //
00304     //    makeNear() adjusts "this" Euler so that its components differ
00305     //               from target by as little as possible. This method
00306     //               might not make sense for Eulers with different order
00307     //               and it probably doesn't work for repeated axis and
00308     //               relative orderings (TODO).
00309     //
00310     //-----------------------------------------------------------------------
00311 
00312     static float        angleMod (T angle);
00313     static void         simpleXYZRotation (Vec3<T> &xyzRot,
00314                                            const Vec3<T> &targetXyzRot);
00315     static void         nearestRotation (Vec3<T> &xyzRot,
00316                                          const Vec3<T> &targetXyzRot,
00317                                          Order order = XYZ);
00318 
00319     void                makeNear (const Euler<T> &target);
00320 
00321     bool                frameStatic() const { return _frameStatic; }
00322     bool                initialRepeated() const { return _initialRepeated; }
00323     bool                parityEven() const { return _parityEven; }
00324     Axis                initialAxis() const { return _initialAxis; }
00325 
00326   protected:
00327 
00328     bool                _frameStatic     : 1;   // relative or static rotations
00329     bool                _initialRepeated : 1;   // init axis repeated as last
00330     bool                _parityEven      : 1;   // "parity of axis permutation"
00331 #if defined _WIN32 || defined _WIN64
00332     Axis                _initialAxis     ;      // First axis of rotation
00333 #else
00334     Axis                _initialAxis     : 2;   // First axis of rotation
00335 #endif
00336 };
00337 
00338 
00339 //--------------------
00340 // Convenient typedefs
00341 //--------------------
00342 
00343 typedef Euler<float>    Eulerf;
00344 typedef Euler<double>   Eulerd;
00345 
00346 
00347 //---------------
00348 // Implementation
00349 //---------------
00350 
00351 template<class T>
00352 inline void
00353  Euler<T>::angleOrder(int &i, int &j, int &k) const
00354 {
00355     i = _initialAxis;
00356     j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
00357     k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
00358 }
00359 
00360 template<class T>
00361 inline void
00362  Euler<T>::angleMapping(int &i, int &j, int &k) const
00363 {
00364     int m[3];
00365 
00366     m[_initialAxis] = 0;
00367     m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
00368     m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
00369     i = m[0];
00370     j = m[1];
00371     k = m[2];
00372 }
00373 
00374 template<class T>
00375 inline void
00376 Euler<T>::setXYZVector(const Vec3<T> &v)
00377 {
00378     int i,j,k;
00379     angleMapping(i,j,k);
00380     (*this)[i] = v.x;
00381     (*this)[j] = v.y;
00382     (*this)[k] = v.z;
00383 }
00384 
00385 template<class T>
00386 inline Vec3<T>
00387 Euler<T>::toXYZVector() const
00388 {
00389     int i,j,k;
00390     angleMapping(i,j,k);
00391     return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
00392 }
00393 
00394 
00395 template<class T>
00396 Euler<T>::Euler() :
00397     Vec3<T>(0,0,0),
00398     _frameStatic(true),
00399     _initialRepeated(false),
00400     _parityEven(true),
00401     _initialAxis(X)
00402 {}
00403 
00404 template<class T>
00405 Euler<T>::Euler(typename Euler<T>::Order p) :
00406     Vec3<T>(0,0,0),
00407     _frameStatic(true),
00408     _initialRepeated(false),
00409     _parityEven(true),
00410     _initialAxis(X)
00411 {
00412     setOrder(p);
00413 }
00414 
00415 template<class T>
00416 inline Euler<T>::Euler( const Vec3<T> &v, 
00417                         typename Euler<T>::Order p, 
00418                         typename Euler<T>::InputLayout l ) 
00419 {
00420     setOrder(p); 
00421     if ( l == XYZLayout ) setXYZVector(v);
00422     else { x = v.x; y = v.y; z = v.z; }
00423 }
00424 
00425 template<class T>
00426 inline Euler<T>::Euler(const Euler<T> &euler)
00427 {
00428     operator=(euler);
00429 }
00430 
00431 template<class T>
00432 inline Euler<T>::Euler(const Euler<T> &euler,Order p)
00433 {
00434     setOrder(p);
00435     Matrix33<T> M = euler.toMatrix33();
00436     extract(M);
00437 }
00438 
00439 template<class T>
00440 inline Euler<T>::Euler( T xi, T yi, T zi, 
00441                         typename Euler<T>::Order p,
00442                         typename Euler<T>::InputLayout l)
00443 {
00444     setOrder(p);
00445     if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
00446     else { x = xi; y = yi; z = zi; }
00447 }
00448 
00449 template<class T>
00450 inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
00451 {
00452     setOrder(p);
00453     extract(M);
00454 }
00455 
00456 template<class T>
00457 inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
00458 {
00459     setOrder(p);
00460     extract(M);
00461 }
00462 
00463 template<class T>
00464 inline void Euler<T>::extract(const Quat<T> &q)
00465 {
00466     extract(q.toMatrix33());
00467 }
00468 
00469 template<class T>
00470 void Euler<T>::extract(const Matrix33<T> &M)
00471 {
00472     int i,j,k;
00473     angleOrder(i,j,k);
00474 
00475     if (_initialRepeated)
00476     {
00477         //
00478         // Extract the first angle, x.
00479         // 
00480 
00481         x = Math<T>::atan2 (M[j][i], M[k][i]);
00482 
00483         //
00484         // Remove the x rotation from M, so that the remaining
00485         // rotation, N, is only around two axes, and gimbal lock
00486         // cannot occur.
00487         //
00488 
00489         Vec3<T> r (0, 0, 0);
00490         r[i] = (_parityEven? -x: x);
00491 
00492         Matrix44<T> N;
00493         N.rotate (r);
00494 
00495         N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
00496                              M[1][0], M[1][1], M[1][2], 0,
00497                              M[2][0], M[2][1], M[2][2], 0,
00498                              0,       0,       0,       1);
00499         //
00500         // Extract the other two angles, y and z, from N.
00501         //
00502 
00503         T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
00504         y = Math<T>::atan2 (sy, N[i][i]);
00505         z = Math<T>::atan2 (N[j][k], N[j][j]);
00506     }
00507     else
00508     {
00509         //
00510         // Extract the first angle, x.
00511         // 
00512 
00513         x = Math<T>::atan2 (M[j][k], M[k][k]);
00514 
00515         //
00516         // Remove the x rotation from M, so that the remaining
00517         // rotation, N, is only around two axes, and gimbal lock
00518         // cannot occur.
00519         //
00520 
00521         Vec3<T> r (0, 0, 0);
00522         r[i] = (_parityEven? -x: x);
00523 
00524         Matrix44<T> N;
00525         N.rotate (r);
00526 
00527         N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
00528                              M[1][0], M[1][1], M[1][2], 0,
00529                              M[2][0], M[2][1], M[2][2], 0,
00530                              0,       0,       0,       1);
00531         //
00532         // Extract the other two angles, y and z, from N.
00533         //
00534 
00535         T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
00536         y = Math<T>::atan2 (-N[i][k], cy);
00537         z = Math<T>::atan2 (-N[j][i], N[j][j]);
00538     }
00539 
00540     if (!_parityEven)
00541         *this *= -1;
00542 
00543     if (!_frameStatic)
00544     {
00545         T t = x;
00546         x = z;
00547         z = t;
00548     }
00549 }
00550 
00551 template<class T>
00552 void Euler<T>::extract(const Matrix44<T> &M)
00553 {
00554     int i,j,k;
00555     angleOrder(i,j,k);
00556 
00557     if (_initialRepeated)
00558     {
00559         //
00560         // Extract the first angle, x.
00561         // 
00562 
00563         x = Math<T>::atan2 (M[j][i], M[k][i]);
00564 
00565         //
00566         // Remove the x rotation from M, so that the remaining
00567         // rotation, N, is only around two axes, and gimbal lock
00568         // cannot occur.
00569         //
00570 
00571         Vec3<T> r (0, 0, 0);
00572         r[i] = (_parityEven? -x: x);
00573 
00574         Matrix44<T> N;
00575         N.rotate (r);
00576         N = N * M;
00577 
00578         //
00579         // Extract the other two angles, y and z, from N.
00580         //
00581 
00582         T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
00583         y = Math<T>::atan2 (sy, N[i][i]);
00584         z = Math<T>::atan2 (N[j][k], N[j][j]);
00585     }
00586     else
00587     {
00588         //
00589         // Extract the first angle, x.
00590         // 
00591 
00592         x = Math<T>::atan2 (M[j][k], M[k][k]);
00593 
00594         //
00595         // Remove the x rotation from M, so that the remaining
00596         // rotation, N, is only around two axes, and gimbal lock
00597         // cannot occur.
00598         //
00599 
00600         Vec3<T> r (0, 0, 0);
00601         r[i] = (_parityEven? -x: x);
00602 
00603         Matrix44<T> N;
00604         N.rotate (r);
00605         N = N * M;
00606 
00607         //
00608         // Extract the other two angles, y and z, from N.
00609         //
00610 
00611         T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
00612         y = Math<T>::atan2 (-N[i][k], cy);
00613         z = Math<T>::atan2 (-N[j][i], N[j][j]);
00614     }
00615 
00616     if (!_parityEven)
00617         *this *= -1;
00618 
00619     if (!_frameStatic)
00620     {
00621         T t = x;
00622         x = z;
00623         z = t;
00624     }
00625 }
00626 
00627 template<class T>
00628 Matrix33<T> Euler<T>::toMatrix33() const
00629 {
00630     int i,j,k;
00631     angleOrder(i,j,k);
00632 
00633     Vec3<T> angles;
00634 
00635     if ( _frameStatic ) angles = (*this);
00636     else angles = Vec3<T>(z,y,x);
00637 
00638     if ( !_parityEven ) angles *= -1.0;
00639 
00640     T ci = Math<T>::cos(angles.x);
00641     T cj = Math<T>::cos(angles.y);
00642     T ch = Math<T>::cos(angles.z);
00643     T si = Math<T>::sin(angles.x);
00644     T sj = Math<T>::sin(angles.y);
00645     T sh = Math<T>::sin(angles.z);
00646 
00647     T cc = ci*ch;
00648     T cs = ci*sh;
00649     T sc = si*ch;
00650     T ss = si*sh;
00651 
00652     Matrix33<T> M;
00653 
00654     if ( _initialRepeated )
00655     {
00656         M[i][i] = cj;     M[j][i] =  sj*si;    M[k][i] =  sj*ci;
00657         M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
00658         M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
00659     }
00660     else
00661     {
00662         M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
00663         M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
00664         M[i][k] = -sj;   M[j][k] = cj*si;    M[k][k] = cj*ci;
00665     }
00666 
00667     return M;
00668 }
00669 
00670 template<class T>
00671 Matrix44<T> Euler<T>::toMatrix44() const
00672 {
00673     int i,j,k;
00674     angleOrder(i,j,k);
00675 
00676     Vec3<T> angles;
00677 
00678     if ( _frameStatic ) angles = (*this);
00679     else angles = Vec3<T>(z,y,x);
00680 
00681     if ( !_parityEven ) angles *= -1.0;
00682 
00683     T ci = Math<T>::cos(angles.x);
00684     T cj = Math<T>::cos(angles.y);
00685     T ch = Math<T>::cos(angles.z);
00686     T si = Math<T>::sin(angles.x);
00687     T sj = Math<T>::sin(angles.y);
00688     T sh = Math<T>::sin(angles.z);
00689 
00690     T cc = ci*ch;
00691     T cs = ci*sh;
00692     T sc = si*ch;
00693     T ss = si*sh;
00694 
00695     Matrix44<T> M;
00696 
00697     if ( _initialRepeated )
00698     {
00699         M[i][i] = cj;     M[j][i] =  sj*si;    M[k][i] =  sj*ci;
00700         M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
00701         M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
00702     }
00703     else
00704     {
00705         M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
00706         M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
00707         M[i][k] = -sj;   M[j][k] = cj*si;    M[k][k] = cj*ci;
00708     }
00709 
00710     return M;
00711 }
00712 
00713 template<class T>
00714 Quat<T> Euler<T>::toQuat() const
00715 {
00716     Vec3<T> angles;
00717     int i,j,k;
00718     angleOrder(i,j,k);
00719 
00720     if ( _frameStatic ) angles = (*this);
00721     else angles = Vec3<T>(z,y,x);
00722 
00723     if ( !_parityEven ) angles.y = -angles.y;
00724 
00725     T ti = angles.x*0.5;
00726     T tj = angles.y*0.5;
00727     T th = angles.z*0.5;
00728     T ci = Math<T>::cos(ti);
00729     T cj = Math<T>::cos(tj);
00730     T ch = Math<T>::cos(th);
00731     T si = Math<T>::sin(ti);
00732     T sj = Math<T>::sin(tj);
00733     T sh = Math<T>::sin(th);
00734     T cc = ci*ch;
00735     T cs = ci*sh;
00736     T sc = si*ch;
00737     T ss = si*sh;
00738 
00739     T parity = _parityEven ? 1.0 : -1.0;
00740 
00741     Quat<T> q;
00742     Vec3<T> a;
00743 
00744     if ( _initialRepeated )
00745     {
00746         a[i]    = cj*(cs + sc);
00747         a[j]    = sj*(cc + ss) * parity,
00748         a[k]    = sj*(cs - sc);
00749         q.r     = cj*(cc - ss);
00750     }
00751     else
00752     {
00753         a[i]    = cj*sc - sj*cs,
00754         a[j]    = (cj*ss + sj*cc) * parity,
00755         a[k]    = cj*cs - sj*sc;
00756         q.r     = cj*cc + sj*ss;
00757     }
00758 
00759     q.v = a;
00760 
00761     return q;
00762 }
00763 
00764 template<class T>
00765 inline bool
00766 Euler<T>::legal(typename Euler<T>::Order order)
00767 {
00768     return (order & ~Legal) ? false : true;
00769 }
00770 
00771 template<class T>
00772 typename Euler<T>::Order
00773 Euler<T>::order() const
00774 {
00775     int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));
00776 
00777     if (_parityEven)      foo |= 0x0100;
00778     if (_initialRepeated) foo |= 0x0010;
00779     if (_frameStatic)     foo++;
00780 
00781     return (Order)foo;
00782 }
00783 
00784 template<class T>
00785 inline void Euler<T>::setOrder(typename Euler<T>::Order p)
00786 {
00787     set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis
00788          !(p & 0x1),                            // static?
00789          !!(p & 0x100),                         // permutation even?
00790          !!(p & 0x10));                         // initial repeats?
00791 }
00792 
00793 template<class T>
00794 void Euler<T>::set(typename Euler<T>::Axis axis,
00795                    bool relative,
00796                    bool parityEven,
00797                    bool firstRepeats)
00798 {
00799     _initialAxis        = axis;
00800     _frameStatic        = !relative;
00801     _parityEven         = parityEven;
00802     _initialRepeated    = firstRepeats;
00803 }
00804 
00805 template<class T>
00806 const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
00807 {
00808     x = euler.x;
00809     y = euler.y;
00810     z = euler.z;
00811     _initialAxis = euler._initialAxis;
00812     _frameStatic = euler._frameStatic;
00813     _parityEven  = euler._parityEven;
00814     _initialRepeated = euler._initialRepeated;
00815     return *this;
00816 }
00817 
00818 template<class T>
00819 const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
00820 {
00821     x = v.x;
00822     y = v.y;
00823     z = v.z;
00824     return *this;
00825 }
00826 
00827 template<class T>
00828 std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
00829 {
00830     char a[3] = { 'X', 'Y', 'Z' };
00831 
00832     const char* r = euler.frameStatic() ? "" : "r";
00833     int i,j,k;
00834     euler.angleOrder(i,j,k);
00835 
00836     if ( euler.initialRepeated() ) k = i;
00837 
00838     return o << "("
00839              << euler.x << " "
00840              << euler.y << " "
00841              << euler.z << " "
00842              << a[i] << a[j] << a[k] << r << ")";
00843 }
00844 
00845 template <class T>
00846 float
00847 Euler<T>::angleMod (T angle)
00848 {
00849     angle = fmod(T (angle), T (2 * M_PI));
00850 
00851     if (angle < -M_PI)  angle += 2 * M_PI;
00852     if (angle > +M_PI)  angle -= 2 * M_PI;
00853 
00854     return angle;
00855 }
00856 
00857 template <class T>
00858 void
00859 Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
00860 {
00861     Vec3<T> d  = xyzRot - targetXyzRot;
00862     xyzRot[0]  = targetXyzRot[0] + angleMod(d[0]);
00863     xyzRot[1]  = targetXyzRot[1] + angleMod(d[1]);
00864     xyzRot[2]  = targetXyzRot[2] + angleMod(d[2]);
00865 }
00866 
00867 template <class T>
00868 void
00869 Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
00870                            Order order)
00871 {
00872     int i,j,k;
00873     Euler<T> e (0,0,0, order);
00874     e.angleOrder(i,j,k);
00875 
00876     simpleXYZRotation(xyzRot, targetXyzRot);
00877 
00878     Vec3<T> otherXyzRot;
00879     otherXyzRot[i] = M_PI+xyzRot[i];
00880     otherXyzRot[j] = M_PI-xyzRot[j];
00881     otherXyzRot[k] = M_PI+xyzRot[k];
00882 
00883     simpleXYZRotation(otherXyzRot, targetXyzRot);
00884             
00885     Vec3<T> d  = xyzRot - targetXyzRot;
00886     Vec3<T> od = otherXyzRot - targetXyzRot;
00887     T dMag     = d.dot(d);
00888     T odMag    = od.dot(od);
00889 
00890     if (odMag < dMag)
00891     {
00892         xyzRot = otherXyzRot;
00893     }
00894 }
00895 
00896 template <class T>
00897 void
00898 Euler<T>::makeNear (const Euler<T> &target)
00899 {
00900     Vec3<T> xyzRot    = toXYZVector();
00901     Euler<T> targetSameOrder = Euler<T>(target, order());
00902     Vec3<T> targetXyz = targetSameOrder.toXYZVector();
00903 
00904     nearestRotation(xyzRot, targetXyz, order());
00905 
00906     setXYZVector(xyzRot);
00907 }
00908 
00909 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
00910 #pragma warning(default:4244)
00911 #endif
00912 
00913 } // namespace Imath
00914 
00915 
00916 #endif