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
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
|
#pragma once
#include "config/types.h"
#include <cmath>
#include <glm/glm.hpp>
#include <glm/gtc/constants.hpp>
#include <numeric>
#include <utility>
struct Arc : public std::pair<float, float> {
using std::pair<float, float>::pair;
Arc(const Position3D & centre3, const Position3D & e0p, const Position3D & e1p);
float
operator[](unsigned int i) const
{
return i ? second : first;
}
};
constexpr const Position3D origin {0, 0, 0};
constexpr const Position3D up {0, 0, 1};
constexpr const Position3D down {0, 0, -1};
constexpr const Position3D north {0, 1, 0};
constexpr const Position3D south {0, -1, 0};
constexpr const Position3D east {1, 0, 0};
constexpr const Position3D west {-1, 0, 0};
constexpr auto half_pi {glm::half_pi<float>()};
constexpr auto quarter_pi {half_pi / 2};
constexpr auto pi {glm::pi<float>()};
constexpr auto two_pi {glm::two_pi<float>()};
glm::mat4 flat_orientation(const Rotation3D & diff);
// C++ wrapper for C's sincosf, but with references, not pointers
inline auto
sincosf(float a, float & s, float & c)
{
return sincosf(a, &s, &c);
}
inline Rotation2D
sincosf(float a)
{
Rotation2D sc;
sincosf(a, sc.x, sc.y);
return sc;
}
glm::mat2 rotate_flat(float);
glm::mat4 rotate_roll(float);
glm::mat4 rotate_yaw(float);
glm::mat4 rotate_pitch(float);
glm::mat4 rotate_yp(Rotation2D);
glm::mat4 rotate_ypr(Rotation3D);
float vector_yaw(const Direction3D & diff);
float vector_pitch(const Direction3D & diff);
float round_frac(const float & v, const float & frac);
template<typename T>
inline constexpr auto
sq(T v)
{
return v * v;
}
template<std::integral T, glm::qualifier Q>
inline constexpr glm::vec<3, T, Q>
crossInt(const glm::vec<3, T, Q> a, const glm::vec<3, T, Q> b)
{
return {
(a.y * b.z) - (a.z * b.y),
(a.z * b.x) - (a.x * b.z),
(a.x * b.y) - (a.y * b.x),
};
}
template<typename R = float, typename Ta, typename Tb>
inline constexpr auto
ratio(Ta a, Tb b)
{
return (static_cast<R>(a) / static_cast<R>(b));
}
template<typename R = float, typename T, glm::qualifier Q>
inline constexpr auto
ratio(glm::vec<2, T, Q> v)
{
return ratio<R>(v.x, v.y);
}
template<glm::length_t L = 3, typename T, glm::qualifier Q>
inline constexpr glm::vec<L, T, Q>
perspective_divide(glm::vec<4, T, Q> v)
{
return v / v.w;
}
constexpr inline Position2D
operator!(const Position3D & v)
{
return {v.x, v.y};
}
constexpr inline Position3D
operator^(const Position2D & v, float z)
{
return {v.x, v.y, z};
}
constexpr inline glm::vec4
operator^(const Position3D & v, float w)
{
return {v.x, v.y, v.z, w};
}
constexpr inline Position3D
operator!(const Position2D & v)
{
return v ^ 0.F;
}
template<glm::length_t L1, glm::length_t L2, typename T, glm::qualifier Q>
inline constexpr glm::vec<L1 + L2, T, Q>
operator||(const glm::vec<L1, T, Q> v1, const glm::vec<L2, T, Q> v2)
{
return {v1, v2};
}
template<glm::length_t L, typename T, glm::qualifier Q>
inline constexpr glm::vec<L + 1, T, Q>
operator||(const glm::vec<L, T, Q> v1, const T v2)
{
return {v1, v2};
}
inline Position3D
operator%(const Position3D & p, const glm::mat4 & mutation)
{
const auto p2 = mutation * (p ^ 1);
return p2 / p2.w;
}
inline Position3D
operator%=(Position3D & p, const glm::mat4 & mutation)
{
return p = p % mutation;
}
constexpr inline float
arc_length(const Arc & arc)
{
return arc.second - arc.first;
}
float normalize(float ang);
std::pair<Position2D, bool> find_arc_centre(Position2D start, float entrys, Position2D end, float entrye);
std::pair<Position2D, bool> find_arc_centre(Position2D start, Position2D ad, Position2D end, Position2D bd);
std::pair<float, float> find_arcs_radius(Position2D start, float entrys, Position2D end, float entrye);
float find_arcs_radius(Position2D start, Position2D ad, Position2D end, Position2D bd);
template<typename T>
auto
midpoint(const std::pair<T, T> & v)
{
return std::midpoint(v.first, v.second);
}
// Conversions
template<typename T>
inline constexpr auto
mph_to_ms(T v)
{
return v / 2.237L;
}
template<typename T>
inline constexpr auto
kph_to_ms(T v)
{
return v / 3.6L;
}
// ... literals are handy for now, probably go away when we load stuff externally
float operator"" _mph(const long double v);
float operator"" _kph(const long double v);
|
