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
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
|
#pragma once
#include "config/types.h"
#include <cmath>
#include <glm/glm.hpp>
#include <glm/gtc/constants.hpp>
#include <numeric>
#include <stdexcept>
#include <utility>
struct Arc : public std::pair<float, float> {
using std::pair<float, float>::pair;
template<typename T, glm::qualifier Q>
Arc(const glm::vec<3, T, Q> & centre3, const glm::vec<3, T, Q> & e0p, const glm::vec<3, T, Q> & e1p);
auto
operator[](bool i) const
{
return i ? second : first;
}
};
constexpr const RelativePosition3D up {0, 0, 1};
constexpr const RelativePosition3D down {0, 0, -1};
constexpr const RelativePosition3D north {0, 1, 0};
constexpr const RelativePosition3D south {0, -1, 0};
constexpr const RelativePosition3D east {1, 0, 0};
constexpr const RelativePosition3D 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<glm::qualifier Q>
inline constexpr glm::vec<3, int64_t, Q>
crossProduct(const glm::vec<3, int64_t, Q> a, const glm::vec<3, int64_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<std::integral T, glm::qualifier Q>
inline constexpr glm::vec<3, T, Q>
crossProduct(const glm::vec<3, T, Q> a, const glm::vec<3, T, Q> b)
{
return crossProduct<int64_t, Q>(a, b);
}
template<std::floating_point T, glm::qualifier Q>
inline constexpr glm::vec<3, T, Q>
crossProduct(const glm::vec<3, T, Q> a, const glm::vec<3, T, Q> b)
{
return glm::cross(a, b);
}
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;
}
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};
}
template<glm::length_t L, typename T, glm::qualifier Q>
inline constexpr glm::vec<L, T, Q>
operator%(const glm::vec<L, T, Q> & p, const glm::mat<L + 1, L + 1, T, Q> & mutation)
{
const auto p2 = mutation * (p || T(1));
return p2 / p2.w;
}
template<glm::length_t L, typename T, glm::qualifier Q>
inline constexpr glm::vec<L, T, Q>
operator%=(glm::vec<L, T, Q> & p, const glm::mat<L + 1, L + 1, T, Q> & mutation)
{
return p = p % mutation;
}
constexpr inline float
arc_length(const Arc & arc)
{
return arc.second - arc.first;
}
float normalize(float ang);
template<typename T, glm::qualifier Q>
std::pair<glm::vec<2, T, Q>, bool>
find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir)
{
const auto det = endDir.x * startDir.y - endDir.y * startDir.x;
if (det != 0) { // near parallel line will yield noisy results
const glm::vec<2, RelativeDistance, Q> d = end - start;
const auto u = (d.y * endDir.x - d.x * endDir.y) / det;
return {start + glm::vec<2, T, Q>(startDir * u), u < 0};
}
throw std::runtime_error("no intersection");
}
template<typename T, glm::qualifier Q>
std::pair<glm::vec<2, T, Q>, bool>
find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
{
if (start == end) {
return {start, false};
}
return find_arc_centre(start, sincosf(entrys + half_pi), end, sincosf(entrye - half_pi));
}
template<typename T, glm::qualifier Q>
Angle
find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end, Rotation2D bd)
{
using std::sqrt;
// Calculates path across both arcs along the normals... pythagorean theorem... for some known radius r
// (2r)^2 = ((m + (X*r)) - (o + (Z*r)))^2 + ((n + (Y*r)) - (p + (W*r)))^2
// According to symbolabs.com equation tool, that solves for r to give:
// r=(-2 m X+2 X o+2 m Z-2 o Z-2 n Y+2 Y p+2 n W-2 p W-sqrt((2 m X-2 X o-2 m Z+2 o Z+2 n Y-2 Y p-2 n W+2 p W)^(2)-4
// (X^(2)-2 X Z+Z^(2)+Y^(2)-2 Y W+W^(2)-4) (m^(2)-2 m o+o^(2)+n^(2)-2 n p+p^(2))))/(2 (X^(2)-2 X Z+Z^(2)+Y^(2)-2 Y
// W+W^(2)-4))
// Locally simplified to work relative, removing one half of the problem and operating on relative positions.
// These exist cos limitations of online formula rearrangement, and I'm OK with that.
const RelativePosition2D diff {end - start};
const auto &o {diff.x}, &p {diff.y};
const auto &X {ad.x}, &Y {ad.y}, &Z {bd.x}, &W {bd.y};
return (-2 * X * o + 2 * o * Z - 2 * Y * p + 2 * p * W
- sqrt(sq(2 * X * o - 2 * o * Z + 2 * Y * p - 2 * p * W)
- (4 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4) * (sq(o) + sq(p)))))
/ (2 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4));
}
template<typename T, glm::qualifier Q>
std::pair<Angle, Angle>
find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
{
const auto getrad = [&](auto leftOrRight) {
return find_arcs_radius(start, sincosf(entrys + leftOrRight), end, sincosf(entrye + leftOrRight));
};
return {getrad(-half_pi), getrad(half_pi)};
}
template<typename T>
auto
midpoint(const std::pair<T, T> & v)
{
return std::midpoint(v.first, v.second);
}
template<typename T, glm::qualifier Q>
Arc::Arc(const glm::vec<3, T, Q> & centre3, const glm::vec<3, T, Q> & e0p, const glm::vec<3, T, Q> & e1p) :
Arc([&]() -> Arc {
const auto diffa = e0p - centre3;
const auto diffb = e1p - centre3;
const auto anga = vector_yaw(diffa);
const auto angb = [&diffb, &anga]() {
const auto angb = vector_yaw(diffb);
return (angb < anga) ? angb + two_pi : angb;
}();
return {anga, angb};
}())
{
}
// 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);
|
