/src/FreeImage/Source/OpenEXR/Imath/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
135namespace 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
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    // The Euler-from-matrix constructors assume that the matrix does
212    // not include shear or non-uniform scaling, but the constructors
213    // do not examine the matrix to verify this assumption.  If necessary,
214    // you can adjust the matrix by calling the removeScalingAndShear()
215    // function, defined in ImathMatrixAlgo.h.
216    //--------------------------------------------------------------------
217
218    Euler();
219    Euler(const Euler&);
220    Euler(Order p);
221    Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
222    Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
223    Euler(const Euler<T> &euler, Order newp);
224    Euler(const Matrix33<T> &, Order o = Default);
225    Euler(const Matrix44<T> &, Order o = Default);
226
227    //---------------------------------
228    //  Algebraic functions/ Operators
229    //---------------------------------
230
231    const Euler<T>&	operator=  (const Euler<T>&);
232    const Euler<T>&	operator=  (const Vec3<T>&);
233
234    //--------------------------------------------------------
235    //	Set the euler value
236    //  This does NOT convert the angles, but setXYZVector() 
237    //	does reorder the input vector.
238    //--------------------------------------------------------
239
240    static bool		legal(Order);
241
242    void		setXYZVector(const Vec3<T> &);
243
244    Order		order() const;
245    void		setOrder(Order);
246
247    void		set(Axis initial,
248			    bool relative,
249			    bool parityEven,
250			    bool firstRepeats);
251
252    //------------------------------------------------------------
253    //	Conversions, toXYZVector() reorders the angles so that
254    //  the X rotation comes first, followed by the Y and Z
255    //  in cases like XYX ordering, the repeated angle will be
256    //	in the "z" component
257    //
258    // The Euler-from-matrix extract() functions assume that the
259    // matrix does not include shear or non-uniform scaling, but
260    // the extract() functions do not examine the matrix to verify
261    // this assumption.  If necessary, you can adjust the matrix
262    // by calling the removeScalingAndShear() function, defined
263    // in ImathMatrixAlgo.h.
264    //------------------------------------------------------------
265
266    void		extract(const Matrix33<T>&);
267    void		extract(const Matrix44<T>&);
268    void		extract(const Quat<T>&);
269
270    Matrix33<T>		toMatrix33() const;
271    Matrix44<T>		toMatrix44() const;
272    Quat<T>		toQuat() const;
273    Vec3<T>		toXYZVector() const;
274
275    //---------------------------------------------------
276    //	Use this function to unpack angles from ijk form
277    //---------------------------------------------------
278
279    void		angleOrder(int &i, int &j, int &k) const;
280
281    //---------------------------------------------------
282    //	Use this function to determine mapping from xyz to ijk
283    // - reshuffles the xyz to match the order
284    //---------------------------------------------------
285    
286    void		angleMapping(int &i, int &j, int &k) const;
287
288    //----------------------------------------------------------------------
289    //
290    //  Utility methods for getting continuous rotations. None of these
291    //  methods change the orientation given by its inputs (or at least
292    //  that is the intent).
293    //
294    //    angleMod() converts an angle to its equivalent in [-PI, PI]
295    //
296    //    simpleXYZRotation() adjusts xyzRot so that its components differ
297    //                        from targetXyzRot by no more than +-PI
298    //
299    //    nearestRotation() adjusts xyzRot so that its components differ
300    //                      from targetXyzRot by as little as possible.
301    //                      Note that xyz here really means ijk, because
302    //                      the order must be provided.
303    //
304    //    makeNear() adjusts "this" Euler so that its components differ
305    //               from target by as little as possible. This method
306    //               might not make sense for Eulers with different order
307    //               and it probably doesn't work for repeated axis and
308    //               relative orderings (TODO).
309    //
310    //-----------------------------------------------------------------------
311
312    static float	angleMod (T angle);
313    static void		simpleXYZRotation (Vec3<T> &xyzRot,
314					   const Vec3<T> &targetXyzRot);
315    static void		nearestRotation (Vec3<T> &xyzRot,
316					 const Vec3<T> &targetXyzRot,
317					 Order order = XYZ);
318
319    void		makeNear (const Euler<T> &target);
320
321    bool		frameStatic() const { return _frameStatic; }
322    bool		initialRepeated() const { return _initialRepeated; }
323    bool		parityEven() const { return _parityEven; }
324    Axis		initialAxis() const { return _initialAxis; }
325
326  protected:
327
328    bool		_frameStatic	 : 1;	// relative or static rotations
329    bool		_initialRepeated : 1;	// init axis repeated as last
330    bool		_parityEven	 : 1;	// "parity of axis permutation"
331#if defined _WIN32 || defined _WIN64
332    Axis		_initialAxis	 ;	// First axis of rotation
333#else
334    Axis		_initialAxis	 : 2;	// First axis of rotation
335#endif
336};
337
338
339//--------------------
340// Convenient typedefs
341//--------------------
342
343typedef Euler<float>	Eulerf;
344typedef Euler<double>	Eulerd;
345
346
347//---------------
348// Implementation
349//---------------
350
351template<class T>
352inline void
353 Euler<T>::angleOrder(int &i, int &j, int &k) const
354{
355    i = _initialAxis;
356    j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
357    k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
358}
359
360template<class T>
361inline void
362 Euler<T>::angleMapping(int &i, int &j, int &k) const
363{
364    int m[3];
365
366    m[_initialAxis] = 0;
367    m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
368    m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
369    i = m[0];
370    j = m[1];
371    k = m[2];
372}
373
374template<class T>
375inline void
376Euler<T>::setXYZVector(const Vec3<T> &v)
377{
378    int i,j,k;
379    angleMapping(i,j,k);
380    (*this)[i] = v.x;
381    (*this)[j] = v.y;
382    (*this)[k] = v.z;
383}
384
385template<class T>
386inline Vec3<T>
387Euler<T>::toXYZVector() const
388{
389    int i,j,k;
390    angleMapping(i,j,k);
391    return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
392}
393
394
395template<class T>
396Euler<T>::Euler() :
397    Vec3<T>(0,0,0),
398    _frameStatic(true),
399    _initialRepeated(false),
400    _parityEven(true),
401    _initialAxis(X)
402{}
403
404template<class T>
405Euler<T>::Euler(typename Euler<T>::Order p) :
406    Vec3<T>(0,0,0),
407    _frameStatic(true),
408    _initialRepeated(false),
409    _parityEven(true),
410    _initialAxis(X)
411{
412    setOrder(p);
413}
414
415template<class T>
416inline Euler<T>::Euler( const Vec3<T> &v, 
417			typename Euler<T>::Order p, 
418			typename Euler<T>::InputLayout l ) 
419{
420    setOrder(p); 
421    if ( l == XYZLayout ) setXYZVector(v);
422    else { x = v.x; y = v.y; z = v.z; }
423}
424
425template<class T>
426inline Euler<T>::Euler(const Euler<T> &euler)
427{
428    operator=(euler);
429}
430
431template<class T>
432inline Euler<T>::Euler(const Euler<T> &euler,Order p)
433{
434    setOrder(p);
435    Matrix33<T> M = euler.toMatrix33();
436    extract(M);
437}
438
439template<class T>
440inline Euler<T>::Euler( T xi, T yi, T zi, 
441			typename Euler<T>::Order p,
442			typename Euler<T>::InputLayout l)
443{
444    setOrder(p);
445    if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
446    else { x = xi; y = yi; z = zi; }
447}
448
449template<class T>
450inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
451{
452    setOrder(p);
453    extract(M);
454}
455
456template<class T>
457inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
458{
459    setOrder(p);
460    extract(M);
461}
462
463template<class T>
464inline void Euler<T>::extract(const Quat<T> &q)
465{
466    extract(q.toMatrix33());
467}
468
469template<class T>
470void Euler<T>::extract(const Matrix33<T> &M)
471{
472    int i,j,k;
473    angleOrder(i,j,k);
474
475    if (_initialRepeated)
476    {
477	//
478	// Extract the first angle, x.
479	// 
480
481	x = Math<T>::atan2 (M[j][i], M[k][i]);
482
483	//
484	// Remove the x rotation from M, so that the remaining
485	// rotation, N, is only around two axes, and gimbal lock
486	// cannot occur.
487	//
488
489	Vec3<T> r (0, 0, 0);
490	r[i] = (_parityEven? -x: x);
491
492	Matrix44<T> N;
493	N.rotate (r);
494
495	N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
496			     M[1][0], M[1][1], M[1][2], 0,
497			     M[2][0], M[2][1], M[2][2], 0,
498			     0,       0,       0,       1);
499	//
500	// Extract the other two angles, y and z, from N.
501	//
502
503	T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
504	y = Math<T>::atan2 (sy, N[i][i]);
505	z = Math<T>::atan2 (N[j][k], N[j][j]);
506    }
507    else
508    {
509	//
510	// Extract the first angle, x.
511	// 
512
513	x = Math<T>::atan2 (M[j][k], M[k][k]);
514
515	//
516	// Remove the x rotation from M, so that the remaining
517	// rotation, N, is only around two axes, and gimbal lock
518	// cannot occur.
519	//
520
521	Vec3<T> r (0, 0, 0);
522	r[i] = (_parityEven? -x: x);
523
524	Matrix44<T> N;
525	N.rotate (r);
526
527	N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
528			     M[1][0], M[1][1], M[1][2], 0,
529			     M[2][0], M[2][1], M[2][2], 0,
530			     0,       0,       0,       1);
531	//
532	// Extract the other two angles, y and z, from N.
533	//
534
535	T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
536	y = Math<T>::atan2 (-N[i][k], cy);
537	z = Math<T>::atan2 (-N[j][i], N[j][j]);
538    }
539
540    if (!_parityEven)
541	*this *= -1;
542
543    if (!_frameStatic)
544    {
545	T t = x;
546	x = z;
547	z = t;
548    }
549}
550
551template<class T>
552void Euler<T>::extract(const Matrix44<T> &M)
553{
554    int i,j,k;
555    angleOrder(i,j,k);
556
557    if (_initialRepeated)
558    {
559	//
560	// Extract the first angle, x.
561	// 
562
563	x = Math<T>::atan2 (M[j][i], M[k][i]);
564
565	//
566	// Remove the x rotation from M, so that the remaining
567	// rotation, N, is only around two axes, and gimbal lock
568	// cannot occur.
569	//
570
571	Vec3<T> r (0, 0, 0);
572	r[i] = (_parityEven? -x: x);
573
574	Matrix44<T> N;
575	N.rotate (r);
576	N = N * M;
577
578	//
579	// Extract the other two angles, y and z, from N.
580	//
581
582	T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
583	y = Math<T>::atan2 (sy, N[i][i]);
584	z = Math<T>::atan2 (N[j][k], N[j][j]);
585    }
586    else
587    {
588	//
589	// Extract the first angle, x.
590	// 
591
592	x = Math<T>::atan2 (M[j][k], M[k][k]);
593
594	//
595	// Remove the x rotation from M, so that the remaining
596	// rotation, N, is only around two axes, and gimbal lock
597	// cannot occur.
598	//
599
600	Vec3<T> r (0, 0, 0);
601	r[i] = (_parityEven? -x: x);
602
603	Matrix44<T> N;
604	N.rotate (r);
605	N = N * M;
606
607	//
608	// Extract the other two angles, y and z, from N.
609	//
610
611	T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
612	y = Math<T>::atan2 (-N[i][k], cy);
613	z = Math<T>::atan2 (-N[j][i], N[j][j]);
614    }
615
616    if (!_parityEven)
617	*this *= -1;
618
619    if (!_frameStatic)
620    {
621	T t = x;
622	x = z;
623	z = t;
624    }
625}
626
627template<class T>
628Matrix33<T> Euler<T>::toMatrix33() const
629{
630    int i,j,k;
631    angleOrder(i,j,k);
632
633    Vec3<T> angles;
634
635    if ( _frameStatic ) angles = (*this);
636    else angles = Vec3<T>(z,y,x);
637
638    if ( !_parityEven ) angles *= -1.0;
639
640    T ci = Math<T>::cos(angles.x);
641    T cj = Math<T>::cos(angles.y);
642    T ch = Math<T>::cos(angles.z);
643    T si = Math<T>::sin(angles.x);
644    T sj = Math<T>::sin(angles.y);
645    T sh = Math<T>::sin(angles.z);
646
647    T cc = ci*ch;
648    T cs = ci*sh;
649    T sc = si*ch;
650    T ss = si*sh;
651
652    Matrix33<T> M;
653
654    if ( _initialRepeated )
655    {
656	M[i][i] = cj;	  M[j][i] =  sj*si;    M[k][i] =  sj*ci;
657	M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
658	M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
659    }
660    else
661    {
662	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
663	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
664	M[i][k] = -sj;	 M[j][k] = cj*si;    M[k][k] = cj*ci;
665    }
666
667    return M;
668}
669
670template<class T>
671Matrix44<T> Euler<T>::toMatrix44() const
672{
673    int i,j,k;
674    angleOrder(i,j,k);
675
676    Vec3<T> angles;
677
678    if ( _frameStatic ) angles = (*this);
679    else angles = Vec3<T>(z,y,x);
680
681    if ( !_parityEven ) angles *= -1.0;
682
683    T ci = Math<T>::cos(angles.x);
684    T cj = Math<T>::cos(angles.y);
685    T ch = Math<T>::cos(angles.z);
686    T si = Math<T>::sin(angles.x);
687    T sj = Math<T>::sin(angles.y);
688    T sh = Math<T>::sin(angles.z);
689
690    T cc = ci*ch;
691    T cs = ci*sh;
692    T sc = si*ch;
693    T ss = si*sh;
694
695    Matrix44<T> M;
696
697    if ( _initialRepeated )
698    {
699	M[i][i] = cj;	  M[j][i] =  sj*si;    M[k][i] =  sj*ci;
700	M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
701	M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
702    }
703    else
704    {
705	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
706	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
707	M[i][k] = -sj;	 M[j][k] = cj*si;    M[k][k] = cj*ci;
708    }
709
710    return M;
711}
712
713template<class T>
714Quat<T> Euler<T>::toQuat() const
715{
716    Vec3<T> angles;
717    int i,j,k;
718    angleOrder(i,j,k);
719
720    if ( _frameStatic ) angles = (*this);
721    else angles = Vec3<T>(z,y,x);
722
723    if ( !_parityEven ) angles.y = -angles.y;
724
725    T ti = angles.x*0.5;
726    T tj = angles.y*0.5;
727    T th = angles.z*0.5;
728    T ci = Math<T>::cos(ti);
729    T cj = Math<T>::cos(tj);
730    T ch = Math<T>::cos(th);
731    T si = Math<T>::sin(ti);
732    T sj = Math<T>::sin(tj);
733    T sh = Math<T>::sin(th);
734    T cc = ci*ch;
735    T cs = ci*sh;
736    T sc = si*ch;
737    T ss = si*sh;
738
739    T parity = _parityEven ? 1.0 : -1.0;
740
741    Quat<T> q;
742    Vec3<T> a;
743
744    if ( _initialRepeated )
745    {
746	a[i]	= cj*(cs + sc);
747	a[j]	= sj*(cc + ss) * parity,
748	a[k]	= sj*(cs - sc);
749	q.r	= cj*(cc - ss);
750    }
751    else
752    {
753	a[i]	= cj*sc - sj*cs,
754	a[j]	= (cj*ss + sj*cc) * parity,
755	a[k]	= cj*cs - sj*sc;
756	q.r	= cj*cc + sj*ss;
757    }
758
759    q.v = a;
760
761    return q;
762}
763
764template<class T>
765inline bool
766Euler<T>::legal(typename Euler<T>::Order order)
767{
768    return (order & ~Legal) ? false : true;
769}
770
771template<class T>
772typename Euler<T>::Order
773Euler<T>::order() const
774{
775    int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));
776
777    if (_parityEven)	  foo |= 0x0100;
778    if (_initialRepeated) foo |= 0x0010;
779    if (_frameStatic)	  foo++;
780
781    return (Order)foo;
782}
783
784template<class T>
785inline void Euler<T>::setOrder(typename Euler<T>::Order p)
786{
787    set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X),	// initial axis
788	 !(p & 0x1),	    			// static?
789	 !!(p & 0x100),				// permutation even?
790	 !!(p & 0x10));				// initial repeats?
791}
792
793template<class T>
794void Euler<T>::set(typename Euler<T>::Axis axis,
795		   bool relative,
796		   bool parityEven,
797		   bool firstRepeats)
798{
799    _initialAxis	= axis;
800    _frameStatic	= !relative;
801    _parityEven		= parityEven;
802    _initialRepeated	= firstRepeats;
803}
804
805template<class T>
806const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
807{
808    x = euler.x;
809    y = euler.y;
810    z = euler.z;
811    _initialAxis = euler._initialAxis;
812    _frameStatic = euler._frameStatic;
813    _parityEven	 = euler._parityEven;
814    _initialRepeated = euler._initialRepeated;
815    return *this;
816}
817
818template<class T>
819const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
820{
821    x = v.x;
822    y = v.y;
823    z = v.z;
824    return *this;
825}
826
827template<class T>
828std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
829{
830    char a[3] = { 'X', 'Y', 'Z' };
831
832    const char* r = euler.frameStatic() ? "" : "r";
833    int i,j,k;
834    euler.angleOrder(i,j,k);
835
836    if ( euler.initialRepeated() ) k = i;
837
838    return o << "("
839	     << euler.x << " "
840	     << euler.y << " "
841	     << euler.z << " "
842	     << a[i] << a[j] << a[k] << r << ")";
843}
844
845template <class T>
846float
847Euler<T>::angleMod (T angle)
848{
849    angle = fmod(T (angle), T (2 * M_PI));
850
851    if (angle < -M_PI)	angle += 2 * M_PI;
852    if (angle > +M_PI)	angle -= 2 * M_PI;
853
854    return angle;
855}
856
857template <class T>
858void
859Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
860{
861    Vec3<T> d  = xyzRot - targetXyzRot;
862    xyzRot[0]  = targetXyzRot[0] + angleMod(d[0]);
863    xyzRot[1]  = targetXyzRot[1] + angleMod(d[1]);
864    xyzRot[2]  = targetXyzRot[2] + angleMod(d[2]);
865}
866
867template <class T>
868void
869Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
870			   Order order)
871{
872    int i,j,k;
873    Euler<T> e (0,0,0, order);
874    e.angleOrder(i,j,k);
875
876    simpleXYZRotation(xyzRot, targetXyzRot);
877
878    Vec3<T> otherXyzRot;
879    otherXyzRot[i] = M_PI+xyzRot[i];
880    otherXyzRot[j] = M_PI-xyzRot[j];
881    otherXyzRot[k] = M_PI+xyzRot[k];
882
883    simpleXYZRotation(otherXyzRot, targetXyzRot);
884	    
885    Vec3<T> d  = xyzRot - targetXyzRot;
886    Vec3<T> od = otherXyzRot - targetXyzRot;
887    T dMag     = d.dot(d);
888    T odMag    = od.dot(od);
889
890    if (odMag < dMag)
891    {
892	xyzRot = otherXyzRot;
893    }
894}
895
896template <class T>
897void
898Euler<T>::makeNear (const Euler<T> &target)
899{
900    Vec3<T> xyzRot    = toXYZVector();
901    Euler<T> targetSameOrder = Euler<T>(target, order());
902    Vec3<T> targetXyz = targetSameOrder.toXYZVector();
903
904    nearestRotation(xyzRot, targetXyz, order());
905
906    setXYZVector(xyzRot);
907}
908
909#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
910#pragma warning(default:4244)
911#endif
912
913} // namespace Imath
914
915
916#endif