Logo Search packages:      
Sourcecode: openexr version File versions  Download package

ImathEuler.h

///////////////////////////////////////////////////////////////////////////
//
// 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 PLATFORM_WINDOWS && _MSC_VER
// Disable MS VC++ warnings about conversion from double to float
#pragma warning(disable:4244)
#endif

template <class T>
00143 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"
#ifdef PLATFORM_WINDOWS
    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)
{
    extract(q.toMatrix33());
}

template<class T>
void Euler<T>::extract(const Matrix33<T> &M)
{
    int i,j,k;
    angleOrder(i,j,k);

    if (_initialRepeated)
    {
      //
      // Extract the first angle, x.
      // 

      x = Math<T>::atan2 (M[j][i], M[k][i]);

      //
      // Remove the x rotation from M, so that the remaining
      // rotation, N, is only around two axes, and gimbal lock
      // cannot occur.
      //

      Vec3<T> r (0, 0, 0);
      r[i] = (_parityEven? -x: x);

      Matrix44<T> N;
      N.rotate (r);

      N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
                       M[1][0], M[1][1], M[1][2], 0,
                       M[2][0], M[2][1], M[2][2], 0,
                       0,       0,       0,       1);
      //
      // Extract the other two angles, y and z, from N.
      //

      T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
      y = Math<T>::atan2 (sy, N[i][i]);
      z = Math<T>::atan2 (N[j][k], N[j][j]);
    }
    else
    {
      //
      // Extract the first angle, x.
      // 

      x = Math<T>::atan2 (M[j][k], M[k][k]);

      //
      // Remove the x rotation from M, so that the remaining
      // rotation, N, is only around two axes, and gimbal lock
      // cannot occur.
      //

      Vec3<T> r (0, 0, 0);
      r[i] = (_parityEven? -x: x);

      Matrix44<T> N;
      N.rotate (r);

      N = N * Matrix44<T> (M[0][0], M[0][1], M[0][2], 0,
                       M[1][0], M[1][1], M[1][2], 0,
                       M[2][0], M[2][1], M[2][2], 0,
                       0,       0,       0,       1);
      //
      // Extract the other two angles, y and z, from N.
      //

      T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
      y = Math<T>::atan2 (-N[i][k], cy);
      z = Math<T>::atan2 (-N[j][i], N[j][j]);
    }

    if (!_parityEven)
      *this *= -1;

    if (!_frameStatic)
    {
      T t = x;
      x = z;
      z = t;
    }
}

template<class T>
void Euler<T>::extract(const Matrix44<T> &M)
{
    int i,j,k;
    angleOrder(i,j,k);

    if (_initialRepeated)
    {
      //
      // Extract the first angle, x.
      // 

      x = Math<T>::atan2 (M[j][i], M[k][i]);

      //
      // Remove the x rotation from M, so that the remaining
      // rotation, N, is only around two axes, and gimbal lock
      // cannot occur.
      //

      Vec3<T> r (0, 0, 0);
      r[i] = (_parityEven? -x: x);

      Matrix44<T> N;
      N.rotate (r);
      N = N * M;

      //
      // Extract the other two angles, y and z, from N.
      //

      T sy = Math<T>::sqrt (N[j][i]*N[j][i] + N[k][i]*N[k][i]);
      y = Math<T>::atan2 (sy, N[i][i]);
      z = Math<T>::atan2 (N[j][k], N[j][j]);
    }
    else
    {
      //
      // Extract the first angle, x.
      // 

      x = Math<T>::atan2 (M[j][k], M[k][k]);

      //
      // Remove the x rotation from M, so that the remaining
      // rotation, N, is only around two axes, and gimbal lock
      // cannot occur.
      //

      Vec3<T> r (0, 0, 0);
      r[i] = (_parityEven? -x: x);

      Matrix44<T> N;
      N.rotate (r);
      N = N * M;

      //
      // Extract the other two angles, y and z, from N.
      //

      T cy = Math<T>::sqrt (N[i][i]*N[i][i] + N[i][j]*N[i][j]);
      y = Math<T>::atan2 (-N[i][k], cy);
      z = Math<T>::atan2 (-N[j][i], N[j][j]);
    }

    if (!_parityEven)
      *this *= -1;

    if (!_frameStatic)
    {
      T t = x;
      x = z;
      z = t;
    }
}

