ImathEuler.h

//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//
 
//
// Euler angle representation of rotation/orientation
//
 
#ifndef INCLUDED_ATOMSMATHEULER_H
#define INCLUDED_ATOMSMATHEULER_H
 
#include <AtomsMath/ImathMath.h>
#include <AtomsMath/ImathMatrix.h>
#include <AtomsMath/ImathNamespace.h>
#include <AtomsMath/ImathQuat.h>
#include <AtomsMath/ImathVec.h>
 
#include <iostream>
 
ATOMSMATH_INTERNAL_NAMESPACE_HEADER_ENTER
 
#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
    {
        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
        //       ABCD
        // Legend: 
        //  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
    };
 
 
    ATOMSMATH_HOSTDEVICE constexpr Euler() noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 Euler (const Euler&) noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 Euler (Order p) noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 Euler (const Vec3<T>& v,
                                              Order o       = Default,
                                              InputLayout l = IJKLayout) noexcept;
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14
    Euler (T i, T j, T k, Order o = Default, InputLayout l = IJKLayout) noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 Euler (const Euler<T>& euler, Order newp) noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 Euler (const Matrix33<T>&, Order o = Default) noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 Euler (const Matrix44<T>&, Order o = Default) noexcept;
 
    ~Euler() = default;
 
    
    
    ATOMSMATH_HOSTDEVICE constexpr static bool legal (Order) noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 Order order() const noexcept;
 
    ATOMSMATH_HOSTDEVICE constexpr bool frameStatic() const { return _frameStatic; }
 
    ATOMSMATH_HOSTDEVICE constexpr bool initialRepeated() const { return _initialRepeated; }
 
    ATOMSMATH_HOSTDEVICE constexpr bool parityEven() const { return _parityEven; }
 
    ATOMSMATH_HOSTDEVICE constexpr Axis initialAxis() const { return _initialAxis; }
 
    ATOMSMATH_HOSTDEVICE void angleOrder (int& i, int& j, int& k) const noexcept;
 
    ATOMSMATH_HOSTDEVICE void angleMapping (int& i, int& j, int& k) const noexcept;
 
 
    
    ATOMSMATH_HOSTDEVICE void setOrder (Order) noexcept;
 
    ATOMSMATH_HOSTDEVICE void setXYZVector (const Vec3<T>&) noexcept;
 
    ATOMSMATH_HOSTDEVICE void set (Axis initial, bool relative, bool parityEven, bool firstRepeats) noexcept;
 
    
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 const Euler<T>& operator= (const Euler<T>&) noexcept;
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 const Euler<T>& operator= (const Vec3<T>&) noexcept;
 
    ATOMSMATH_HOSTDEVICE void extract (const Matrix33<T>&) noexcept;
 
    ATOMSMATH_HOSTDEVICE void extract (const Matrix44<T>&) noexcept;
 
    ATOMSMATH_HOSTDEVICE void extract (const Quat<T>&) noexcept;
 
    ATOMSMATH_HOSTDEVICE Matrix33<T> toMatrix33() const noexcept;
 
    ATOMSMATH_HOSTDEVICE Matrix44<T> toMatrix44() const noexcept;
 
    ATOMSMATH_HOSTDEVICE Quat<T> toQuat() const noexcept;
 
    ATOMSMATH_HOSTDEVICE Vec3<T> toXYZVector() const noexcept;
 
    
 
    ATOMSMATH_HOSTDEVICE ATOMSMATH_CONSTEXPR14 static float angleMod (T angle) noexcept;
 
    ATOMSMATH_HOSTDEVICE static void simpleXYZRotation (Vec3<T>& xyzRot, const Vec3<T>& targetXyzRot) noexcept;
 
    ATOMSMATH_HOSTDEVICE static void
    nearestRotation (Vec3<T>& xyzRot, const Vec3<T>& targetXyzRot, Order order = XYZ) noexcept;
 
    ATOMSMATH_HOSTDEVICE void makeNear (const Euler<T>& target) noexcept;
 
 
  protected:
 
    bool _frameStatic : 1;     
 
    bool _initialRepeated : 1; 
 
    bool _parityEven : 1;      
 
