euler/eulerconvention.cpp at master · keithvetter/euler · GitHub

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
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
#include "eulerconvention.h"
#include <QDebug>


EulerConvention::EulerConvention(const QVector3D& roll_axis,
                                 const QVector3D& pitch_axis,
                                 const QVector3D& yaw_axis):
    _roll_axis(roll_axis),
    _pitch_axis(pitch_axis),
    _yaw_axis(yaw_axis)
{
    bool is_convention_proper = _is_convention_proper(roll_axis,
                                                      pitch_axis,
                                                      yaw_axis);
    // TODO: throw exception
    Q_ASSERT(is_convention_proper);
}

QVector3D EulerConvention::roll_axis() const
{
    return _roll_axis;
}

QVector3D EulerConvention::pitch_axis() const
{
    return _pitch_axis;
}

QVector3D EulerConvention::yaw_axis() const
{
    return _yaw_axis;
}


void EulerConvention::set_axes(const QVector3D& roll_axis,
                               const QVector3D& pitch_axis,
                               const QVector3D& yaw_axis)
{
    bool is_convention_proper = _is_convention_proper(roll_axis,
                                                      pitch_axis,
                                                      yaw_axis);
    Q_ASSERT(is_convention_proper);
    // TODO: throw exception
    _roll_axis  = roll_axis;
    _pitch_axis = pitch_axis;
    _yaw_axis   = yaw_axis;
}

QString EulerConvention::to_string(RotationName name)
{
    QString str;

    switch (name) {
    case Roll:
        str = QString("roll");
        break;
    case Pitch:
        str = QString("pitch");
        break;
    case Yaw:
        str = QString("yaw");
        break;
    }

    return str;
}

QVector3D EulerConvention::to_axis(const QString& str) const
{
    QVector3D axis;

    QString strl = str.toLower();

    if ( strl == QString("roll") ) {
        axis = _roll_axis;
    } else if ( strl == QString("pitch") ) {
        axis = _pitch_axis;
    } else if ( strl == QString("yaw") ) {
        axis = _yaw_axis;
    } else {
        Q_ASSERT(0);
        // TODO: throw exception on bad direction string
    }

    return axis;
}

bool EulerConvention::_is_convention_proper(const QVector3D& roll_axis,
                                            const QVector3D& pitch_axis,
                                            const QVector3D& yaw_axis)
{
    bool is_proper = false;

    if ( roll_axis.length() > 0 &&
         pitch_axis.length() > 0 &&
         yaw_axis.length() > 0 ) {

        QVector3D cross = QVector3D::crossProduct(roll_axis,pitch_axis);
        if ( cross == yaw_axis || cross == -yaw_axis ) {
            is_proper = true;
        }
    }

    return is_proper;

}