_Euler4.cc

Go to the documentation of this file.

#include <vecmath/_Euler4.h>

#include <iostream>

static const char *eulerDsc[] = {
    // matching order!
    "EulOrdXYZs",
    "EulOrdZYXr",

    "EulOrdXYXs",
    "EulOrdXYXr",

    "EulOrdXZYs",
    "EulOrdYZXr",

    "EulOrdXZXs",
    "EulOrdXZXr",

    "EulOrdYZXs",
    "EulOrdXZYr",

    "EulOrdYZYs",
    "EulOrdYZYr",

    "EulOrdYXZs",
    "EulOrdZXYr",

    "EulOrdYXYs",
    "EulOrdYXYr",

    "EulOrdZXYs",
    "EulOrdYXZr",

    "EulOrdZXZs",
    "EulOrdZXZr",

    "EulOrdZYXs",
    "EulOrdXYZr",

    "EulOrdZYZs",
    "EulOrdZYZr"
};

/* using the power of C */

#define SET_BY_MATRIX(m) do { \
    int i, j, k, h, n, s, f;\
    order = _order;\
    EulGetOrd(order, i, j, k, h, n, s, f);\
    if (s == EulRepYes) {\
        double sy = sqrt(Math::sqr(m(i,j)) + Math::sqr(m(i,k)));\
        if (sy > 16 * FLT_EPSILON) {\
            x = atan2(m(i,j), m(i,k));\
            y = atan2(sy, m(i,i));\
            z = atan2(m(j,i), -m(k,i));\
        } else {\
            x = atan2(-m(j,k), m(j,j));\
            y = atan2(sy, m(i,i));\
            z = 0;\
        }\
    } else {\
        double cy = sqrt(m(i,i)*m(i,i) + m(j,i)*m(j,i));\
        if (cy > 16 * FLT_EPSILON) {\
            x = atan2(m(k,j), m(k,k));\
            y = atan2(-m(k,i), cy);\
            z = atan2(m(j,i), m(i,i));\
        } else {\
            x = atan2(-m(j,k), m(j,j));\
            y = atan2(-m(k,i), cy);\
            z = 0;\
        }\
    }\
    if (n == EulParOdd) {\
        x = -x; y = -y; z = -z;\
    }\
    if (f == EulFrmR)\
        Math::swap(x, z);\
    } while(0)

#define COMMON_CODE \
    int i, j, k, h, n, s, f;\
    EulGetOrd(order,i,j,k,h,n,s,f);\
    double ti = x;\
    double tj = y;\
    double th = z;\
    if (f == EulFrmR)\
        Math::swap(ti, th);\
    if (n == EulParOdd) {\
        ti = -ti; tj = -tj; th = -th;\
    }\
    double ci, cj, ch, si, sj, sh, cc, cs, sc, ss;


#define GET_BY_MATRIX(m) do {\
    COMMON_CODE\
    ci = cos(ti); cj = cos(tj); ch = cos(th);\
    si = sin(ti); sj = sin(tj); sh = sin(th);\
    cc = ci * ch; cs = ci * sh; sc = si * ch; ss = si * sh;\
    if (s==EulRepYes) {\
        m(i,i) =  cj;      m(i,j) =  sj * si;      m(i,k) =  sj * ci;\
        m(j,i) =  sj * sh; m(j,j) = -cj * ss + cc; m(j,k) = -cj * cs - sc;\
        m(k,i) = -sj * ch; m(k,j) =  cj * sc + cs; m(k,k) =  cj * cc - ss;\
    } else {\
        m(i,i) =  cj * ch; m(i,j) = sj *sc - cs;   m(i,k) = sj * cc + ss;\
        m(j,i) =  cj * sh; m(j,j) = sj *ss + cc;   m(j,k) = sj * cs - sc;\
        m(k,i) = -sj;      m(k,j) = cj *si;        m(k,k) = cj * ci;\
    }\
    } while(0)

// typeless
#define CLEAN_MATRIX4(m) do {\
    m(W,X) = m(W,Y) = m(W,Z) = m(X,W) = m(Y,W) = m(Z,W) = 0.0;\
    m(W,W) = 1.0;\
    } while(0)


template <class Type>
void _Euler4<Type>::get(_Matrix4<double>& m) const
{
    GET_BY_MATRIX(m);
    CLEAN_MATRIX4(m);
}

template <class Type>
void _Euler4<Type>::get(_Matrix4<float>& m) const
{
    GET_BY_MATRIX(m);
    CLEAN_MATRIX4(m);
}

template <class Type>
void _Euler4<Type>::get(_Matrix3<double>& m) const
{
    GET_BY_MATRIX(m);
}

template <class Type>
void _Euler4<Type>::get(_Matrix3<float>& m) const
{
    GET_BY_MATRIX(m);
}

template <class Type>
void _Euler4<Type>::get(_Quat4<double>& q) const
{
    COMMON_CODE
    ti *= 0.5; tj *= 0.5; th *= 0.5;
    ci = cos(ti);  cj = cos(tj);  ch = cos(th);
    si = sin(ti);  sj = sin(tj);  sh = sin(th);
    cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;

    double a[3];
    if (s==EulRepYes) {
        a[i] = cj*(cs + sc);    /* Could speed up with */
        a[j] = sj*(cc + ss);    /* trig identities. */
        a[k] = sj*(cs - sc);
        q.w = cj*(cc - ss);
    } else {
        a[i] = cj*sc - sj*cs;
        a[j] = cj*ss + sj*cc;
        a[k] = cj*cs - sj*sc;
        q.w = cj*cc + sj*ss;
    }
    if (n == EulParOdd)
        a[j] = -a[j];

    q.x = a[X];
    q.y = a[Y];
    q.z = a[Z];
}


template <class Type>
void _Euler4<Type>::set(const _Matrix4<double>& m, order_t _order)
{
    SET_BY_MATRIX(m);
}

template <class Type>
void _Euler4<Type>::set(const _Matrix4<float>& m, order_t _order)
{
    SET_BY_MATRIX(m);
}

template <class Type>
void _Euler4<Type>::set(const _Matrix3<double>& m, order_t _order)
{
    SET_BY_MATRIX(m);
}

template <class Type>
void _Euler4<Type>::set(const _Matrix3<float>& m, order_t _order)
{
    SET_BY_MATRIX(m);
}

template <class Type> std::ostream& operator <<(std::ostream& os, const _Euler4<Type>* t)
{
    return os << *t;
}

template <class Type> std::ostream& operator <<(std::ostream& os, const _Euler4<Type>& t)
{
    os << t.EulOrdXYZr << " UUU" << std::endl;
    return os << "( Phi(Roll):" << t.x << ", Theta(Pitch): " << t.y <<
        ", Psi(Yaw): " << t.z << ", " << eulerDsc[t.order] << " )";
}

template class _Euler4<float>;
template class _Euler4<double>;

template std::ostream& operator <<(std::ostream&, const _Euler4<double>&);
template std::ostream& operator <<(std::ostream&, const _Euler4<double>*);
template std::ostream& operator <<(std::ostream&, const _Euler4<float>&);
template std::ostream& operator <<(std::ostream&, const _Euler4<float>*);

---