summaryrefslogtreecommitdiffhomepage
path: root/compat/euler.cpp
blob: e9789beef53d0b2a6d08c1daa2b978fd2484480a (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
#include "euler.hpp"
#include "math-imports.hpp"
#include <cmath>

// rotation order is XYZ

namespace euler {

euler_t OTR_COMPAT_EXPORT rmat_to_euler(const rmat& R)
{
    double alpha, beta, gamma;

    beta  = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
    alpha = atan2( R(1,0), R(0,0));
    gamma = atan2( R(2,1), R(2,2));

    return { alpha, beta, gamma };
}

rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& e)
{
    const double X = -e(2);
    const double Y = -e(1);
    const double Z = -e(0);

    const double cx = cos(X);
    const double sx = sin(X);
    const double cy = cos(Y);
    const double sy = sin(Y);
    const double cz = cos(Z);
    const double sz = sin(Z);

    return {
        cy*cz,              -cy*sz,             sy,
        cz*sx*sy + cx*sz,   cx*cz - sx*sy*sz,   -cy*sx,
        -cx*cz*sy + sx*sz,  cz*sx + cx*sy*sz,   cx*cy,
    };
}

} // end ns euler