src/toBeSorted/deg_angle_util.cpp


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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156

#include "toBeSorted/deg_angle_util.h"

#include "c/c_math.h"
#include "common.h"
#include "m/m_angle.h"
#include "m/m_vec.h"

#include <cmath>

f32 dDegree::adjust(f32 value) {
    while (value < -180.0f) {
        value += 360.0f;
    }

    while (value > 180.0f) {
        value -= 360.0f;
    }

    return value;
}

f32 dDegree::canonicalize(f32 value) {
    while (value < -180.0f) {
        value += 360.0f;
    }

    while (value >= 180.0f) {
        value -= 360.0f;
    }

    return value;
}

f32 dDegree::rotate180F() const {
    return adjust(value + 180.0f);
}

void dDegree::rotate180() {
    value = adjust(value + 180.0f);
}

f32 dDegree::abs() const {
    f32 v = value;
    if (v < 0.0f) {
        v = -v;
    }
    return v;
}

f32 dDegree::sin() const {
    return std::sin(toRad());
}

f32 dDegree::cos() const {
    return std::cos(toRad());
}

f32 dDegree::tan() const {
    f32 c = cos();
    f32 s = sin();
    if (cM::isZero(c)) {
        return s > 0.0f ? INFINITY : -INFINITY;
    }
    return s / c;
}

f32 dDegree::tan2() const {
    return tan();
}

f32 dDegree::toRad() const {
    return mAng::d2r_c(value);
}

s16 dDegree::toIdx() {
    return mAng::fromDeg(value);
}

dPolar::dPolar(f32 r, f32 u, f32 v) {
    Set(r, u, v);
}

dPolar::dPolar(const mVec3_c &v) {
    setCartesian(v);
}

dPolar *dPolar::canonicalize() {
    // NONMATCHING - redundant stack copies
    if (R < 0.0f) {
        R = -R;

        U.Set(-U.value);
        V.Set(V.rotate180F());
    }

    if (U.value < -90.0f) {
        U.Set(U.value + 180.0f);
        V.Set(V.rotate180F());
    } else if (U.value > 90.0f) {
        U.Set(U - 180.0f);
        V.Set(V.rotate180F());
    }
    return this;
}

void dPolar::Set(f32 r, f32 u, f32 v) {
    R = r;
    U.Set(u);
    V.Set(v);
    canonicalize();
}

void dPolar::setCartesian(const mVec3_c &v) {
    f32 x = v.x;
    f32 y = v.y;
    f32 z = v.z;

    f64 magXZSq = (f64)(z * z) + (f64)(x * x);
    f64 magSq = magXZSq + (f64)(y * y);

    f32 magXZ = (magXZSq > 0.0) ? (f32)sqrt(magXZSq) : 0.f;
    R = (magSq > 0.0) ? (f32)sqrt(magSq) : 0.f;
    U.Set(mAng::r2d_c(M_PI / 2 - cM::atan2f(magXZ, y)));
    V.Set(mAng::r2d_c(cM::atan2f(x, z)));
    canonicalize();
}

mVec3_c dPolar::toCartesian() const {
    f32 cosU = U.cos();
    f32 sinU = U.sin();
    f32 cosV = V.cos();
    f32 sinV = V.sin();

    f32 x = sinV * (R * cosU);
    f32 z = cosV * (R * cosU);
    f32 y = R * sinU;

    return mVec3_c(x, y, z);
}

f32 dPolar::SetR(f32 r) {
    R = r;
    canonicalize();
    return R;
}

f32 dPolar::SetU(f32 u) {
    U.Set(u);
    canonicalize();
    return U.value;
}

f32 dPolar::SetV(f32 v) {
    V.Set(v);
    return V.value;
}