| /////////////////////////////////////////////////////////////////////////// |
| // |
| // 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) |
| { |
| 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 _WIN32 || defined _WIN64) && defined _MSC_VER |
| #pragma warning(default:4244) |
| #endif |
| |
| } // namespace Imath |
| |
| |
| #endif |