imatheuler.h

对gif

///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
// 
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// *       Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// *       Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// *       Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission. 
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////



#ifndef INCLUDED_IMATHEULER_H
#define INCLUDED_IMATHEULER_H

//----------------------------------------------------------------------
//
//	template class Euler<T>
//
//      This class represents euler angle orientations. The class
//	inherits from Vec3 to it can be freely cast. The additional
//	information is the euler priorities rep. This class is
//	essentially a rip off of Ken Shoemake's GemsIV code. It has
//	been modified minimally to make it more understandable, but
//	hardly enough to make it easy to grok completely.
//
//	There are 24 possible combonations of Euler angle
//	representations of which 12 are common in CG and you will
//	probably only use 6 of these which in this scheme are the
//	non-relative-non-repeating types. 
//
//	The representations can be partitioned according to two
//	criteria:
//
//	   1) Are the angles measured relative to a set of fixed axis
//	      or relative to each other (the latter being what happens
//	      when rotation matrices are multiplied together and is
//	      almost ubiquitous in the cg community)
//
//	   2) Is one of the rotations repeated (ala XYX rotation)
//
//	When you construct a given representation from scratch you
//	must order the angles according to their priorities. So, the
//	easiest is a softimage or aerospace (yaw/pitch/roll) ordering
//	of ZYX. 
//
//	    float x_rot = 1;
//	    float y_rot = 2;
//	    float z_rot = 3;
//
//	    Eulerf angles(z_rot, y_rot, x_rot, Eulerf::ZYX);
//		-or-
//	    Eulerf angles( V3f(z_rot,y_rot,z_rot), Eulerf::ZYX );
//
//	If instead, the order was YXZ for instance you would have to
//	do this:
//
//	    float x_rot = 1;
//	    float y_rot = 2;
//	    float z_rot = 3;
//
//	    Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
//		-or-
//	    Eulerf angles( V3f(y_rot,x_rot,z_rot), Eulerf::YXZ );
//
//	Notice how the order you put the angles into the three slots
//	should correspond to the enum (YXZ) ordering. The input angle
//	vector is called the "ijk" vector -- not an "xyz" vector. The
//	ijk vector order is the same as the enum. If you treat the
//	Euler<> as a Vec<> (which it inherts from) you will find the
//	angles are ordered in the same way, i.e.:
//
//	    V3f v = angles;
//	    // v.x == y_rot, v.y == x_rot, v.z == z_rot
//
//	If you just want the x, y, and z angles stored in a vector in
//	that order, you can do this:
//
//	    V3f v = angles.toXYZVector()
//	    // v.x == x_rot, v.y == y_rot, v.z == z_rot
//
//	If you want to set the Euler with an XYZVector use the
//	optional layout argument:
//
//	    Eulerf angles(x_rot, y_rot, z_rot, 
//			  Eulerf::YXZ,
//		          Eulerf::XYZLayout);
//
//	This is the same as:
//
//	    Eulerf angles(y_rot, x_rot, z_rot, Eulerf::YXZ);
//	    
//	Note that this won't do anything intelligent if you have a
//	repeated axis in the euler angles (e.g. XYX)
//
//	If you need to use the "relative" versions of these, you will
//	need to use the "r" enums. 
//
//      The units of the rotation angles are assumed to be radians.
//
//----------------------------------------------------------------------


#include "ImathMath.h"
#include "ImathVec.h"
#include "ImathQuat.h"
#include "ImathMatrix.h"
#include "ImathLimits.h"
#include <iostream>

namespace Imath {

#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
// Disable MS VC++ warnings about conversion from double to float
#pragma warning(disable:4244)
#endif

template <class T>
class Euler : public Vec3<T>
{
  public:
 
    using Vec3<T>::x;
    using Vec3<T>::y;
    using Vec3<T>::z;

    enum Order
    {
	//
	//  All 24 possible orderings
	//

	XYZ	= 0x0101,	// "usual" orderings
	XZY	= 0x0001,
	YZX	= 0x1101,
	YXZ	= 0x1001,
	ZXY	= 0x2101,
	ZYX	= 0x2001,
	
	XZX	= 0x0011,	// first axis repeated
	XYX	= 0x0111,
	YXY	= 0x1011,
	YZY	= 0x1111,
	ZYZ	= 0x2011,
	ZXZ	= 0x2111,