template<class T>
Matrix33<T> Euler<T>::toMatrix33() const
{
    int i,j,k;
    angleOrder(i,j,k);

    Vec3<T> angles;

    if ( _frameStatic ) angles = (*this);
    else angles = Vec3<T>(z,y,x);

    if ( !_parityEven ) angles *= -1.0;

    T ci = Math<T>::cos(angles.x);
    T cj = Math<T>::cos(angles.y);
    T ch = Math<T>::cos(angles.z);
    T si = Math<T>::sin(angles.x);
    T sj = Math<T>::sin(angles.y);
    T sh = Math<T>::sin(angles.z);

    T cc = ci*ch;
    T cs = ci*sh;
    T sc = si*ch;
    T ss = si*sh;

    Matrix33<T> M;

    if ( _initialRepeated )
    {
      M[i][i] = cj;       M[j][i] =  sj*si;    M[k][i] =  sj*ci;
      M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
      M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
    }
    else
    {
      M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
      M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
      M[i][k] = -sj;     M[j][k] = cj*si;    M[k][k] = cj*ci;
    }

    return M;
}

template<class T>
Matrix44<T> Euler<T>::toMatrix44() const
{
    int i,j,k;
    angleOrder(i,j,k);

    Vec3<T> angles;

    if ( _frameStatic ) angles = (*this);
    else angles = Vec3<T>(z,y,x);

    if ( !_parityEven ) angles *= -1.0;

    T ci = Math<T>::cos(angles.x);
    T cj = Math<T>::cos(angles.y);
    T ch = Math<T>::cos(angles.z);
    T si = Math<T>::sin(angles.x);
    T sj = Math<T>::sin(angles.y);
    T sh = Math<T>::sin(angles.z);

    T cc = ci*ch;
    T cs = ci*sh;
    T sc = si*ch;
    T ss = si*sh;

    Matrix44<T> M;

    if ( _initialRepeated )
    {
      M[i][i] = cj;       M[j][i] =  sj*si;    M[k][i] =  sj*ci;
      M[i][j] = sj*sh;  M[j][j] = -cj*ss+cc; M[k][j] = -cj*cs-sc;
      M[i][k] = -sj*ch; M[j][k] =  cj*sc+cs; M[k][k] =  cj*cc-ss;
    }
    else
    {
      M[i][i] = cj*ch; M[j][i] = sj*sc-cs; M[k][i] = sj*cc+ss;
      M[i][j] = cj*sh; M[j][j] = sj*ss+cc; M[k][j] = sj*cs-sc;
      M[i][k] = -sj;     M[j][k] = cj*si;    M[k][k] = cj*ci;
    }

    return M;
}

template<class T>
Quat<T> Euler<T>::toQuat() const
{
    Vec3<T> angles;
    int i,j,k;
    angleOrder(i,j,k);

    if ( _frameStatic ) angles = (*this);
    else angles = Vec3<T>(z,y,x);

    if ( !_parityEven ) angles.y = -angles.y;

    T ti = angles.x*0.5;
    T tj = angles.y*0.5;
    T th = angles.z*0.5;
    T ci = Math<T>::cos(ti);
    T cj = Math<T>::cos(tj);
    T ch = Math<T>::cos(th);
    T si = Math<T>::sin(ti);
    T sj = Math<T>::sin(tj);
    T sh = Math<T>::sin(th);
    T cc = ci*ch;
    T cs = ci*sh;
    T sc = si*ch;
    T ss = si*sh;

    T parity = _parityEven ? 1.0 : -1.0;

    Quat<T> q;
    Vec3<T> a;

    if ( _initialRepeated )
    {
      a[i]  = cj*(cs + sc);
      a[j]  = sj*(cc + ss) * parity,
      a[k]  = sj*(cs - sc);
      q.r   = cj*(cc - ss);
    }
    else
    {
      a[i]  = cj*sc - sj*cs,
      a[j]  = (cj*ss + sj*cc) * parity,
      a[k]  = cj*cs - sj*sc;
      q.r   = cj*cc + sj*ss;
    }

    q.v = a;

    return q;
}

template<class T>
inline bool
Euler<T>::legal(typename Euler<T>::Order order)
{
    return (order & ~Legal) ? false : true;
}

