Euler.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __math_Euler_h__
#define __math_Euler_h__


namespace fe
{

/**************************************************************************//**
    @brief Special vector for concatenated Euler angles

    This represents a multiplication of three rotation matrices,
    about X, Y, and Z, in that order.

    These are raw post-multiplication steps,
    not the pre-multiplication concatenation convention like Matrix::rotate,
    so the conversion to matrix rotates in the reverse order
    (rotate Z, rotate Y, then rotate X).

    In a X-forward Z-up world this maps into a transformation
    of heading CCW, pitch down, and then bank right.

    @ingroup math
*//***************************************************************************/
template<typename T>
class Euler: public Vector<3,T>
{
    public:
                    Euler(void):
                            Vector<3,T>()                                   {}

                    Euler(T x,T y,T z):
                            Vector<3,T>(x,y,z)                              {}

                    Euler(const Vector<3,T> &other):
                            Vector<3,T>(other)                              {}
                    Euler(const Euler<T> &other):
                            Vector<3,T>()               { operator=(other); }
                    Euler(const Quaternion<T>& quaternion)
                    {   operator=(quaternion); }
                    Euler(const Matrix<3,4,T>& matrix):
                            Vector<3,T>()               { operator=(matrix); }

        Euler<T>&   operator=(const Euler<T>& euler);
        Euler<T>&   operator=(const Quaternion<T>& quaternion);
        Euler<T>&   operator=(const Matrix<3,4,T>& matrix);

                    operator Matrix<3,4,T>(void) const;

using               Vector<3,T>::operator[];
using               Vector<3,T>::operator=;
};

template<class T>
inline Euler<T>& Euler<T>::operator=(const Euler<T>& other)
{
    set(*this,other[0],other[1],other[2]);
    return *this;
}

template<class T>
inline Euler<T>& Euler<T>::operator=(const Matrix<3,4,T>& matrix)
{
    const T pi2=T(0.5)*fe::pi-1e-6f;
    (*this)[1]=asin(-matrix(2,0));
    if((*this)[1] < pi2)
    {
        if((*this)[1] > -pi2)
        {
            (*this)[0]=atan2(matrix(2,1),matrix(2,2));
            (*this)[2]=atan2(matrix(1,0),matrix(0,0));
        }
        else
        {
            // singularity (twist is x+z)
            (*this)[0]=T(0);
            (*this)[2]=atan2(-matrix(0,1),-matrix(0,2));
        }
    }
    else
    {
        // singularity (twist is x-z)
        (*this)[0]=T(0);
        (*this)[2]= -atan2(matrix(0,1),matrix(0,2));
    }
    return *this;
}

// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
template<class T>
inline Euler<T>& Euler<T>::operator=(const Quaternion<T>& quat)
{

    // roll (x-axis rotation)
    const double sinr_cosp = 2 * (quat[3] * quat[0] + quat[1] * quat[2]);
    const double cosr_cosp = 1 - 2 * (quat[0] * quat[0] + quat[1] * quat[1]);
    (*this)[0] = atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    const double sinp = 2 * (quat[3] * quat[1] - quat[2] * quat[0]);
    if(fabs(sinp) >= 1)
    {
        // use 90 degrees if out of range
        (*this)[1] = copysign(M_PI / 2, sinp);
    }
    else
    {
        (*this)[1] = asin(sinp);
    }

    // yaw (z-axis rotation)
    const double siny_cosp = 2 * (quat[3] * quat[2] + quat[0] * quat[1]);
    const double cosy_cosp = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]);
    (*this)[2] = atan2(siny_cosp, cosy_cosp);

    return *this;
}

template<class T>
inline Euler<T>::operator Matrix<3,4,T>(void) const
{
    Matrix<3,4,T> matrix;
    fe::setIdentity(matrix);
    rotate(matrix,(*this)[2],e_zAxis);
    rotate(matrix,(*this)[1],e_yAxis);
    rotate(matrix,(*this)[0],e_xAxis);
    return matrix;
}

typedef Euler<F32>      Eulerf;
typedef Euler<F64>      Eulerd;
typedef Euler<Real>     SpatialEuler;

} /* namespace */

#endif /* __math_Euler_h__ */