	XYZr	= 0x2000,	// relative orderings -- not common
	XZYr	= 0x2100,
	YZXr	= 0x1000,
	YXZr	= 0x1100,
	ZXYr	= 0x0000,
	ZYXr	= 0x0100,
	
	XZXr	= 0x2110,	// relative first axis repeated 
	XYXr	= 0x2010,
	YXYr	= 0x1110,
	YZYr	= 0x1010,
	ZYZr	= 0x0110,
	ZXZr	= 0x0010,
	//          ||||
	//          VVVV
	//  Legend: ABCD
	//  A -> Initial Axis (0==x, 1==y, 2==z)
	//  B -> Parity Even (1==true)
	//  C -> Initial Repeated (1==true)
	//  D -> Frame Static (1==true)
	//

	Legal	=   XYZ | XZY | YZX | YXZ | ZXY | ZYX |
		    XZX | XYX | YXY | YZY | ZYZ | ZXZ |
		    XYZr| XZYr| YZXr| YXZr| ZXYr| ZYXr|
		    XZXr| XYXr| YXYr| YZYr| ZYZr| ZXZr,

	Min	= 0x0000,
	Max	= 0x2111,
	Default	= XYZ
    };

    enum Axis { X = 0, Y = 1, Z = 2 };

    enum InputLayout { XYZLayout, IJKLayout };

    //----------------------------------------------------------------
    //	Constructors -- all default to ZYX non-relative ala softimage
    //			(where there is no argument to specify it)
    //----------------------------------------------------------------

    Euler();
    Euler(const Euler&);
    Euler(Order p);
    Euler(const Vec3<T> &v, Order o = Default, InputLayout l = IJKLayout);
    Euler(T i, T j, T k, Order o = Default, InputLayout l = IJKLayout);
    Euler(const Euler<T> &euler, Order newp);
    Euler(const Matrix33<T> &, Order o = Default);
    Euler(const Matrix44<T> &, Order o = Default);

    //---------------------------------
    //  Algebraic functions/ Operators
    //---------------------------------

    const Euler<T>&	operator=  (const Euler<T>&);
    const Euler<T>&	operator=  (const Vec3<T>&);

    //--------------------------------------------------------
    //	Set the euler value
    //  This does NOT convert the angles, but setXYZVector() 
    //	does reorder the input vector.
    //--------------------------------------------------------

    static bool		legal(Order);

    void		setXYZVector(const Vec3<T> &);

    Order		order() const;
    void		setOrder(Order);

    void		set(Axis initial,
			    bool relative,
			    bool parityEven,
			    bool firstRepeats);

    //---------------------------------------------------------
    //	Conversions, toXYZVector() reorders the angles so that
    //  the X rotation comes first, followed by the Y and Z
    //  in cases like XYX ordering, the repeated angle will be
    //	in the "z" component
    //---------------------------------------------------------

    void		extract(const Matrix33<T>&);
    void		extract(const Matrix44<T>&);
    void		extract(const Quat<T>&);

    Matrix33<T>		toMatrix33() const;
    Matrix44<T>		toMatrix44() const;
    Quat<T>		toQuat() const;
    Vec3<T>		toXYZVector() const;

    //---------------------------------------------------
    //	Use this function to unpack angles from ijk form
    //---------------------------------------------------

    void		angleOrder(int &i, int &j, int &k) const;

    //---------------------------------------------------
    //	Use this function to determine mapping from xyz to ijk
    // - reshuffles the xyz to match the order
    //---------------------------------------------------
    
    void		angleMapping(int &i, int &j, int &k) const;

    //----------------------------------------------------------------------
    //
    //  Utility methods for getting continuous rotations. None of these
    //  methods change the orientation given by its inputs (or at least
    //  that is the intent).
    //
    //    angleMod() converts an angle to its equivalent in [-PI, PI]
    //
    //    simpleXYZRotation() adjusts xyzRot so that its components differ
    //                        from targetXyzRot by no more than +-PI
    //
    //    nearestRotation() adjusts xyzRot so that its components differ
    //                      from targetXyzRot by as little as possible.
    //                      Note that xyz here really means ijk, because
    //                      the order must be provided.
    //
    //    makeNear() adjusts "this" Euler so that its components differ
    //               from target by as little as possible. This method
    //               might not make sense for Eulers with different order
    //               and it probably doesn't work for repeated axis and
    //               relative orderings (TODO).
    //
    //-----------------------------------------------------------------------

