git.maemo.org Git - opencv/blob - 3rdparty/include/OpenEXR/ImathEuler.h

   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_IMATHEULER_H
  38 #define INCLUDED_IMATHEULER_H
  39
  40 //----------------------------------------------------------------------
  41 //
  42 //      template class Euler<T>
  43 //
  44 //      This class represents euler angle orientations. The class
  45 //      inherits from Vec3 to it can be freely cast. The additional
  46 //      information is the euler priorities rep. This class is
  47 //      essentially a rip off of Ken Shoemake's GemsIV code. It has
  48 //      been modified minimally to make it more understandable, but
  49 //      hardly enough to make it easy to grok completely.
  50 //
  51 //      There are 24 possible combonations of Euler angle
  52 //      representations of which 12 are common in CG and you will
  53 //      probably only use 6 of these which in this scheme are the
  54 //      non-relative-non-repeating types.
  55 //
  56 //      The representations can be partitioned according to two
  57 //      criteria:
  58 //
  59 //         1) Are the angles measured relative to a set of fixed axis
  60 //            or relative to each other (the latter being what happens
  61 //            when rotation matrices are multiplied together and is
  62 //            almost ubiquitous in the cg community)
  63 //
  64 //         2) Is one of the rotations repeated (ala XYX rotation)
  65 //
  66 //      When you construct a given representation from scratch you
  67 //      must order the angles according to their priorities. So, the
  68 //      easiest is a softimage or aerospace (yaw/pitch/roll) ordering
  69 //      of ZYX.
  70 //
  71 //          float x_rot = 1;
  72 //          float y_rot = 2;
  73 //          float z_rot = 3;
  74 //
  75 //          Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX);
  76 //              -or-
  77 //          Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX );
  78 //
  79 //      If instead, the order was YXZ for instance you would have to
  80 //      do this:
  81 //
  82 //          float x_rot = 1;
  83 //          float y_rot = 2;
  84 //          float z_rot = 3;
  85 //
  86 //          Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
  87 //              -or-
  88 //          Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ );
  89 //
  90 //      Notice how the order you put the angles into the three slots
  91 //      should correspond to the enum (YXZ) ordering. The input angle
  92 //      vector is called the "ijk" vector -- not an "xyz" vector. The
  93 //      ijk vector order is the same as the enum. If you treat the
  94 //      Euler<> as a Vec<> (which it inherts from) you will find the
  95 //      angles are ordered in the same way, i.e.:
  96 //
  97 //          V3f v = angles;
  98 //          // v.x == y_rot, v.y == x_rot, v.z == z_rot
  99 //
 100 //      If you just want the x, y, and z angles stored in a vector in
 101 //      that order, you can do this:
 102 //
 103 //          V3f v = angles.toXYZVector()
 104 //          // v.x == x_rot, v.y == y_rot, v.z == z_rot
 105 //
 106 //      If you want to set the Euler with an XYZVector use the
 107 //      optional layout argument:
 108 //
 109 //          Eulerf angles(x_rot, y_rot, z_rot,
 110 //                        Eulerf::YXZ,
 111 //                        Eulerf::XYZLayout);
 112 //
 113 //      This is the same as:
 114 //
 115 //          Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
 116 //
 117 //      Note that this won't do anything intelligent if you have a
 118 //      repeated axis in the euler angles (e.g. XYX)
 119 //
 120 //      If you need to use the "relative" versions of these, you will
 121 //      need to use the "r" enums.
 122 //
 123 //      The units of the rotation angles are assumed to be radians.
 124 //
 125 //----------------------------------------------------------------------
 126
 127
 128 #include "ImathMath.h"
 129 #include "ImathVec.h"
 130 #include "ImathQuat.h"
 131 #include "ImathMatrix.h"
 132 #include "ImathLimits.h"
 133 #include <iostream>
 134
 135 namespace Imath {
 136
 137 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
 138 // Disable MS VC++ warnings about conversion from double to float
 139 #pragma warning(disable:4244)
 140 #endif
 141
 142 template <class T>
 143 class Euler : public Vec3<T>
 144 {
 145   public:
 146
 147     using Vec3<T>::x;
 148     using Vec3<T>::y;
 149     using Vec3<T>::z;
 150
 151     enum Order
 152     {
 153         //
 154         //  All 24 possible orderings
 155         //
 156
 157         XYZ     = 0x0101,       // "usual" orderings
 158         XZY     = 0x0001,
 159         YZX     = 0x1101,
 160         YXZ     = 0x1001,
 161         ZXY     = 0x2101,
 162         ZYX     = 0x2001,
 163
 164         XZX     = 0x0011,       // first axis repeated
 165         XYX     = 0x0111,
 166         YXY     = 0x1011,
 167         YZY     = 0x1111,
 168         ZYZ     = 0x2011,
 169         ZXZ     = 0x2111,
 170
 171         XYZr    = 0x2000,       // relative orderings -- not common
 172         XZYr    = 0x2100,
 173         YZXr    = 0x1000,
 174         YXZr    = 0x1100,
 175         ZXYr    = 0x0000,
 176         ZYXr    = 0x0100,
 177
 178         XZXr    = 0x2110,       // relative first axis repeated
 179         XYXr    = 0x2010,
 180         YXYr    = 0x1110,
 181         YZYr    = 0x1010,
 182         ZYZr    = 0x0110,
 183         ZXZr    = 0x0010,
 184         //          ||||
 185         //          VVVV
 186         //  Legend: ABCD
 187         //  A -> Initial Axis (0==x, 1==y, 2==z)
 188         //  B -> Parity Even (1==true)
 189         //  C -> Initial Repeated (1==true)
 190         //  D -> Frame Static (1==true)
 191         //
 192
 193         Legal   =   XYZ | XZY | YZX | YXZ | ZXY | ZYX |
 194                     XZX | XYX | YXY | YZY | ZYZ | ZXZ |
 195                     XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr|
 196                     XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr,
 197
 198         Min     = 0x0000,
 199         Max     = 0x2111,
 200         Default = XYZ
 201     };
 202
 203     enum Axis { X = 0, Y = 1, Z = 2 };
 204
 205     enum InputLayout { XYZLayout, IJKLayout };
 206
 207     //----------------------------------------------------------------
 208     //  Constructors -- all default to ZYX non-relative ala softimage
 209     //                  (where there is no argument to specify it)
 210     //----------------------------------------------------------------
 211
 212     Euler();
 213     Euler(const Euler&);
 214     Euler(Order p);
 215     Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
 216     Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
 217     Euler(const Euler<T> &euler, Order newp);
 218     Euler(const Matrix33<T> &, Order o = Default);
 219     Euler(const Matrix44<T> &, Order o = Default);
 220
 221     //---------------------------------
 222     //  Algebraic functions/ Operators
 223     //---------------------------------
 224
 225     const Euler<T>&     operator=  (const Euler<T>&);
 226     const Euler<T>&     operator=  (const Vec3<T>&);
 227
 228     //--------------------------------------------------------
 229     //  Set the euler value
 230     //  This does NOT convert the angles, but setXYZVector()
 231     //  does reorder the input vector.
 232     //--------------------------------------------------------
 233
 234     static bool         legal(Order);
 235
 236     void                setXYZVector(const Vec3<T> &);
 237
 238     Order               order() const;
 239     void                setOrder(Order);
 240
 241     void                set(Axis initial,
 242                             bool relative,
 243                             bool parityEven,
 244                             bool firstRepeats);
 245
 246     //---------------------------------------------------------
 247     //  Conversions, toXYZVector() reorders the angles so that
 248     //  the X rotation comes first, followed by the Y and Z
 249     //  in cases like XYX ordering, the repeated angle will be
 250     //  in the "z" component
 251     //---------------------------------------------------------
 252
 253     void                extract(const Matrix33<T>&);
 254     void                extract(const Matrix44<T>&);
 255     void                extract(const Quat<T>&);
 256
 257     Matrix33<T>         toMatrix33() const;
 258     Matrix44<T>         toMatrix44() const;
 259     Quat<T>             toQuat() const;
 260     Vec3<T>             toXYZVector() const;
 261
 262     //---------------------------------------------------
 263     //  Use this function to unpack angles from ijk form
 264     //---------------------------------------------------
 265
 266     void                angleOrder(int &i, int &j, int &k) const;
 267
 268     //---------------------------------------------------
 269     //  Use this function to determine mapping from xyz to ijk
 270     // - reshuffles the xyz to match the order
 271     //---------------------------------------------------
 272
 273     void                angleMapping(int &i, int &j, int &k) const;
 274
 275     //----------------------------------------------------------------------
 276     //
 277     //  Utility methods for getting continuous rotations. None of these
 278     //  methods change the orientation given by its inputs (or at least
 279     //  that is the intent).
 280     //
 281     //    angleMod() converts an angle to its equivalent in [-PI, PI]
 282     //
 283     //    simpleXYZRotation() adjusts xyzRot so that its components differ
 284     //                        from targetXyzRot by no more than +-PI
 285     //
 286     //    nearestRotation() adjusts xyzRot so that its components differ
 287     //                      from targetXyzRot by as little as possible.
 288     //                      Note that xyz here really means ijk, because
 289     //                      the order must be provided.
 290     //
 291     //    makeNear() adjusts "this" Euler so that its components differ
 292     //               from target by as little as possible. This method
 293     //               might not make sense for Eulers with different order
 294     //               and it probably doesn't work for repeated axis and
 295     //               relative orderings (TODO).
 296     //
 297     //-----------------------------------------------------------------------
 298
 299     static float        angleMod (T angle);
 300     static void         simpleXYZRotation (Vec3<T> &xyzRot,
 301                                            const Vec3<T> &targetXyzRot);
 302     static void         nearestRotation (Vec3<T> &xyzRot,
 303                                          const Vec3<T> &targetXyzRot,
 304                                          Order order = XYZ);
 305
 306     void                makeNear (const Euler<T> &target);
 307
 308     bool                frameStatic() const { return _frameStatic; }
 309     bool                initialRepeated() const { return _initialRepeated; }
 310     bool                parityEven() const { return _parityEven; }
 311     Axis                initialAxis() const { return _initialAxis; }
 312
 313   protected:
 314
 315     bool                _frameStatic     : 1;   // relative or static rotations
 316     bool                _initialRepeated : 1;   // init axis repeated as last
 317     bool                _parityEven      : 1;   // "parity of axis permutation"
 318 #if defined _WIN32 || defined _WIN64
 319     Axis                _initialAxis     ;      // First axis of rotation
 320 #else
 321     Axis                _initialAxis     : 2;   // First axis of rotation
 322 #endif
 323 };
 324
 325
 326 //--------------------
 327 // Convenient typedefs
 328 //--------------------
 329
 330 typedef Euler<float>    Eulerf;
 331 typedef Euler<double>   Eulerd;
 332
 333
 334 //---------------
 335 // Implementation
 336 //---------------
 337
 338 template<class T>
 339 inline void
 340  Euler<T>::angleOrder(int &i, int &j, int &k) const
 341 {
 342     i = _initialAxis;
 343     j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
 344     k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
 345 }
 346
 347 template<class T>
 348 inline void
 349  Euler<T>::angleMapping(int &i, int &j, int &k) const
 350 {
 351     int m[3];
 352
 353     m[_initialAxis] = 0;
 354     m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
 355     m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
 356     i = m[0];
 357     j = m[1];
 358     k = m[2];
 359 }
 360
 361 template<class T>
 362 inline void
 363 Euler<T>::setXYZVector(const Vec3<T> &v)
 364 {
 365     int i,j,k;
 366     angleMapping(i,j,k);
 367     (*this)[i] = v.x;
 368     (*this)[j] = v.y;
 369     (*this)[k] = v.z;
 370 }
 371
 372 template<class T>
 373 inline Vec3<T>
 374 Euler<T>::toXYZVector() const
 375 {
 376     int i,j,k;
 377     angleMapping(i,j,k);
 378     return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
 379 }
 380
 381
 382 template<class T>
 383 Euler<T>::Euler() :
 384     Vec3<T>(0,0,0),
 385     _frameStatic(true),
 386     _initialRepeated(false),
 387     _parityEven(true),
 388     _initialAxis(X)
 389 {}
 390
 391 template<class T>
 392 Euler<T>::Euler(typename Euler<T>::Order p) :
 393     Vec3<T>(0,0,0),
 394     _frameStatic(true),
 395     _initialRepeated(false),
 396     _parityEven(true),
 397     _initialAxis(X)
 398 {
 399     setOrder(p);
 400 }
 401
 402 template<class T>
 403 inline Euler<T>::Euler( const Vec3<T> &v,
 404                         typename Euler<T>::Order p,
 405                         typename Euler<T>::InputLayout l )
 406 {
 407     setOrder(p);
 408     if ( l == XYZLayout ) setXYZVector(v);
 409     else { x = v.x; y = v.y; z = v.z; }
 410 }
 411
 412 template<class T>
 413 inline Euler<T>::Euler(const Euler<T> &euler)
 414 {
 415     operator=(euler);
 416 }
 417
 418 template<class T>
 419 inline Euler<T>::Euler(const Euler<T> &euler,Order p)
 420 {
 421     setOrder(p);
 422     Matrix33<T> M = euler.toMatrix33();
 423     extract(M);
 424 }
 425
 426 template<class T>
 427 inline Euler<T>::Euler( T xi, T yi, T zi,
 428                         typename Euler<T>::Order p,
 429                         typename Euler<T>::InputLayout l)
 430 {
 431     setOrder(p);
 432     if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
 433     else { x = xi; y = yi; z = zi; }
 434 }
 435
 436 template<class T>
 437 inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
 438 {
 439     setOrder(p);
 440     extract(M);
 441 }
 442
 443 template<class T>
 444 inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
 445 {
 446     setOrder(p);
 447     extract(M);
 448 }
 449
 450 template<class T>
 451 inline void Euler<T>::extract(const Quat<T> &q)
 452 {
 453     extract(q.toMatrix33());
 454 }
 455
 456 template<class T>
 457 void Euler<T>::extract(const Matrix33<T> &M)
 458 {
 459     int i,j,k;
 460     angleOrder(i,j,k);
 461
 462     if (_initialRepeated)
 463     {
 464         //
 465         // Extract the first angle, x.
 466         //
 467
 468         x = Math<T>::atan2 (M[j][i], M[k][i]);
 469
 470         //
 471         // Remove the x rotation from M, so that the remaining
 472         // rotation, N, is only around two axes, and gimbal lock
 473         // cannot occur.
 474         //
 475
 476         Vec3<T> r (0, 0, 0);
 477         r[i] = (_parityEven? -x: x);
 478
 479         Matrix44<T> N;
 480         N.rotate (r);
 481
 482         N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
 483                              M[1][0], M[1][1], M[1][2], 0,
 484                              M[2][0], M[2][1], M[2][2], 0,
 485                              0,       0,       0,       1);
 486         //
 487         // Extract the other two angles, y and z, from N.
 488         //
 489
 490         T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
 491         y = Math<T>::atan2 (sy, N[i][i]);
 492         z = Math<T>::atan2 (N[j][k], N[j][j]);
 493     }
 494     else
 495     {
 496         //
 497         // Extract the first angle, x.
 498         //
 499
 500         x = Math<T>::atan2 (M[j][k], M[k][k]);
 501
 502         //
 503         // Remove the x rotation from M, so that the remaining
 504         // rotation, N, is only around two axes, and gimbal lock
 505         // cannot occur.
 506         //
 507
 508         Vec3<T> r (0, 0, 0);
 509         r[i] = (_parityEven? -x: x);
 510
 511         Matrix44<T> N;
 512         N.rotate (r);
 513
 514         N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
 515                              M[1][0], M[1][1], M[1][2], 0,
 516                              M[2][0], M[2][1], M[2][2], 0,
 517                              0,       0,       0,       1);
 518         //
 519         // Extract the other two angles, y and z, from N.
 520         //
 521
 522         T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
 523         y = Math<T>::atan2 (-N[i][k], cy);
 524         z = Math<T>::atan2 (-N[j][i], N[j][j]);
 525     }
 526
 527     if (!_parityEven)
 528         *this *= -1;
 529
 530     if (!_frameStatic)
 531     {
 532         T t = x;
 533         x = z;
 534         z = t;
 535     }
 536 }
 537
 538 template<class T>
 539 void Euler<T>::extract(const Matrix44<T> &M)
 540 {
 541     int i,j,k;
 542     angleOrder(i,j,k);
 543
 544     if (_initialRepeated)
 545     {
 546         //
 547         // Extract the first angle, x.
 548         //
 549
 550         x = Math<T>::atan2 (M[j][i], M[k][i]);
 551
 552         //
 553         // Remove the x rotation from M, so that the remaining
 554         // rotation, N, is only around two axes, and gimbal lock
 555         // cannot occur.
 556         //
 557
 558         Vec3<T> r (0, 0, 0);
 559         r[i] = (_parityEven? -x: x);
 560
 561         Matrix44<T> N;
 562         N.rotate (r);
 563         N = N * M;
 564
 565         //
 566         // Extract the other two angles, y and z, from N.
 567         //
 568
 569         T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
 570         y = Math<T>::atan2 (sy, N[i][i]);
 571         z = Math<T>::atan2 (N[j][k], N[j][j]);
 572     }
 573     else
 574     {
 575         //
 576         // Extract the first angle, x.
 577         //
 578
 579         x = Math<T>::atan2 (M[j][k], M[k][k]);
 580
 581         //
 582         // Remove the x rotation from M, so that the remaining
 583         // rotation, N, is only around two axes, and gimbal lock
 584         // cannot occur.
 585         //
 586
 587         Vec3<T> r (0, 0, 0);
 588         r[i] = (_parityEven? -x: x);
 589
 590         Matrix44<T> N;
 591         N.rotate (r);
 592         N = N * M;
 593
 594         //
 595         // Extract the other two angles, y and z, from N.
 596         //
 597
 598         T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
 599         y = Math<T>::atan2 (-N[i][k], cy);
 600         z = Math<T>::atan2 (-N[j][i], N[j][j]);
 601     }
 602
 603     if (!_parityEven)
 604         *this *= -1;
 605
 606     if (!_frameStatic)
 607     {
 608         T t = x;
 609         x = z;
 610         z = t;
 611     }
 612 }
 613
 614 template<class T>
 615 Matrix33<T> Euler<T>::toMatrix33() const
 616 {
 617     int i,j,k;
 618     angleOrder(i,j,k);
 619
 620     Vec3<T> angles;
 621
 622     if ( _frameStatic ) angles = (*this);
 623     else angles = Vec3<T>(z,y,x);
 624
 625     if ( !_parityEven ) angles *= -1.0;
 626
 627     T ci = Math<T>::cos(angles.x);
 628     T cj = Math<T>::cos(angles.y);
 629     T ch = Math<T>::cos(angles.z);
 630     T si = Math<T>::sin(angles.x);
 631     T sj = Math<T>::sin(angles.y);
 632     T sh = Math<T>::sin(angles.z);
 633
 634     T cc = ci*ch;
 635     T cs = ci*sh;
 636     T sc = si*ch;
 637     T ss = si*sh;
 638
 639     Matrix33<T> M;
 640
 641     if ( _initialRepeated )
 642     {
 643         M[i][i] = cj;     M[j][i] =  sj*si;    M[k][i] =  sj*ci;
 644         M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
 645         M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
 646     }
 647     else
 648     {
 649         M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
 650         M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
 651         M[i][k] = -sj;   M[j][k] = cj*si;    M[k][k] = cj*ci;
 652     }
 653
 654     return M;
 655 }
 656
 657 template<class T>
 658 Matrix44<T> Euler<T>::toMatrix44() const
 659 {
 660     int i,j,k;
 661     angleOrder(i,j,k);
 662
 663     Vec3<T> angles;
 664
 665     if ( _frameStatic ) angles = (*this);
 666     else angles = Vec3<T>(z,y,x);
 667
 668     if ( !_parityEven ) angles *= -1.0;
 669
 670     T ci = Math<T>::cos(angles.x);
 671     T cj = Math<T>::cos(angles.y);
 672     T ch = Math<T>::cos(angles.z);
 673     T si = Math<T>::sin(angles.x);
 674     T sj = Math<T>::sin(angles.y);
 675     T sh = Math<T>::sin(angles.z);
 676
 677     T cc = ci*ch;
 678     T cs = ci*sh;
 679     T sc = si*ch;
 680     T ss = si*sh;
 681
 682     Matrix44<T> M;
 683
 684     if ( _initialRepeated )
 685     {
 686         M[i][i] = cj;     M[j][i] =  sj*si;    M[k][i] =  sj*ci;
 687         M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
 688         M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
 689     }
 690     else
 691     {
 692         M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
 693         M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
 694         M[i][k] = -sj;   M[j][k] = cj*si;    M[k][k] = cj*ci;
 695     }
 696
 697     return M;
 698 }
 699
 700 template<class T>
 701 Quat<T> Euler<T>::toQuat() const
 702 {
 703     Vec3<T> angles;
 704     int i,j,k;
 705     angleOrder(i,j,k);
 706
 707     if ( _frameStatic ) angles = (*this);
 708     else angles = Vec3<T>(z,y,x);
 709
 710     if ( !_parityEven ) angles.y = -angles.y;
 711
 712     T ti = angles.x*0.5;
 713     T tj = angles.y*0.5;
 714     T th = angles.z*0.5;
 715     T ci = Math<T>::cos(ti);
 716     T cj = Math<T>::cos(tj);
 717     T ch = Math<T>::cos(th);
 718     T si = Math<T>::sin(ti);
 719     T sj = Math<T>::sin(tj);
 720     T sh = Math<T>::sin(th);
 721     T cc = ci*ch;
 722     T cs = ci*sh;
 723     T sc = si*ch;
 724     T ss = si*sh;
 725
 726     T parity = _parityEven ? 1.0 : -1.0;
 727
 728     Quat<T> q;
 729     Vec3<T> a;
 730
 731     if ( _initialRepeated )
 732     {
 733         a[i]    = cj*(cs + sc);
 734         a[j]    = sj*(cc + ss) * parity,
 735         a[k]    = sj*(cs - sc);
 736         q.r     = cj*(cc - ss);
 737     }
 738     else
 739     {
 740         a[i]    = cj*sc - sj*cs,
 741         a[j]    = (cj*ss + sj*cc) * parity,
 742         a[k]    = cj*cs - sj*sc;
 743         q.r     = cj*cc + sj*ss;
 744     }
 745
 746     q.v = a;
 747
 748     return q;
 749 }
 750
 751 template<class T>
 752 inline bool
 753 Euler<T>::legal(typename Euler<T>::Order order)
 754 {
 755     return (order & ~Legal) ? false : true;
 756 }
 757
 758 template<class T>
 759 typename Euler<T>::Order
 760 Euler<T>::order() const
 761 {
 762     int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));
 763
 764     if (_parityEven)      foo |= 0x0100;
 765     if (_initialRepeated) foo |= 0x0010;
 766     if (_frameStatic)     foo++;
 767
 768     return (Order)foo;
 769 }
 770
 771 template<class T>
 772 inline void Euler<T>::setOrder(typename Euler<T>::Order p)
 773 {
 774     set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis
 775          !(p & 0x1),                            // static?
 776          !!(p & 0x100),                         // permutation even?
 777          !!(p & 0x10));                         // initial repeats?
 778 }
 779
 780 template<class T>
 781 void Euler<T>::set(typename Euler<T>::Axis axis,
 782                    bool relative,
 783                    bool parityEven,
 784                    bool firstRepeats)
 785 {
 786     _initialAxis        = axis;
 787     _frameStatic        = !relative;
 788     _parityEven         = parityEven;
 789     _initialRepeated    = firstRepeats;
 790 }
 791
 792 template<class T>
 793 const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
 794 {
 795     x = euler.x;
 796     y = euler.y;
 797     z = euler.z;
 798     _initialAxis = euler._initialAxis;
 799     _frameStatic = euler._frameStatic;
 800     _parityEven  = euler._parityEven;
 801     _initialRepeated = euler._initialRepeated;
 802     return *this;
 803 }
 804
 805 template<class T>
 806 const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
 807 {
 808     x = v.x;
 809     y = v.y;
 810     z = v.z;
 811     return *this;
 812 }
 813
 814 template<class T>
 815 std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
 816 {
 817     char a[3] = { 'X', 'Y', 'Z' };
 818
 819     const char* r = euler.frameStatic() ? "" : "r";
 820     int i,j,k;
 821     euler.angleOrder(i,j,k);
 822
 823     if ( euler.initialRepeated() ) k = i;
 824
 825     return o << "("
 826              << euler.x << " "
 827              << euler.y << " "
 828              << euler.z << " "
 829              << a[i] << a[j] << a[k] << r << ")";
 830 }
 831
 832 template <class T>
 833 float
 834 Euler<T>::angleMod (T angle)
 835 {
 836     angle = fmod(T (angle), T (2 * M_PI));
 837
 838     if (angle < -M_PI)  angle += 2 * M_PI;
 839     if (angle > +M_PI)  angle -= 2 * M_PI;
 840
 841     return angle;
 842 }
 843
 844 template <class T>
 845 void
 846 Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
 847 {
 848     Vec3<T> d  = xyzRot - targetXyzRot;
 849     xyzRot[0]  = targetXyzRot[0] + angleMod(d[0]);
 850     xyzRot[1]  = targetXyzRot[1] + angleMod(d[1]);
 851     xyzRot[2]  = targetXyzRot[2] + angleMod(d[2]);
 852 }
 853
 854 template <class T>
 855 void
 856 Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
 857                            Order order)
 858 {
 859     int i,j,k;
 860     Euler<T> e (0,0,0, order);
 861     e.angleOrder(i,j,k);
 862
 863     simpleXYZRotation(xyzRot, targetXyzRot);
 864
 865     Vec3<T> otherXyzRot;
 866     otherXyzRot[i] = M_PI+xyzRot[i];
 867     otherXyzRot[j] = M_PI-xyzRot[j];
 868     otherXyzRot[k] = M_PI+xyzRot[k];
 869
 870     simpleXYZRotation(otherXyzRot, targetXyzRot);
 871
 872     Vec3<T> d  = xyzRot - targetXyzRot;
 873     Vec3<T> od = otherXyzRot - targetXyzRot;
 874     T dMag     = d.dot(d);
 875     T odMag    = od.dot(od);
 876
 877     if (odMag < dMag)
 878     {
 879         xyzRot = otherXyzRot;
 880     }
 881 }
 882
 883 template <class T>
 884 void
 885 Euler<T>::makeNear (const Euler<T> &target)
 886 {
 887     Vec3<T> xyzRot    = toXYZVector();
 888     Euler<T> targetSameOrder = Euler<T>(target, order());
 889     Vec3<T> targetXyz = targetSameOrder.toXYZVector();
 890
 891     nearestRotation(xyzRot, targetXyz, order());
 892
 893     setXYZVector(xyzRot);
 894 }
 895
 896 #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
 897 #pragma warning(default:4244)
 898 #endif
 899
 900 } // namespace Imath
 901
 902
 903 #endif