template<class T>
typename Euler<T>::Order
Euler<T>::order() const
{
    int foo = (_initialAxis == Z ? 0x2000 : (_initialAxis == Y ? 0x1000 : 0));

    if (_parityEven)      foo |= 0x0100;
    if (_initialRepeated) foo |= 0x0010;
    if (_frameStatic)     foo++;

    return (Order)foo;
}

template<class T>
inline void Euler<T>::setOrder(typename Euler<T>::Order p)
{
    set( p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis
       !(p & 0x1),                        // static?
       !!(p & 0x100),                     // permutation even?
       !!(p & 0x10));                     // initial repeats?
}

template<class T>
void Euler<T>::set(typename Euler<T>::Axis axis,
               bool relative,
               bool parityEven,
               bool firstRepeats)
{
    _initialAxis  = axis;
    _frameStatic  = !relative;
    _parityEven         = parityEven;
    _initialRepeated    = firstRepeats;
}

template<class T>
const Euler<T>& Euler<T>::operator= (const Euler<T> &euler)
{
    x = euler.x;
    y = euler.y;
    z = euler.z;
    _initialAxis = euler._initialAxis;
    _frameStatic = euler._frameStatic;
    _parityEven    = euler._parityEven;
    _initialRepeated = euler._initialRepeated;
    return *this;
}

template<class T>
const Euler<T>& Euler<T>::operator= (const Vec3<T> &v)
{
    x = v.x;
    y = v.y;
    z = v.z;
    return *this;
}

template<class T>
std::ostream& operator << (std::ostream &o, const Euler<T> &euler)
{
    char a[3] = { 'X', 'Y', 'Z' };

    const char* r = euler.frameStatic() ? "" : "r";
    int i,j,k;
    euler.angleOrder(i,j,k);

    if ( euler.initialRepeated() ) k = i;

    return o << "("
           << euler.x << " "
           << euler.y << " "
           << euler.z << " "
           << a[i] << a[j] << a[k] << r << ")";
}

template <class T>
float
Euler<T>::angleMod (T angle)
{
    angle = fmod(T (angle), T (2 * M_PI));

    if (angle < -M_PI)  angle += 2 * M_PI;
    if (angle > +M_PI)  angle -= 2 * M_PI;

    return angle;
}

template <class T>
void
Euler<T>::simpleXYZRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot)
{
    Vec3<T> d  = xyzRot - targetXyzRot;
    xyzRot[0]  = targetXyzRot[0] + angleMod(d[0]);
    xyzRot[1]  = targetXyzRot[1] + angleMod(d[1]);
    xyzRot[2]  = targetXyzRot[2] + angleMod(d[2]);
}

template <class T>
void
Euler<T>::nearestRotation (Vec3<T> &xyzRot, const Vec3<T> &targetXyzRot,
                     Order order)
{
    int i,j,k;
    Euler<T> e (0,0,0, order);
    e.angleOrder(i,j,k);

    simpleXYZRotation(xyzRot, targetXyzRot);

    Vec3<T> otherXyzRot;
    otherXyzRot[i] = M_PI+xyzRot[i];
    otherXyzRot[j] = M_PI-xyzRot[j];
    otherXyzRot[k] = M_PI+xyzRot[k];

    simpleXYZRotation(otherXyzRot, targetXyzRot);
          
    Vec3<T> d  = xyzRot - targetXyzRot;
    Vec3<T> od = otherXyzRot - targetXyzRot;
    T dMag     = d.dot(d);
    T odMag    = od.dot(od);

    if (odMag < dMag)
    {
      xyzRot = otherXyzRot;
    }
}

template <class T>
void
Euler<T>::makeNear (const Euler<T> &target)
{
    Vec3<T> xyzRot    = toXYZVector();
    Euler<T> targetSameOrder = Euler<T>(target, order());
    Vec3<T> targetXyz = targetSameOrder.toXYZVector();

    nearestRotation(xyzRot, targetXyz, order());

    setXYZVector(xyzRot);
}

#if defined PLATFORM_WINDOWS && _MSC_VER
#pragma warning(default:4244)
#endif

} // namespace Imath


#endif

Generated by  Doxygen 1.6.0   Back to index