src/Math/Rotation.hpp


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

#pragma once

#include <cmath>
#include "Functions.hpp"
#include "Vector.hpp"

struct Rotation {
    Rotation() = default;

    Rotation(Vector<3> vector) : vector{ wrap(vector) } {}

    explicit Rotation(Real angles[3]) : Rotation(angles[0], angles[1], angles[2]) {}

    Rotation(Real pitch, Real yaw, Real roll) {
        vector = wrap({pitch, yaw, roll });
    }

    static Rotation zero() {
        return {0, 0, 0};
    }

    Vector<3> radians() const {
        return vector.map([](auto a) { return Math::radians(a); });
    }

    Rotation operator+(Rotation other) const {
        return wrap(vector + other.vector);
    }

    Rotation& operator+=(const Rotation& other) {
        *this = *this + other;
        return *this;
    }

    std::string string() const {
        return vector.string();
    }

    static Vector<3> wrap(Vector<3> v) {
        return v.map([](auto a) { return fmod(a, 360.0f); });
    }

    Real& pitch() { return vector.x(); }
    const Real& pitch() const { return vector.x(); }

    Real& yaw() { return vector.y(); }
    const Real& yaw() const { return vector.y(); }

    Real& roll() { return vector.z(); }
    const Real& roll() const { return vector.z(); }

    Vector<3> vector;
};