1/* (c) Magnus Auvinen. See licence.txt in the root of the distribution for more information. */
2/* If you are missing that file, acquire a complete release at teeworlds.com. */
3#ifndef BASE_VMATH_H
4#define BASE_VMATH_H
5
6#include "math.h"
7
8#include <algorithm>
9#include <cmath>
10#include <cstdint>
11
12// ------------------------------------
13
14template<Numeric T>
15class vector2_base
16{
17public:
18 union
19 {
20 T x, u;
21 };
22 union
23 {
24 T y, v;
25 };
26
27 constexpr vector2_base() = default;
28 constexpr vector2_base(T nx, T ny) :
29 x(nx), y(ny)
30 {
31 }
32
33 constexpr vector2_base operator-() const { return vector2_base(-x, -y); }
34 constexpr vector2_base operator-(const vector2_base &vec) const { return vector2_base(x - vec.x, y - vec.y); }
35 constexpr vector2_base operator+(const vector2_base &vec) const { return vector2_base(x + vec.x, y + vec.y); }
36 constexpr vector2_base operator*(const T rhs) const { return vector2_base(x * rhs, y * rhs); }
37 constexpr vector2_base operator*(const vector2_base &vec) const { return vector2_base(x * vec.x, y * vec.y); }
38 constexpr vector2_base operator/(const T rhs) const { return vector2_base(x / rhs, y / rhs); }
39 constexpr vector2_base operator/(const vector2_base &vec) const { return vector2_base(x / vec.x, y / vec.y); }
40
41 constexpr vector2_base &operator+=(const vector2_base &vec)
42 {
43 x += vec.x;
44 y += vec.y;
45 return *this;
46 }
47 constexpr vector2_base &operator-=(const vector2_base &vec)
48 {
49 x -= vec.x;
50 y -= vec.y;
51 return *this;
52 }
53 constexpr vector2_base &operator*=(const T rhs)
54 {
55 x *= rhs;
56 y *= rhs;
57 return *this;
58 }
59 constexpr vector2_base &operator*=(const vector2_base &vec)
60 {
61 x *= vec.x;
62 y *= vec.y;
63 return *this;
64 }
65 constexpr vector2_base &operator/=(const T rhs)
66 {
67 x /= rhs;
68 y /= rhs;
69 return *this;
70 }
71 constexpr vector2_base &operator/=(const vector2_base &vec)
72 {
73 x /= vec.x;
74 y /= vec.y;
75 return *this;
76 }
77
78 constexpr bool operator==(const vector2_base &vec) const { return x == vec.x && y == vec.y; } // TODO: do this with an eps instead
79 constexpr bool operator!=(const vector2_base &vec) const { return x != vec.x || y != vec.y; }
80
81 constexpr T &operator[](const int index) { return index ? y : x; }
82 constexpr const T &operator[](const int index) const { return index ? y : x; }
83};
84
85template<Numeric T>
86constexpr vector2_base<T> rotate(const vector2_base<T> &a, float angle)
87{
88 angle = angle * pi / 180.0f;
89 float s = std::sin(x: angle);
90 float c = std::cos(x: angle);
91 return vector2_base<T>(static_cast<T>(c * a.x - s * a.y), static_cast<T>(s * a.x + c * a.y));
92}
93
94template<Numeric T>
95inline T distance(const vector2_base<T> a, const vector2_base<T> &b)
96{
97 return length(a - b);
98}
99
100template<Numeric T>
101inline T distance_squared(const vector2_base<T> a, const vector2_base<T> &b)
102{
103 return length_squared(a - b);
104}
105
106template<Numeric T>
107constexpr T dot(const vector2_base<T> a, const vector2_base<T> &b)
108{
109 return a.x * b.x + a.y * b.y;
110}
111
112template<std::floating_point T>
113inline float length(const vector2_base<T> &a)
114{
115 return std::sqrt(dot(a, a));
116}
117
118template<std::integral T>
119inline float length(const vector2_base<T> &a)
120{
121 return std::sqrt(x: static_cast<float>(dot(a, a)));
122}
123
124constexpr float length_squared(const vector2_base<float> &a)
125{
126 return dot(a, b: a);
127}
128
129constexpr float angle(const vector2_base<float> &a)
130{
131 if(a.x == 0 && a.y == 0)
132 return 0.0f;
133 else if(a.x == 0)
134 return a.y < 0 ? -pi / 2 : pi / 2;
135 float result = std::atan(x: a.y / a.x);
136 if(a.x < 0)
137 result = result + pi;
138 return result;
139}
140
141template<Numeric T>
142constexpr vector2_base<T> normalize_pre_length(const vector2_base<T> &v, T len)
143{
144 if(len == 0)
145 return vector2_base<T>();
146 return vector2_base<T>(v.x / len, v.y / len);
147}
148
149inline vector2_base<float> normalize(const vector2_base<float> &v)
150{
151 float divisor = length(a: v);
152 if(divisor == 0.0f)
153 return vector2_base<float>(0.0f, 0.0f);
154 float l = 1.0f / divisor;
155 return vector2_base<float>(v.x * l, v.y * l);
156}
157
158inline vector2_base<float> direction(float angle)
159{
160 return vector2_base<float>(std::cos(x: angle), std::sin(x: angle));
161}
162
163inline vector2_base<float> random_direction()
164{
165 return direction(angle: random_angle());
166}
167
168typedef vector2_base<float> vec2;
169typedef vector2_base<bool> bvec2;
170typedef vector2_base<int> ivec2;
171
172template<Numeric T>
173constexpr bool closest_point_on_line(vector2_base<T> line_pointA, vector2_base<T> line_pointB, vector2_base<T> target_point, vector2_base<T> &out_pos)
174{
175 vector2_base<T> AB = line_pointB - line_pointA;
176 T SquaredMagnitudeAB = dot(AB, AB);
177 if(SquaredMagnitudeAB > 0)
178 {
179 vector2_base<T> AP = target_point - line_pointA;
180 T APdotAB = dot(AP, AB);
181 T t = APdotAB / SquaredMagnitudeAB;
182 out_pos = line_pointA + AB * std::clamp(t, (T)0, (T)1);
183 return true;
184 }
185 else
186 {
187 return false;
188 }
189}
190
191constexpr int intersect_line_circle(const vec2 LineStart, const vec2 LineEnd, const vec2 CircleCenter, float Radius, vec2 aIntersections[2])
192{
193 vec2 Delta = LineEnd - LineStart;
194 vec2 Offset = LineStart - CircleCenter;
195
196 // A * Time^2 + B * Time + c == 0
197 float A = length_squared(a: Delta);
198 float B = 2.0f * dot(a: Offset, b: Delta);
199 float C = dot(a: Offset, b: Offset) - Radius * Radius;
200
201 float Discriminant = B * B - 4.0f * A * C;
202 if(Discriminant < 0.0f || A == 0.0f)
203 {
204 // no intersection
205 return 0;
206 }
207 else if(Discriminant == 0.0f)
208 {
209 // tangent
210 float Time = -B / (2.0f * A);
211 aIntersections[0] = LineStart + Delta * Time;
212 return 1;
213 }
214 else
215 {
216 Discriminant = std::sqrt(x: Discriminant);
217 float Time1 = (-B - Discriminant) / (2.0f * A);
218 float Time2 = (-B + Discriminant) / (2.0f * A);
219
220 aIntersections[0] = LineStart + Delta * Time1;
221 aIntersections[1] = LineStart + Delta * Time2;
222
223 return 2;
224 }
225}
226
227// ------------------------------------
228template<Numeric T>
229class vector3_base
230{
231public:
232 union
233 {
234 T x, r, h, u;
235 };
236 union
237 {
238 T y, g, s, v;
239 };
240 union
241 {
242 T z, b, l, w;
243 };
244
245 constexpr vector3_base() = default;
246 constexpr vector3_base(T nx, T ny, T nz) :
247 x(nx), y(ny), z(nz)
248 {
249 }
250
251 constexpr vector3_base operator-(const vector3_base &vec) const { return vector3_base(x - vec.x, y - vec.y, z - vec.z); }
252 constexpr vector3_base operator-() const { return vector3_base(-x, -y, -z); }
253 constexpr vector3_base operator+(const vector3_base &vec) const { return vector3_base(x + vec.x, y + vec.y, z + vec.z); }
254 constexpr vector3_base operator*(const T rhs) const { return vector3_base(x * rhs, y * rhs, z * rhs); }
255 constexpr vector3_base operator*(const vector3_base &vec) const { return vector3_base(x * vec.x, y * vec.y, z * vec.z); }
256 constexpr vector3_base operator/(const T rhs) const { return vector3_base(x / rhs, y / rhs, z / rhs); }
257 constexpr vector3_base operator/(const vector3_base &vec) const { return vector3_base(x / vec.x, y / vec.y, z / vec.z); }
258
259 constexpr vector3_base &operator+=(const vector3_base &vec)
260 {
261 x += vec.x;
262 y += vec.y;
263 z += vec.z;
264 return *this;
265 }
266 constexpr vector3_base &operator-=(const vector3_base &vec)
267 {
268 x -= vec.x;
269 y -= vec.y;
270 z -= vec.z;
271 return *this;
272 }
273 constexpr vector3_base &operator*=(const T rhs)
274 {
275 x *= rhs;
276 y *= rhs;
277 z *= rhs;
278 return *this;
279 }
280 constexpr vector3_base &operator*=(const vector3_base &vec)
281 {
282 x *= vec.x;
283 y *= vec.y;
284 z *= vec.z;
285 return *this;
286 }
287 constexpr vector3_base &operator/=(const T rhs)
288 {
289 x /= rhs;
290 y /= rhs;
291 z /= rhs;
292 return *this;
293 }
294 constexpr vector3_base &operator/=(const vector3_base &vec)
295 {
296 x /= vec.x;
297 y /= vec.y;
298 z /= vec.z;
299 return *this;
300 }
301
302 constexpr bool operator==(const vector3_base &vec) const { return x == vec.x && y == vec.y && z == vec.z; } // TODO: do this with an eps instead
303 constexpr bool operator!=(const vector3_base &vec) const { return x != vec.x || y != vec.y || z != vec.z; }
304};
305
306template<Numeric T>
307inline T distance(const vector3_base<T> &a, const vector3_base<T> &b)
308{
309 return length(a - b);
310}
311
312template<Numeric T>
313constexpr T dot(const vector3_base<T> &a, const vector3_base<T> &b)
314{
315 return a.x * b.x + a.y * b.y + a.z * b.z;
316}
317
318template<Numeric T>
319constexpr vector3_base<T> cross(const vector3_base<T> &a, const vector3_base<T> &b)
320{
321 return vector3_base<T>(
322 a.y * b.z - a.z * b.y,
323 a.z * b.x - a.x * b.z,
324 a.x * b.y - a.y * b.x);
325}
326
327//
328inline float length(const vector3_base<float> &a)
329{
330 return std::sqrt(x: dot(a, b: a));
331}
332
333inline vector3_base<float> normalize(const vector3_base<float> &v)
334{
335 float divisor = length(a: v);
336 if(divisor == 0.0f)
337 return vector3_base<float>(0.0f, 0.0f, 0.0f);
338 float l = 1.0f / divisor;
339 return vector3_base<float>(v.x * l, v.y * l, v.z * l);
340}
341
342typedef vector3_base<float> vec3;
343typedef vector3_base<bool> bvec3;
344typedef vector3_base<int> ivec3;
345
346// ------------------------------------
347
348template<Numeric T>
349class vector4_base
350{
351public:
352 union
353 {
354 T x, r, h;
355 };
356 union
357 {
358 T y, g, s;
359 };
360 union
361 {
362 T z, b, l;
363 };
364 union
365 {
366 T w, a;
367 };
368
369 constexpr vector4_base() = default;
370 constexpr vector4_base(T nx, T ny, T nz, T nw) :
371 x(nx), y(ny), z(nz), w(nw)
372 {
373 }
374
375 constexpr vector4_base operator+(const vector4_base &vec) const { return vector4_base(x + vec.x, y + vec.y, z + vec.z, w + vec.w); }
376 constexpr vector4_base operator-(const vector4_base &vec) const { return vector4_base(x - vec.x, y - vec.y, z - vec.z, w - vec.w); }
377 constexpr vector4_base operator-() const { return vector4_base(-x, -y, -z, -w); }
378 constexpr vector4_base operator*(const vector4_base &vec) const { return vector4_base(x * vec.x, y * vec.y, z * vec.z, w * vec.w); }
379 constexpr vector4_base operator*(const T rhs) const { return vector4_base(x * rhs, y * rhs, z * rhs, w * rhs); }
380 constexpr vector4_base operator/(const vector4_base &vec) const { return vector4_base(x / vec.x, y / vec.y, z / vec.z, w / vec.w); }
381 constexpr vector4_base operator/(const T vec) const { return vector4_base(x / vec, y / vec, z / vec, w / vec); }
382
383 constexpr vector4_base &operator+=(const vector4_base &vec)
384 {
385 x += vec.x;
386 y += vec.y;
387 z += vec.z;
388 w += vec.w;
389 return *this;
390 }
391 constexpr vector4_base &operator-=(const vector4_base &vec)
392 {
393 x -= vec.x;
394 y -= vec.y;
395 z -= vec.z;
396 w -= vec.w;
397 return *this;
398 }
399 constexpr vector4_base &operator*=(const T rhs)
400 {
401 x *= rhs;
402 y *= rhs;
403 z *= rhs;
404 w *= rhs;
405 return *this;
406 }
407 constexpr vector4_base &operator*=(const vector4_base &vec)
408 {
409 x *= vec.x;
410 y *= vec.y;
411 z *= vec.z;
412 w *= vec.w;
413 return *this;
414 }
415 constexpr vector4_base &operator/=(const T rhs)
416 {
417 x /= rhs;
418 y /= rhs;
419 z /= rhs;
420 w /= rhs;
421 return *this;
422 }
423 constexpr vector4_base &operator/=(const vector4_base &vec)
424 {
425 x /= vec.x;
426 y /= vec.y;
427 z /= vec.z;
428 w /= vec.w;
429 return *this;
430 }
431
432 constexpr bool operator==(const vector4_base &vec) const { return x == vec.x && y == vec.y && z == vec.z && w == vec.w; } // TODO: do this with an eps instead
433 constexpr bool operator!=(const vector4_base &vec) const { return x != vec.x || y != vec.y || z != vec.z || w != vec.w; }
434};
435
436typedef vector4_base<float> vec4;
437typedef vector4_base<bool> bvec4;
438typedef vector4_base<int> ivec4;
439typedef vector4_base<uint8_t> ubvec4;
440
441#endif
442