Blame Imath/ImathEuler.h

Packit 8dc392
///////////////////////////////////////////////////////////////////////////
Packit 8dc392
//
Packit 8dc392
// Copyright (c) 2002-2012, Industrial Light & Magic, a division of Lucas
Packit 8dc392
// Digital Ltd. LLC
Packit 8dc392
// 
Packit 8dc392
// All rights reserved.
Packit 8dc392
// 
Packit 8dc392
// Redistribution and use in source and binary forms, with or without
Packit 8dc392
// modification, are permitted provided that the following conditions are
Packit 8dc392
// met:
Packit 8dc392
// *       Redistributions of source code must retain the above copyright
Packit 8dc392
// notice, this list of conditions and the following disclaimer.
Packit 8dc392
// *       Redistributions in binary form must reproduce the above
Packit 8dc392
// copyright notice, this list of conditions and the following disclaimer
Packit 8dc392
// in the documentation and/or other materials provided with the
Packit 8dc392
// distribution.
Packit 8dc392
// *       Neither the name of Industrial Light & Magic nor the names of
Packit 8dc392
// its contributors may be used to endorse or promote products derived
Packit 8dc392
// from this software without specific prior written permission. 
Packit 8dc392
// 
Packit 8dc392
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
Packit 8dc392
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
Packit 8dc392
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
Packit 8dc392
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
Packit 8dc392
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
Packit 8dc392
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
Packit 8dc392
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
Packit 8dc392
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
Packit 8dc392
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
Packit 8dc392
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Packit 8dc392
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Packit 8dc392
//
Packit 8dc392
///////////////////////////////////////////////////////////////////////////
Packit 8dc392
Packit 8dc392
Packit 8dc392
Packit 8dc392
#ifndef INCLUDED_IMATHEULER_H
Packit 8dc392
#define INCLUDED_IMATHEULER_H
Packit 8dc392
Packit 8dc392
//----------------------------------------------------------------------
Packit 8dc392
//
Packit 8dc392
//	template class Euler<T>
Packit 8dc392
//
Packit 8dc392
//      This class represents euler angle orientations. The class
Packit 8dc392
//	inherits from Vec3 to it can be freely cast. The additional
Packit 8dc392
//	information is the euler priorities rep. This class is
Packit 8dc392
//	essentially a rip off of Ken Shoemake's GemsIV code. It has
Packit 8dc392
//	been modified minimally to make it more understandable, but
Packit 8dc392
//	hardly enough to make it easy to grok completely.
Packit 8dc392
//
Packit 8dc392
//	There are 24 possible combonations of Euler angle
Packit 8dc392
//	representations of which 12 are common in CG and you will
Packit 8dc392
//	probably only use 6 of these which in this scheme are the
Packit 8dc392
//	non-relative-non-repeating types. 
Packit 8dc392
//
Packit 8dc392
//	The representations can be partitioned according to two
Packit 8dc392
//	criteria:
Packit 8dc392
//
Packit 8dc392
//	   1) Are the angles measured relative to a set of fixed axis
Packit 8dc392
//	      or relative to each other (the latter being what happens
Packit 8dc392
//	      when rotation matrices are multiplied together and is
Packit 8dc392
//	      almost ubiquitous in the cg community)
Packit 8dc392
//
Packit 8dc392
//	   2) Is one of the rotations repeated (ala XYX rotation)
Packit 8dc392
//
Packit 8dc392
//	When you construct a given representation from scratch you
Packit 8dc392
//	must order the angles according to their priorities. So, the
Packit 8dc392
//	easiest is a softimage or aerospace (yaw/pitch/roll) ordering
Packit 8dc392
//	of ZYX. 
Packit 8dc392
//
Packit 8dc392
//	    float x_rot = 1;
Packit 8dc392
//	    float y_rot = 2;
Packit 8dc392
//	    float z_rot = 3;
Packit 8dc392
//
Packit 8dc392
//	    Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX);
Packit 8dc392
//		-or-
Packit 8dc392
//	    Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX );
Packit 8dc392
//
Packit 8dc392
//	If instead, the order was YXZ for instance you would have to
Packit 8dc392
//	do this:
Packit 8dc392
//
Packit 8dc392
//	    float x_rot = 1;
Packit 8dc392
//	    float y_rot = 2;
Packit 8dc392
//	    float z_rot = 3;
Packit 8dc392
//
Packit 8dc392
//	    Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
Packit 8dc392
//		-or-
Packit 8dc392
//	    Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ );
Packit 8dc392
//
Packit 8dc392
//	Notice how the order you put the angles into the three slots
Packit 8dc392
//	should correspond to the enum (YXZ) ordering. The input angle
Packit 8dc392
//	vector is called the "ijk" vector -- not an "xyz" vector. The
Packit 8dc392
//	ijk vector order is the same as the enum. If you treat the
Packit 8dc392
//	Euler<> as a Vec<> (which it inherts from) you will find the
Packit 8dc392
//	angles are ordered in the same way, i.e.:
Packit 8dc392
//
Packit 8dc392
//	    V3f v = angles;
Packit 8dc392
//	    // v.x == y_rot, v.y == x_rot, v.z == z_rot
Packit 8dc392
//
Packit 8dc392
//	If you just want the x, y, and z angles stored in a vector in
Packit 8dc392
//	that order, you can do this:
Packit 8dc392
//
Packit 8dc392
//	    V3f v = angles.toXYZVector()
Packit 8dc392
//	    // v.x == x_rot, v.y == y_rot, v.z == z_rot
Packit 8dc392
//
Packit 8dc392
//	If you want to set the Euler with an XYZVector use the
Packit 8dc392
//	optional layout argument:
Packit 8dc392
//
Packit 8dc392
//	    Eulerf angles(x_rot, y_rot, z_rot, 
Packit 8dc392
//			  Eulerf::YXZ,
Packit 8dc392
//		          Eulerf::XYZLayout);
Packit 8dc392
//
Packit 8dc392
//	This is the same as:
Packit 8dc392
//
Packit 8dc392
//	    Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
Packit 8dc392
//	    
Packit 8dc392
//	Note that this won't do anything intelligent if you have a
Packit 8dc392
//	repeated axis in the euler angles (e.g. XYX)
Packit 8dc392
//
Packit 8dc392
//	If you need to use the "relative" versions of these, you will
Packit 8dc392
//	need to use the "r" enums. 
Packit 8dc392
//
Packit 8dc392
//      The units of the rotation angles are assumed to be radians.
Packit 8dc392
//
Packit 8dc392
//----------------------------------------------------------------------
Packit 8dc392
Packit 8dc392
Packit 8dc392
#include "ImathMath.h"
Packit 8dc392
#include "ImathVec.h"
Packit 8dc392
#include "ImathQuat.h"
Packit 8dc392
#include "ImathMatrix.h"
Packit 8dc392
#include "ImathLimits.h"
Packit 8dc392
#include "ImathNamespace.h"
Packit 8dc392
Packit 8dc392
#include <iostream>
Packit 8dc392
Packit 8dc392
IMATH_INTERNAL_NAMESPACE_HEADER_ENTER
Packit 8dc392
Packit 8dc392
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
Packit 8dc392
// Disable MS VC++ warnings about conversion from double to float
Packit 8dc392
#pragma warning(disable:4244)
Packit 8dc392
#endif
Packit 8dc392
Packit 8dc392
template <class T>
Packit 8dc392
class Euler : public Vec3<T>
Packit 8dc392
{
Packit 8dc392
  public:
Packit 8dc392
 
Packit 8dc392
    using Vec3<T>::x;
Packit 8dc392
    using Vec3<T>::y;
Packit 8dc392
    using Vec3<T>::z;
Packit 8dc392
Packit 8dc392
    enum Order
Packit 8dc392
    {
Packit 8dc392
	//
Packit 8dc392
	//  All 24 possible orderings
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	XYZ	= 0x0101,	// "usual" orderings
Packit 8dc392
	XZY	= 0x0001,
Packit 8dc392
	YZX	= 0x1101,
Packit 8dc392
	YXZ	= 0x1001,
Packit 8dc392
	ZXY	= 0x2101,
Packit 8dc392
	ZYX	= 0x2001,
Packit 8dc392
	
Packit 8dc392
	XZX	= 0x0011,	// first axis repeated
Packit 8dc392
	XYX	= 0x0111,
Packit 8dc392
	YXY	= 0x1011,
Packit 8dc392
	YZY	= 0x1111,
Packit 8dc392
	ZYZ	= 0x2011,
Packit 8dc392
	ZXZ	= 0x2111,
Packit 8dc392
Packit 8dc392
	XYZr	= 0x2000,	// relative orderings -- not common
Packit 8dc392
	XZYr	= 0x2100,
Packit 8dc392
	YZXr	= 0x1000,
Packit 8dc392
	YXZr	= 0x1100,
Packit 8dc392
	ZXYr	= 0x0000,
Packit 8dc392
	ZYXr	= 0x0100,
Packit 8dc392
	
Packit 8dc392
	XZXr	= 0x2110,	// relative first axis repeated 
Packit 8dc392
	XYXr	= 0x2010,
Packit 8dc392
	YXYr	= 0x1110,
Packit 8dc392
	YZYr	= 0x1010,
Packit 8dc392
	ZYZr	= 0x0110,
Packit 8dc392
	ZXZr	= 0x0010,
Packit 8dc392
	//          ||||
Packit 8dc392
	//          VVVV
Packit 8dc392
	//  Legend: ABCD
Packit 8dc392
	//  A -> Initial Axis (0==x, 1==y, 2==z)
Packit 8dc392
	//  B -> Parity Even (1==true)
Packit 8dc392
	//  C -> Initial Repeated (1==true)
Packit 8dc392
	//  D -> Frame Static (1==true)
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	Legal	=   XYZ | XZY | YZX | YXZ | ZXY | ZYX |
Packit 8dc392
		    XZX | XYX | YXY | YZY | ZYZ | ZXZ |
Packit 8dc392
		    XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr|
Packit 8dc392
		    XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr,
Packit 8dc392
Packit 8dc392
	Min	= 0x0000,
Packit 8dc392
	Max	= 0x2111,
Packit 8dc392
	Default	= XYZ
Packit 8dc392
    };
Packit 8dc392
Packit 8dc392
    enum Axis { X = 0, Y = 1, Z = 2 };
Packit 8dc392
Packit 8dc392
    enum InputLayout { XYZLayout, IJKLayout };
Packit 8dc392
Packit 8dc392
    //--------------------------------------------------------------------
Packit 8dc392
    //	Constructors -- all default to ZYX non-relative ala softimage
Packit 8dc392
    //			(where there is no argument to specify it)
Packit 8dc392
    //
Packit 8dc392
    // The Euler-from-matrix constructors assume that the matrix does
Packit 8dc392
    // not include shear or non-uniform scaling, but the constructors
Packit 8dc392
    // do not examine the matrix to verify this assumption.  If necessary,
Packit 8dc392
    // you can adjust the matrix by calling the removeScalingAndShear()
Packit 8dc392
    // function, defined in ImathMatrixAlgo.h.
Packit 8dc392
    //--------------------------------------------------------------------
Packit 8dc392
Packit 8dc392
    Euler();
Packit 8dc392
    Euler(const Euler&);
Packit 8dc392
    Euler(Order p);
Packit 8dc392
    Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
Packit 8dc392
    Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
Packit 8dc392
    Euler(const Euler<T> &euler, Order newp);
Packit 8dc392
    Euler(const Matrix33<T> &, Order o = Default);
Packit 8dc392
    Euler(const Matrix44<T> &, Order o = Default);
Packit 8dc392
Packit 8dc392
    //---------------------------------
Packit 8dc392
    //  Algebraic functions/ Operators
Packit 8dc392
    //---------------------------------
Packit 8dc392
Packit 8dc392
    const Euler<T>&	operator=  (const Euler<T>&);
Packit 8dc392
    const Euler<T>&	operator=  (const Vec3<T>&);
Packit 8dc392
Packit 8dc392
    //--------------------------------------------------------
Packit 8dc392
    //	Set the euler value
Packit 8dc392
    //  This does NOT convert the angles, but setXYZVector() 
Packit 8dc392
    //	does reorder the input vector.
Packit 8dc392
    //--------------------------------------------------------
Packit 8dc392
Packit 8dc392
    static bool		legal(Order);
Packit 8dc392
Packit 8dc392
    void		setXYZVector(const Vec3<T> &);
Packit 8dc392
Packit 8dc392
    Order		order() const;
Packit 8dc392
    void		setOrder(Order);
Packit 8dc392
Packit 8dc392
    void		set(Axis initial,
Packit 8dc392
			    bool relative,
Packit 8dc392
			    bool parityEven,
Packit 8dc392
			    bool firstRepeats);
Packit 8dc392
Packit 8dc392
    //------------------------------------------------------------
Packit 8dc392
    //	Conversions, toXYZVector() reorders the angles so that
Packit 8dc392
    //  the X rotation comes first, followed by the Y and Z
Packit 8dc392
    //  in cases like XYX ordering, the repeated angle will be
Packit 8dc392
    //	in the "z" component
Packit 8dc392
    //
Packit 8dc392
    // The Euler-from-matrix extract() functions assume that the
Packit 8dc392
    // matrix does not include shear or non-uniform scaling, but
Packit 8dc392
    // the extract() functions do not examine the matrix to verify
Packit 8dc392
    // this assumption.  If necessary, you can adjust the matrix
Packit 8dc392
    // by calling the removeScalingAndShear() function, defined
Packit 8dc392
    // in ImathMatrixAlgo.h.
Packit 8dc392
    //------------------------------------------------------------
Packit 8dc392
Packit 8dc392
    void		extract(const Matrix33<T>&);
Packit 8dc392
    void		extract(const Matrix44<T>&);
Packit 8dc392
    void		extract(const Quat<T>&);
Packit 8dc392
Packit 8dc392
    Matrix33<T>		toMatrix33() const;
Packit 8dc392
    Matrix44<T>		toMatrix44() const;
Packit 8dc392
    Quat<T>		toQuat() const;
Packit 8dc392
    Vec3<T>		toXYZVector() const;
Packit 8dc392
Packit 8dc392
    //---------------------------------------------------
Packit 8dc392
    //	Use this function to unpack angles from ijk form
Packit 8dc392
    //---------------------------------------------------
Packit 8dc392
Packit 8dc392
    void		angleOrder(int &i, int &j, int &k) const;
Packit 8dc392
Packit 8dc392
    //---------------------------------------------------
Packit 8dc392
    //	Use this function to determine mapping from xyz to ijk
Packit 8dc392
    // - reshuffles the xyz to match the order
Packit 8dc392
    //---------------------------------------------------
Packit 8dc392
    
Packit 8dc392
    void		angleMapping(int &i, int &j, int &k) const;
Packit 8dc392
Packit 8dc392
    //----------------------------------------------------------------------
Packit 8dc392
    //
Packit 8dc392
    //  Utility methods for getting continuous rotations. None of these
Packit 8dc392
    //  methods change the orientation given by its inputs (or at least
Packit 8dc392
    //  that is the intent).
Packit 8dc392
    //
Packit 8dc392
    //    angleMod() converts an angle to its equivalent in [-PI, PI]
Packit 8dc392
    //
Packit 8dc392
    //    simpleXYZRotation() adjusts xyzRot so that its components differ
Packit 8dc392
    //                        from targetXyzRot by no more than +-PI
Packit 8dc392
    //
Packit 8dc392
    //    nearestRotation() adjusts xyzRot so that its components differ
Packit 8dc392
    //                      from targetXyzRot by as little as possible.
Packit 8dc392
    //                      Note that xyz here really means ijk, because
Packit 8dc392
    //                      the order must be provided.
Packit 8dc392
    //
Packit 8dc392
    //    makeNear() adjusts "this" Euler so that its components differ
Packit 8dc392
    //               from target by as little as possible. This method
Packit 8dc392
    //               might not make sense for Eulers with different order
Packit 8dc392
    //               and it probably doesn't work for repeated axis and
Packit 8dc392
    //               relative orderings (TODO).
Packit 8dc392
    //
Packit 8dc392
    //-----------------------------------------------------------------------
Packit 8dc392
Packit 8dc392
    static float	angleMod (T angle);
Packit 8dc392
    static void		simpleXYZRotation (Vec3<T> &xyzRot,
Packit 8dc392
					   const Vec3<T> &targetXyzRot);
Packit 8dc392
    static void		nearestRotation (Vec3<T> &xyzRot,
Packit 8dc392
					 const Vec3<T> &targetXyzRot,
Packit 8dc392
					 Order order = XYZ);
Packit 8dc392
Packit 8dc392
    void		makeNear (const Euler<T> &target);
Packit 8dc392
Packit 8dc392
    bool		frameStatic() const { return _frameStatic; }
Packit 8dc392
    bool		initialRepeated() const { return _initialRepeated; }
Packit 8dc392
    bool		parityEven() const { return _parityEven; }
Packit 8dc392
    Axis		initialAxis() const { return _initialAxis; }
Packit 8dc392
Packit 8dc392
  protected:
Packit 8dc392
Packit 8dc392
    bool		_frameStatic	 : 1;	// relative or static rotations
Packit 8dc392
    bool		_initialRepeated : 1;	// init axis repeated as last
Packit 8dc392
    bool		_parityEven	 : 1;	// "parity of axis permutation"
Packit 8dc392
#if defined _WIN32 || defined _WIN64
Packit 8dc392
    Axis		_initialAxis	 ;	// First axis of rotation
Packit 8dc392
#else
Packit 8dc392
    Axis		_initialAxis	 : 2;	// First axis of rotation
Packit 8dc392
#endif
Packit 8dc392
};
Packit 8dc392
Packit 8dc392
Packit 8dc392
//--------------------
Packit 8dc392
// Convenient typedefs
Packit 8dc392
//--------------------
Packit 8dc392
Packit 8dc392
typedef Euler<float>	Eulerf;
Packit 8dc392
typedef Euler<double>	Eulerd;
Packit 8dc392
Packit 8dc392
Packit 8dc392
//---------------
Packit 8dc392
// Implementation
Packit 8dc392
//---------------
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline void
Packit 8dc392
 Euler<T>::angleOrder(int &i, int &j, int &k) const
