source: NonGTP/OpenEXR/include/Imath/ImathEuler.h @ 855

Revision 855, 21.9 KB checked in by igarcia, 19 years ago (diff)
Line 
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
135namespace Imath {
136
137#if defined PLATFORM_WINDOWS && _MSC_VER
138// Disable MS VC++ warnings about conversion from double to float
139#pragma warning(disable:4244)
140#endif
141
142template <class T>
143class 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#ifdef PLATFORM_WINDOWS
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
330typedef Euler<float>    Eulerf;
331typedef Euler<double>   Eulerd;
332
333
334//---------------
335// Implementation
336//---------------
337
338template<class T>
339inline 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
347template<class T>
348inline 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
361template<class T>
362inline void
363Euler<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
372template<class T>
373inline Vec3<T>
374Euler<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
382template<class T>
383Euler<T>::Euler() :
384    Vec3<T>(0,0,0),
385    _frameStatic(true),
386    _initialRepeated(false),
387    _parityEven(true),
388    _initialAxis(X)
389{}
390
391template<class T>
392Euler<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
402template<class T>
403inline 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
412template<class T>
413inline Euler<T>::Euler(const Euler<T> &euler)
414{
415    operator=(euler);
416}
417
418template<class T>
419inline Euler<T>::Euler(const Euler<T> &euler,Order p)
420{
421    setOrder(p);
422    Matrix33<T> M = euler.toMatrix33();
423    extract(M);
424}
425
426template<class T>
427inline 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
436template<class T>
437inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
438{
439    setOrder(p);
440    extract(M);
441}
442
443template<class T>
444inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
445{
446    setOrder(p);
447    extract(M);
448}
449
450template<class T>
451inline void Euler<T>::extract(const Quat<T> &q)
452{
453    extract(q.toMatrix33());
454}
455
456template<class T>
457void 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
538template<class T>
539void 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
614template<class T>
615Matrix33<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
657template<class T>
658Matrix44<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
700template<class T>
701Quat<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
751template<class T>
752inline bool
753Euler<T>::legal(typename Euler<T>::Order order)
754{
755    return (order & ~Legal) ? false : true;
756}
757
758template<class T>
759typename Euler<T>::Order
760Euler<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
771template<class T>
772inline 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
780template<class T>
781void 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
792template<class T>
793const 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
805template<class T>
806const 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
814template<class T>
815std::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
832template <class T>
833float
834Euler<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
844template <class T>
845void
846Euler<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
854template <class T>
855void
856Euler<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
883template <class T>
884void
885Euler<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 PLATFORM_WINDOWS && _MSC_VER
897#pragma warning(default:4244)
898#endif
899
900} // namespace Imath
901
902
903#endif
Note: See TracBrowser for help on using the repository browser.