    static float	angleMod (T angle);
    static void		simpleXYZRotation (Vec3<T> &xyzRot,
					   const Vec3<T> &targetXyzRot);
    static void		nearestRotation (Vec3<T> &xyzRot,
					 const Vec3<T> &targetXyzRot,
					 Order order = XYZ);

    void		makeNear (const Euler<T> &target);

    bool		frameStatic() const { return _frameStatic; }
    bool		initialRepeated() const { return _initialRepeated; }
    bool		parityEven() const { return _parityEven; }
    Axis		initialAxis() const { return _initialAxis; }

  protected:

    bool		_frameStatic	 : 1;	// relative or static rotations
    bool		_initialRepeated : 1;	// init axis repeated as last
    bool		_parityEven	 : 1;	// "parity of axis permutation"
#if defined _WIN32 || defined _WIN64
    Axis		_initialAxis	 ;	// First axis of rotation
#else
    Axis		_initialAxis	 : 2;	// First axis of rotation
#endif
};


//--------------------
// Convenient typedefs
//--------------------

typedef Euler<float>	Eulerf;
typedef Euler<double>	Eulerd;


//---------------
// Implementation
//---------------

template<class T>
inline void
 Euler<T>::angleOrder(int &i, int &j, int &k) const
{
    i = _initialAxis;
    j = _parityEven ? (i+1)%3 : (i > 0 ? i-1 : 2);
    k = _parityEven ? (i > 0 ? i-1 : 2) : (i+1)%3;
}

template<class T>
inline void
 Euler<T>::angleMapping(int &i, int &j, int &k) const
{
    int m[3];

    m[_initialAxis] = 0;
    m[(_initialAxis+1) % 3] = _parityEven ? 1 : 2;
    m[(_initialAxis+2) % 3] = _parityEven ? 2 : 1;
    i = m[0];
    j = m[1];
    k = m[2];
}

template<class T>
inline void
Euler<T>::setXYZVector(const Vec3<T> &v)
{
    int i,j,k;
    angleMapping(i,j,k);
    (*this)[i] = v.x;
    (*this)[j] = v.y;
    (*this)[k] = v.z;
}

template<class T>
inline Vec3<T>
Euler<T>::toXYZVector() const
{
    int i,j,k;
    angleMapping(i,j,k);
    return Vec3<T>((*this)[i],(*this)[j],(*this)[k]);
}


template<class T>
Euler<T>::Euler() :
    Vec3<T>(0,0,0),
    _frameStatic(true),
    _initialRepeated(false),
    _parityEven(true),
    _initialAxis(X)
{}

template<class T>
Euler<T>::Euler(typename Euler<T>::Order p) :
    Vec3<T>(0,0,0),
    _frameStatic(true),
    _initialRepeated(false),
    _parityEven(true),
    _initialAxis(X)
{
    setOrder(p);
}

template<class T>
inline Euler<T>::Euler( const Vec3<T> &v, 
			typename Euler<T>::Order p, 
			typename Euler<T>::InputLayout l ) 
{
    setOrder(p); 
    if ( l == XYZLayout ) setXYZVector(v);
    else { x = v.x; y = v.y; z = v.z; }
}

template<class T>
inline Euler<T>::Euler(const Euler<T> &euler)
{
    operator=(euler);
}

template<class T>
inline Euler<T>::Euler(const Euler<T> &euler,Order p)
{
    setOrder(p);
    Matrix33<T> M = euler.toMatrix33();
    extract(M);
}

template<class T>
inline Euler<T>::Euler( T xi, T yi, T zi, 
			typename Euler<T>::Order p,
			typename Euler<T>::InputLayout l)
{
    setOrder(p);
    if ( l == XYZLayout ) setXYZVector(Vec3<T>(xi,yi,zi));
    else { x = xi; y = yi; z = zi; }
}

template<class T>
inline Euler<T>::Euler( const Matrix33<T> &M, typename Euler::Order p )
{
    setOrder(p);
    extract(M);
}

template<class T>
inline Euler<T>::Euler( const Matrix44<T> &M, typename Euler::Order p )
{
    setOrder(p);
    extract(M);
}

template<class T>
inline void Euler<T>::extract(const Quat<T> &q)
{