Packit 8dc392
{
Packit 8dc392
    i = _initialAxis;
Packit 8dc392
    j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
Packit 8dc392
    k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline void
Packit 8dc392
 Euler<T>::angleMapping(int &i, int &j, int &k) const
Packit 8dc392
{
Packit 8dc392
    int m[3];
Packit 8dc392
Packit 8dc392
    m[_initialAxis] = 0;
Packit 8dc392
    m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
Packit 8dc392
    m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
Packit 8dc392
    i = m[0];
Packit 8dc392
    j = m[1];
Packit 8dc392
    k = m[2];
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline void
Packit 8dc392
Euler<T>::setXYZVector(const Vec3<T> &v)
Packit 8dc392
{
Packit 8dc392
    int i,j,k;
Packit 8dc392
    angleMapping(i,j,k);
Packit 8dc392
    (*this)[i] = v.x;
Packit 8dc392
    (*this)[j] = v.y;
Packit 8dc392
    (*this)[k] = v.z;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline Vec3<T>
Packit 8dc392
Euler<T>::toXYZVector() const
Packit 8dc392
{
Packit 8dc392
    int i,j,k;
Packit 8dc392
    angleMapping(i,j,k);
Packit 8dc392
    return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
Euler<T>::Euler() :
Packit 8dc392
    Vec3<T>(0,0,0),
Packit 8dc392
    _frameStatic(true),
Packit 8dc392
    _initialRepeated(false),
Packit 8dc392
    _parityEven(true),
Packit 8dc392
    _initialAxis(X)
Packit 8dc392
{}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
Euler<T>::Euler(typename Euler<T>::Order p) :
Packit 8dc392
    Vec3<T>(0,0,0),
Packit 8dc392
    _frameStatic(true),
Packit 8dc392
    _initialRepeated(false),
Packit 8dc392
    _parityEven(true),
Packit 8dc392
    _initialAxis(X)
Packit 8dc392
{
Packit 8dc392
    setOrder(p);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline Euler<T>::Euler( const Vec3<T> &v, 
Packit 8dc392
			typename Euler<T>::Order p, 
Packit 8dc392
			typename Euler<T>::InputLayout l ) 
Packit 8dc392
{
Packit 8dc392
    setOrder(p); 
Packit 8dc392
    if ( l == XYZLayout ) setXYZVector(v);
Packit 8dc392
    else { x = v.x; y = v.y; z = v.z; }
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline Euler<T>::Euler(const Euler<T> &euler)
Packit 8dc392
{
Packit 8dc392
    operator=(euler);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline Euler<T>::Euler(const Euler<T> &euler,Order p)
Packit 8dc392
{
Packit 8dc392
    setOrder(p);
Packit 8dc392
    Matrix33<T> M = euler.toMatrix33();
Packit 8dc392
    extract(M);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline Euler<T>::Euler( T xi, T yi, T zi, 
Packit 8dc392
			typename Euler<T>::Order p,
Packit 8dc392
			typename Euler<T>::InputLayout l)
Packit 8dc392
{
Packit 8dc392
    setOrder(p);
Packit 8dc392
    if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
Packit 8dc392
    else { x = xi; y = yi; z = zi; }
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
Packit 8dc392
{
Packit 8dc392
    setOrder(p);
Packit 8dc392
    extract(M);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
Packit 8dc392
{
Packit 8dc392
    setOrder(p);
Packit 8dc392
    extract(M);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline void Euler<T>::extract(const Quat<T> &q)
Packit 8dc392
{
Packit 8dc392
    extract(q.toMatrix33());
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
void Euler<T>::extract(const Matrix33<T> &M)
Packit 8dc392
{
Packit 8dc392
    int i,j,k;
Packit 8dc392
    angleOrder(i,j,k);
Packit 8dc392
Packit 8dc392
    if (_initialRepeated)
Packit 8dc392
    {
Packit 8dc392
	//
Packit 8dc392
	// Extract the first angle, x.
Packit 8dc392
	// 
Packit 8dc392
Packit 8dc392
	x = Math<T>::atan2 (M[j][i], M[k][i]);
Packit 8dc392
Packit 8dc392
	//
Packit 8dc392
	// Remove the x rotation from M, so that the remaining
Packit 8dc392
	// rotation, N, is only around two axes, and gimbal lock
Packit 8dc392
	// cannot occur.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	Vec3<T> r (0, 0, 0);
Packit 8dc392
	r[i] = (_parityEven? -x: x);
Packit 8dc392
Packit 8dc392
	Matrix44<T> N;
Packit 8dc392
	N.rotate (r);
Packit 8dc392
Packit 8dc392
	N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
Packit 8dc392
			     M[1][0], M[1][1], M[1][2], 0,
Packit 8dc392
			     M[2][0], M[2][1], M[2][2], 0,
Packit 8dc392
			     0,       0,       0,       1);
Packit 8dc392
	//
Packit 8dc392
	// Extract the other two angles, y and z, from N.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
Packit 8dc392
	y = Math<T>::atan2 (sy, N[i][i]);
Packit 8dc392
	z = Math<T>::atan2 (N[j][k], N[j][j]);
Packit 8dc392
    }
Packit 8dc392
    else
Packit 8dc392
    {
Packit 8dc392
	//
Packit 8dc392
	// Extract the first angle, x.
Packit 8dc392
	// 
Packit 8dc392
Packit 8dc392
	x = Math<T>::atan2 (M[j][k], M[k][k]);
Packit 8dc392
Packit 8dc392
	//
Packit 8dc392
	// Remove the x rotation from M, so that the remaining
Packit 8dc392
	// rotation, N, is only around two axes, and gimbal lock
Packit 8dc392
	// cannot occur.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	Vec3<T> r (0, 0, 0);
Packit 8dc392
	r[i] = (_parityEven? -x: x);
Packit 8dc392
Packit 8dc392
	Matrix44<T> N;
Packit 8dc392
	N.rotate (r);
Packit 8dc392
Packit 8dc392
	N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
Packit 8dc392
			     M[1][0], M[1][1], M[1][2], 0,
Packit 8dc392
			     M[2][0], M[2][1], M[2][2], 0,
Packit 8dc392
			     0,       0,       0,       1);
Packit 8dc392
	//
Packit 8dc392
	// Extract the other two angles, y and z, from N.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
Packit 8dc392
	y = Math<T>::atan2 (-N[i][k], cy);
Packit 8dc392
	z = Math<T>::atan2 (-N[j][i], N[j][j]);
Packit 8dc392
    }
Packit 8dc392
Packit 8dc392
    if (!_parityEven)
Packit 8dc392
	*this *= -1;
Packit 8dc392
Packit 8dc392
    if (!_frameStatic)
Packit 8dc392
    {
Packit 8dc392
	T t = x;
Packit 8dc392
	x = z;
Packit 8dc392
	z = t;
Packit 8dc392
    }
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
void Euler<T>::extract(const Matrix44<T> &M)
Packit 8dc392
{
Packit 8dc392
    int i,j,k;
Packit 8dc392
    angleOrder(i,j,k);
Packit 8dc392
Packit 8dc392
    if (_initialRepeated)
Packit 8dc392
    {
Packit 8dc392
	//
Packit 8dc392
	// Extract the first angle, x.
Packit 8dc392
	// 
Packit 8dc392
Packit 8dc392
	x = Math<T>::atan2 (M[j][i], M[k][i]);
Packit 8dc392
Packit 8dc392
	//
Packit 8dc392
	// Remove the x rotation from M, so that the remaining
Packit 8dc392
	// rotation, N, is only around two axes, and gimbal lock
Packit 8dc392
	// cannot occur.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	Vec3<T> r (0, 0, 0);
Packit 8dc392
	r[i] = (_parityEven? -x: x);
Packit 8dc392
Packit 8dc392
	Matrix44<T> N;
Packit 8dc392
	N.rotate (r);
Packit 8dc392
	N = N * M;
Packit 8dc392
Packit 8dc392
	//
Packit 8dc392
	// Extract the other two angles, y and z, from N.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
Packit 8dc392
	y = Math<T>::atan2 (sy, N[i][i]);
Packit 8dc392
	z = Math<T>::atan2 (N[j][k], N[j][j]);
Packit 8dc392
    }
Packit 8dc392
    else
Packit 8dc392
    {
Packit 8dc392
	//
Packit 8dc392
	// Extract the first angle, x.
Packit 8dc392
	// 
Packit 8dc392
Packit 8dc392
	x = Math<T>::atan2 (M[j][k], M[k][k]);
Packit 8dc392
Packit 8dc392
	//
Packit 8dc392
	// Remove the x rotation from M, so that the remaining
Packit 8dc392
	// rotation, N, is only around two axes, and gimbal lock
Packit 8dc392
	// cannot occur.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	Vec3<T> r (0, 0, 0);
Packit 8dc392
	r[i] = (_parityEven? -x: x);
Packit 8dc392
Packit 8dc392
	Matrix44<T> N;
Packit 8dc392
	N.rotate (r);
Packit 8dc392
	N = N * M;
Packit 8dc392
Packit 8dc392
	//
Packit 8dc392
	// Extract the other two angles, y and z, from N.
Packit 8dc392
	//
Packit 8dc392
Packit 8dc392
	T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
Packit 8dc392
	y = Math<T>::atan2 (-N[i][k], cy);
Packit 8dc392
	z = Math<T>::atan2 (-N[j][i], N[j][j]);
Packit 8dc392
    }
Packit 8dc392
Packit 8dc392
    if (!_parityEven)
Packit 8dc392
	*this *= -1;
Packit 8dc392
Packit 8dc392
    if (!_frameStatic)
Packit 8dc392
    {
Packit 8dc392
	T t = x;
Packit 8dc392
	x = z;
Packit 8dc392
	z = t;
Packit 8dc392
    }
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
Matrix33<T> Euler<T>::toMatrix33() const
Packit 8dc392
{
Packit 8dc392
    int i,j,k;
Packit 8dc392
    angleOrder(i,j,k);
Packit 8dc392
Packit 8dc392
    Vec3<T> angles;
Packit 8dc392
Packit 8dc392
    if ( _frameStatic ) angles = (*this);
Packit 8dc392
    else angles = Vec3<T>(z,y,x);
Packit 8dc392
Packit 8dc392
    if ( !_parityEven ) angles *= -1.0;
Packit 8dc392
Packit 8dc392
    T ci = Math<T>::cos(angles.x);
Packit 8dc392
    T cj = Math<T>::cos(angles.y);
Packit 8dc392
    T ch = Math<T>::cos(angles.z);
Packit 8dc392
    T si = Math<T>::sin(angles.x);
Packit 8dc392
    T sj = Math<T>::sin(angles.y);
Packit 8dc392
    T sh = Math<T>::sin(angles.z);
Packit 8dc392
Packit 8dc392
    T cc = ci*ch;
Packit 8dc392
    T cs = ci*sh;
Packit 8dc392
    T sc = si*ch;
Packit 8dc392
    T ss = si*sh;
Packit 8dc392
Packit 8dc392
    Matrix33<T> M;
Packit 8dc392
Packit 8dc392
    if ( _initialRepeated )
Packit 8dc392
    {
Packit 8dc392
	M[i][i] = cj;	  M[j][i] =  sj*si;    M[k][i] =  sj*ci;
Packit 8dc392
	M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
Packit 8dc392
	M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
Packit 8dc392
    }
Packit 8dc392
    else
Packit 8dc392
    {
Packit 8dc392
	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
Packit 8dc392
	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
Packit 8dc392
	M[i][k] = -sj;	 M[j][k] = cj*si;    M[k][k] = cj*ci;
Packit 8dc392
    }
Packit 8dc392
Packit 8dc392
    return M;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
Matrix44<T> Euler<T>::toMatrix44() const
Packit 8dc392
{
Packit 8dc392
    int i,j,k;
Packit 8dc392
    angleOrder(i,j,k);
Packit 8dc392
Packit 8dc392
    Vec3<T> angles;
Packit 8dc392
Packit 8dc392
    if ( _frameStatic ) angles = (*this);
Packit 8dc392
    else angles = Vec3<T>(z,y,x);
Packit 8dc392
Packit 8dc392
    if ( !_parityEven ) angles *= -1.0;
Packit 8dc392
Packit 8dc392
    T ci = Math<T>::cos(angles.x);
Packit 8dc392
    T cj = Math<T>::cos(angles.y);
Packit 8dc392
    T ch = Math<T>::cos(angles.z);
Packit 8dc392
    T si = Math<T>::sin(angles.x);
Packit 8dc392
    T sj = Math<T>::sin(angles.y);
Packit 8dc392
    T sh = Math<T>::sin(angles.z);
Packit 8dc392
Packit 8dc392
    T cc = ci*ch;
Packit 8dc392
    T cs = ci*sh;
Packit 8dc392
    T sc = si*ch;
Packit 8dc392
    T ss = si*sh;
Packit 8dc392
Packit 8dc392
    Matrix44<T> M;
Packit 8dc392
Packit 8dc392
    if ( _initialRepeated )
Packit 8dc392
    {
Packit 8dc392
	M[i][i] = cj;	  M[j][i] =  sj*si;    M[k][i] =  sj*ci;
Packit 8dc392
	M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
Packit 8dc392
	M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
Packit 8dc392
    }
Packit 8dc392
    else
Packit 8dc392
    {
Packit 8dc392
	M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
Packit 8dc392
	M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
Packit 8dc392
	M[i][k] = -sj;	 M[j][k] = cj*si;    M[k][k] = cj*ci;
Packit 8dc392
    }
Packit 8dc392
Packit 8dc392
    return M;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
Quat<T> Euler<T>::toQuat() const
Packit 8dc392
{
Packit 8dc392
    Vec3<T> angles;
Packit 8dc392
    int i,j,k;
Packit 8dc392
    angleOrder(i,j,k);
Packit 8dc392
Packit 8dc392
    if ( _frameStatic ) angles = (*this);
Packit 8dc392
    else angles = Vec3<T>(z,y,x);
Packit 8dc392
Packit 8dc392
    if ( !_parityEven ) angles.y = -angles.y;
Packit 8dc392
Packit 8dc392
    T ti = angles.x*0.5;
Packit 8dc392
    T tj = angles.y*0.5;
Packit 8dc392
    T th = angles.z*0.5;
Packit 8dc392
    T ci = Math<T>::cos(ti);
Packit 8dc392
    T cj = Math<T>::cos(tj);
Packit 8dc392
    T ch = Math<T>::cos(th);
Packit 8dc392
    T si = Math<T>::sin(ti);
Packit 8dc392
    T sj = Math<T>::sin(tj);
Packit 8dc392
    T sh = Math<T>::sin(th);
Packit 8dc392
    T cc = ci*ch;
Packit 8dc392
    T cs = ci*sh;
Packit 8dc392
    T sc = si*ch;
Packit 8dc392
    T ss = si*sh;
Packit 8dc392
Packit 8dc392
    T parity = _parityEven ? 1.0 : -1.0;
Packit 8dc392
Packit 8dc392
    Quat<T> q;
Packit 8dc392
    Vec3<T> a;
Packit 8dc392
Packit 8dc392
    if ( _initialRepeated )
Packit 8dc392
    {
Packit 8dc392
	a[i]	= cj*(cs + sc);
Packit 8dc392
	a[j]	= sj*(cc + ss) * parity,
Packit 8dc392
	a[k]	= sj*(cs - sc);
Packit 8dc392
	q.r	= cj*(cc - ss);
Packit 8dc392
    }
Packit 8dc392
    else
Packit 8dc392
    {
Packit 8dc392
	a[i]	= cj*sc - sj*cs,
Packit 8dc392
	a[j]	= (cj*ss + sj*cc) * parity,
Packit 8dc392
	a[k]	= cj*cs - sj*sc;
Packit 8dc392
	q.r	= cj*cc + sj*ss;
Packit 8dc392
    }
Packit 8dc392
Packit 8dc392
    q.v = a;
Packit 8dc392
Packit 8dc392
    return q;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline bool
Packit 8dc392
Euler<T>::legal(typename Euler<T>::Order order)
Packit 8dc392
{
Packit 8dc392
    return (order & ~Legal) ? false : true;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
typename Euler<T>::Order
Packit 8dc392
Euler<T>::order() const
Packit 8dc392
{
Packit 8dc392
    int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));
Packit 8dc392
Packit 8dc392
    if (_parityEven)	  foo |= 0x0100;
Packit 8dc392
    if (_initialRepeated) foo |= 0x0010;
Packit 8dc392
    if (_frameStatic)	  foo++;
Packit 8dc392
Packit 8dc392
    return (Order)foo;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
inline void Euler<T>::setOrder(typename Euler<T>::Order p)
Packit 8dc392
{
Packit 8dc392
    set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X),	// initial axis
Packit 8dc392
	 !(p & 0x1),	    			// static?
Packit 8dc392
	 !!(p & 0x100),				// permutation even?
Packit 8dc392
	 !!(p & 0x10));				// initial repeats?
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
void Euler<T>::set(typename Euler<T>::Axis axis,
Packit 8dc392
		   bool relative,
Packit 8dc392
		   bool parityEven,
Packit 8dc392
		   bool firstRepeats)
Packit 8dc392
{
Packit 8dc392
    _initialAxis	= axis;
Packit 8dc392
    _frameStatic	= !relative;
Packit 8dc392
    _parityEven		= parityEven;
Packit 8dc392
    _initialRepeated	= firstRepeats;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
Packit 8dc392
{
Packit 8dc392
    x = euler.x;
Packit 8dc392
    y = euler.y;
Packit 8dc392
    z = euler.z;
Packit 8dc392
    _initialAxis = euler._initialAxis;
Packit 8dc392
    _frameStatic = euler._frameStatic;
Packit 8dc392
    _parityEven	 = euler._parityEven;
Packit 8dc392
    _initialRepeated = euler._initialRepeated;
Packit 8dc392
    return *this;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
Packit 8dc392
{
Packit 8dc392
    x = v.x;
Packit 8dc392
    y = v.y;
Packit 8dc392
    z = v.z;
Packit 8dc392
    return *this;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template<class T>
Packit 8dc392
std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
Packit 8dc392
{
Packit 8dc392
    char a[3] = { 'X', 'Y', 'Z' };
Packit 8dc392
Packit 8dc392
    const char* r = euler.frameStatic() ? "" : "r";
Packit 8dc392
    int i,j,k;
Packit 8dc392
    euler.angleOrder(i,j,k);
Packit 8dc392
Packit 8dc392
    if ( euler.initialRepeated() ) k = i;
Packit 8dc392
Packit 8dc392
    return o << "("
Packit 8dc392
	     << euler.x << " "
Packit 8dc392
	     << euler.y << " "
Packit 8dc392
	     << euler.z << " "
Packit 8dc392
	     << a[i] << a[j] << a[k] << r << ")";
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template <class T>
Packit 8dc392
float
Packit 8dc392
Euler<T>::angleMod (T angle)
Packit 8dc392
{
Packit 8dc392
    angle = fmod(T (angle), T (2 * M_PI));
Packit 8dc392
Packit 8dc392
    if (angle < -M_PI)	angle += 2 * M_PI;
Packit 8dc392
    if (angle > +M_PI)	angle -= 2 * M_PI;
Packit 8dc392
Packit 8dc392
    return angle;
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template <class T>
Packit 8dc392
void
Packit 8dc392
Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
Packit 8dc392
{
Packit 8dc392
    Vec3<T> d  = xyzRot - targetXyzRot;
Packit 8dc392
    xyzRot[0]  = targetXyzRot[0] + angleMod(d[0]);
Packit 8dc392
    xyzRot[1]  = targetXyzRot[1] + angleMod(d[1]);
Packit 8dc392
    xyzRot[2]  = targetXyzRot[2] + angleMod(d[2]);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template <class T>
Packit 8dc392
void
Packit 8dc392
Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
Packit 8dc392
			   Order order)
Packit 8dc392
{
Packit 8dc392
    int i,j,k;
Packit 8dc392
    Euler<T> e (0,0,0, order);
Packit 8dc392
    e.angleOrder(i,j,k);
Packit 8dc392
Packit 8dc392
    simpleXYZRotation(xyzRot, targetXyzRot);
Packit 8dc392
Packit 8dc392
    Vec3<T> otherXyzRot;
Packit 8dc392
    otherXyzRot[i] = M_PI+xyzRot[i];
Packit 8dc392
    otherXyzRot[j] = M_PI-xyzRot[j];
Packit 8dc392
    otherXyzRot[k] = M_PI+xyzRot[k];
Packit 8dc392
Packit 8dc392
    simpleXYZRotation(otherXyzRot, targetXyzRot);
Packit 8dc392
	    
Packit 8dc392
    Vec3<T> d  = xyzRot - targetXyzRot;
Packit 8dc392
    Vec3<T> od = otherXyzRot - targetXyzRot;
Packit 8dc392
    T dMag     = d.dot(d);
Packit 8dc392
    T odMag    = od.dot(od);
Packit 8dc392
Packit 8dc392
    if (odMag < dMag)
Packit 8dc392
    {
Packit 8dc392
	xyzRot = otherXyzRot;
Packit 8dc392
    }
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
template <class T>
Packit 8dc392
void
Packit 8dc392
Euler<T>::makeNear (const Euler<T> &target)
Packit 8dc392
{
Packit 8dc392
    Vec3<T> xyzRot = toXYZVector();
Packit 8dc392
    Vec3<T> targetXyz;
Packit 8dc392
    if (order() != target.order())
Packit 8dc392
    {
Packit 8dc392
        Euler<T> targetSameOrder = Euler<T>(target, order());
Packit 8dc392
        targetXyz = targetSameOrder.toXYZVector();
Packit 8dc392
    }
Packit 8dc392
    else
Packit 8dc392
    {
Packit 8dc392
        targetXyz = target.toXYZVector();
Packit 8dc392
    }
Packit 8dc392
Packit 8dc392
    nearestRotation(xyzRot, targetXyz, order());
Packit 8dc392
Packit 8dc392
    setXYZVector(xyzRot);
Packit 8dc392
}
Packit 8dc392
Packit 8dc392
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
Packit 8dc392
#pragma warning(default:4244)
Packit 8dc392
#endif
Packit 8dc392
Packit 8dc392
IMATH_INTERNAL_NAMESPACE_HEADER_EXIT
Packit 8dc392
Packit 8dc392
Packit 8dc392
#endif // INCLUDED_IMATHEULER_H