#if defined _WIN32 || defined _WIN64
    Axis _initialAxis; 
#else
    Axis _initialAxis : 2; 
#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 noexcept
{
    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 noexcept
{
    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) noexcept
{
    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 noexcept
{
    int i, j, k;
    angleMapping (i, j, k);
    return Vec3<T> ((*this)[i], (*this)[j], (*this)[k]);
}
 
template <class T>
constexpr inline Euler<T>::Euler() noexcept
    : Vec3<T> (0, 0, 0),
      _frameStatic (true),
      _initialRepeated (false),
      _parityEven (true),
      _initialAxis (X)
{}
 
template <class T>
ATOMSMATH_CONSTEXPR14 inline Euler<T>::Euler (typename Euler<T>::Order p) noexcept
    : Vec3<T> (0, 0, 0),
      _frameStatic (true),
      _initialRepeated (false),
      _parityEven (true),
      _initialAxis (X)
{
    setOrder (p);
}
 
template <class T>
ATOMSMATH_CONSTEXPR14 inline Euler<T>::Euler (const Vec3<T>& v,
                                          typename Euler<T>::Order p,
                                          typename Euler<T>::InputLayout l) noexcept
{
    setOrder (p);
    if (l == XYZLayout)
        setXYZVector (v);
    else
    {
        x = v.x;
        y = v.y;
        z = v.z;
    }
}
 
template <class T> ATOMSMATH_CONSTEXPR14 inline Euler<T>::Euler (const Euler<T>& euler) noexcept
{
    operator= (euler);
}
 
template <class T> ATOMSMATH_CONSTEXPR14 inline Euler<T>::Euler (const Euler<T>& euler, Order p) noexcept
{
    setOrder (p);
    Matrix33<T> M = euler.toMatrix33();
    extract (M);
}
 
template <class T>
ATOMSMATH_CONSTEXPR14 inline Euler<T>::Euler (T xi,
                                          T yi,
                                          T zi,
                                          typename Euler<T>::Order p,
                                          typename Euler<T>::InputLayout l) noexcept
{
    setOrder (p);
    if (l == XYZLayout)
        setXYZVector (Vec3<T> (xi, yi, zi));
    else
    {
        x = xi;
        y = yi;
        z = zi;
    }
}
 
template <class T>
ATOMSMATH_CONSTEXPR14 inline Euler<T>::Euler (const Matrix33<T>& M, typename Euler::Order p) noexcept
{
    setOrder (p);
    extract (M);
}
 
template <class T>
ATOMSMATH_CONSTEXPR14 inline Euler<T>::Euler (const Matrix44<T>& M, typename Euler::Order p) noexcept
{
    setOrder (p);
    extract (M);
}
 
template <class T>
inline void
Euler<T>::extract (const Quat<T>& q) noexcept
{
    extract (q.toMatrix33());
}
 
template <class T>
void
Euler<T>::extract (const Matrix33<T>& M) noexcept
{
    int i, j, k;
    angleOrder (i, j, k);
 
    if (_initialRepeated)
    {
        //
        // Extract the first angle, x.
        //
 
        x = std::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 = std::sqrt (N[j][i] * N[j][i] + N[k][i] * N[k][i]);
        y    = std::atan2 (sy, N[i][i]);
        z    = std::atan2 (N[j][k], N[j][j]);
    }
    else
    {
        //
        // Extract the first angle, x.
        //
 
        x = std::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 = std::sqrt (N[i][i] * N[i][i] + N[i][j] * N[i][j]);
        y    = std::atan2 (-N[i][k], cy);
        z    = std::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) noexcept
{
    int i, j, k;
    angleOrder (i, j, k);
 
    if (_initialRepeated)
    {
        //
        // Extract the first angle, x.
        //
 
        x = std::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 = std::sqrt (N[j][i] * N[j][i] + N[k][i] * N[k][i]);
        y    = std::atan2 (sy, N[i][i]);
        z    = std::atan2 (N[j][k], N[j][j]);
    }
    else
    {
        //
        // Extract the first angle, x.
        //
 
        x = std::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 = std::sqrt (N[i][i] * N[i][i] + N[i][j] * N[i][j]);
        y    = std::atan2 (-N[i][k], cy);
        z    = std::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 noexcept
{
    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 = std::cos (angles.x);
    T cj = std::cos (angles.y);
    T ch = std::cos (angles.z);
    T si = std::sin (angles.x);
    T sj = std::sin (angles.y);
    T sh = std::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 noexcept
{
    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 = std::cos (angles.x);
    T cj = std::cos (angles.y);
    T ch = std::cos (angles.z);
    T si = std::sin (angles.x);
    T sj = std::sin (angles.y);
    T sh = std::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 noexcept
{
    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 = std::cos (ti);
    T cj = std::cos (tj);
    T ch = std::cos (th);
    T si = std::sin (ti);
    T sj = std::sin (tj);
    T sh = std::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, // NOSONAR - suppress SonarCloud bug report.
            a[k] = sj * (cs - sc);
        q.r      = cj * (cc - ss);
    }
    else
    {
        a[i]     = cj * sc - sj * cs,
        a[j]     = (cj * ss + sj * cc) * parity, // NOSONAR - suppress SonarCloud bug report.
            a[k] = cj * cs - sj * sc;
        q.r      = cj * cc + sj * ss;
    }
 
    q.v = a;
 
    return q;
}
 
template <class T>
constexpr inline bool
Euler<T>::legal (typename Euler<T>::Order order) noexcept
{
    return (order & ~Legal) ? false : true;
}
 
template <class T>
ATOMSMATH_CONSTEXPR14 typename Euler<T>::Order
Euler<T>::order() const noexcept
{
    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) noexcept
{
    set (p & 0x2000 ? Z : (p & 0x1000 ? Y : X), // initial axis
         !(p & 0x1),                            // static?
         !!(p & 0x100),                         // permutation even?
         !!(p & 0x10));                         // initial repeats?
}
 
template <class T>
inline void
Euler<T>::set (typename Euler<T>::Axis axis, bool relative, bool parityEven, bool firstRepeats) noexcept
{
    _initialAxis     = axis;
    _frameStatic     = !relative;
    _parityEven      = parityEven;
    _initialRepeated = firstRepeats;
}
 
template <class T>
ATOMSMATH_CONSTEXPR14 inline const Euler<T>&
Euler<T>::operator= (const Euler<T>& euler) noexcept
{
    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>
ATOMSMATH_CONSTEXPR14 inline const Euler<T>&
Euler<T>::operator= (const Vec3<T>& v) noexcept
{
    x = v.x;
    y = v.y;
    z = v.z;
    return *this;
}
 
template <class T>
std::ostream&
operator<< (std::ostream& o, const Euler<T>& euler) noexcept
{
    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>
ATOMSMATH_CONSTEXPR14 inline float
Euler<T>::angleMod (T angle) noexcept
{
    const T pi = static_cast<T> (M_PI);
    angle      = fmod (T (angle), T (2 * pi));
 
    if (angle < -pi)
        angle += 2 * pi;
    if (angle > +pi)
        angle -= 2 * pi;
 
    return angle;
}
 
template <class T>
inline void
Euler<T>::simpleXYZRotation (Vec3<T>& xyzRot, const Vec3<T>& targetXyzRot) noexcept
{
    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) noexcept
{
    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) noexcept
{
    Vec3<T> xyzRot = toXYZVector();
    Vec3<T> targetXyz;
    if (order() != target.order())
    {
        Euler<T> targetSameOrder = Euler<T> (target, order());
        targetXyz                = targetSameOrder.toXYZVector();
    }
    else
    {
        targetXyz = target.toXYZVector();
    }
 
    nearestRotation (xyzRot, targetXyz, order());
 
    setXYZVector (xyzRot);
}
 
#if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
#    pragma warning(default : 4244)
#endif
 
 
ATOMSMATH_INTERNAL_NAMESPACE_HEADER_EXIT
 
#endif // INCLUDED_ATOMSMATHEULER_H