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>
101constexpr T dot(const vector2_base<T> a, const vector2_base<T> &b)
102{
103 return a.x * b.x + a.y * b.y;
104}
105
106template<std::floating_point T>
107inline float length(const vector2_base<T> &a)
108{
109 return std::sqrt(dot(a, a));
110}
111
112template<std::integral T>
113inline float length(const vector2_base<T> &a)
114{
115 return std::sqrt(x: static_cast<float>(dot(a, a)));
116}
117
118constexpr float length_squared(const vector2_base<float> &a)
119{
120 return dot(a, b: a);
121}
122
123constexpr float angle(const vector2_base<float> &a)
124{
125 if(a.x == 0 && a.y == 0)
126 return 0.0f;
127 else if(a.x == 0)
128 return a.y < 0 ? -pi / 2 : pi / 2;
129 float result = std::atan(x: a.y / a.x);
130 if(a.x < 0)
131 result = result + pi;
132 return result;
133}
134
135template<Numeric T>
136constexpr vector2_base<T> normalize_pre_length(const vector2_base<T> &v, T len)
137{
138 if(len == 0)
139 return vector2_base<T>();
140 return vector2_base<T>(v.x / len, v.y / len);
141}
142
143inline vector2_base<float> normalize(const vector2_base<float> &v)
144{
145 float divisor = length(a: v);
146 if(divisor == 0.0f)
147 return vector2_base<float>(0.0f, 0.0f);
148 float l = 1.0f / divisor;
149 return vector2_base<float>(v.x * l, v.y * l);
150}
151
152inline vector2_base<float> direction(float angle)
153{
154 return vector2_base<float>(std::cos(x: angle), std::sin(x: angle));
155}
156
157inline vector2_base<float> random_direction()
158{
159 return direction(angle: random_angle());
160}
161
162typedef vector2_base<float> vec2;
163typedef vector2_base<bool> bvec2;
164typedef vector2_base<int> ivec2;
165
166template<Numeric T>
167constexpr 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)
168{
169 vector2_base<T> AB = line_pointB - line_pointA;
170 T SquaredMagnitudeAB = dot(AB, AB);
171 if(SquaredMagnitudeAB > 0)
172 {
173 vector2_base<T> AP = target_point - line_pointA;
174 T APdotAB = dot(AP, AB);
175 T t = APdotAB / SquaredMagnitudeAB;
176 out_pos = line_pointA + AB * std::clamp(t, (T)0, (T)1);
177 return true;
178 }
179 else
180 {
181 return false;
182 }
183}
184
185constexpr int intersect_line_circle(const vec2 LineStart, const vec2 LineEnd, const vec2 CircleCenter, float Radius, vec2 aIntersections[2])
186{
187 vec2 Delta = LineEnd - LineStart;
188 vec2 Offset = LineStart - CircleCenter;
189
190 // A * Time^2 + B * Time + c == 0
191 float A = length_squared(a: Delta);
192 float B = 2.0f * dot(a: Offset, b: Delta);
193 float C = dot(a: Offset, b: Offset) - Radius * Radius;
194
195 float Discriminant = B * B - 4.0f * A * C;
196 if(Discriminant < 0.0f || A == 0.0f)
197 {
198 // no intersection
199 return 0;
200 }
201 else if(Discriminant == 0.0f)
202 {
203 // tangent
204 float Time = -B / (2.0f * A);
205 aIntersections[0] = LineStart + Delta * Time;
206 return 1;
207 }
208 else
209 {
210 Discriminant = std::sqrt(x: Discriminant);
211 float Time1 = (-B - Discriminant) / (2.0f * A);
212 float Time2 = (-B + Discriminant) / (2.0f * A);
213
214 aIntersections[0] = LineStart + Delta * Time1;
215 aIntersections[1] = LineStart + Delta * Time2;
216
217 return 2;
218 }
219}
220
221// ------------------------------------
222template<Numeric T>
223class vector3_base
224{
225public:
226 union
227 {
228 T x, r, h, u;
229 };
230 union
231 {
232 T y, g, s, v;
233 };
234 union
235 {
236 T z, b, l, w;
237 };
238
239 constexpr vector3_base() = default;
240 constexpr vector3_base(T nx, T ny, T nz) :
241 x(nx), y(ny), z(nz)
242 {
243 }
244
245 constexpr vector3_base operator-(const vector3_base &vec) const { return vector3_base(x - vec.x, y - vec.y, z - vec.z); }
246 constexpr vector3_base operator-() const { return vector3_base(-x, -y, -z); }
247 constexpr vector3_base operator+(const vector3_base &vec) const { return vector3_base(x + vec.x, y + vec.y, z + vec.z); }
248 constexpr vector3_base operator*(const T rhs) const { return vector3_base(x * rhs, y * rhs, z * rhs); }
249 constexpr vector3_base operator*(const vector3_base &vec) const { return vector3_base(x * vec.x, y * vec.y, z * vec.z); }
250 constexpr vector3_base operator/(const T rhs) const { return vector3_base(x / rhs, y / rhs, z / rhs); }
251 constexpr vector3_base operator/(const vector3_base &vec) const { return vector3_base(x / vec.x, y / vec.y, z / vec.z); }
252
253 constexpr vector3_base &operator+=(const vector3_base &vec)
254 {
255 x += vec.x;
256 y += vec.y;
257 z += vec.z;
258 return *this;
259 }
260 constexpr vector3_base &operator-=(const vector3_base &vec)
261 {
262 x -= vec.x;
263 y -= vec.y;
264 z -= vec.z;
265 return *this;
266 }
267 constexpr vector3_base &operator*=(const T rhs)
268 {
269 x *= rhs;
270 y *= rhs;
271 z *= rhs;
272 return *this;
273 }
274 constexpr vector3_base &operator*=(const vector3_base &vec)
275 {
276 x *= vec.x;
277 y *= vec.y;
278 z *= vec.z;
279 return *this;
280 }
281 constexpr vector3_base &operator/=(const T rhs)
282 {
283 x /= rhs;
284 y /= rhs;
285 z /= rhs;
286 return *this;
287 }
288 constexpr vector3_base &operator/=(const vector3_base &vec)
289 {
290 x /= vec.x;
291 y /= vec.y;
292 z /= vec.z;
293 return *this;
294 }
295
296 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
297 constexpr bool operator!=(const vector3_base &vec) const { return x != vec.x || y != vec.y || z != vec.z; }
298};
299
300template<Numeric T>
301inline T distance(const vector3_base<T> &a, const vector3_base<T> &b)
302{
303 return length(a - b);
304}
305
306template<Numeric T>
307constexpr T dot(const vector3_base<T> &a, const vector3_base<T> &b)
308{
309 return a.x * b.x + a.y * b.y + a.z * b.z;
310}
311
312template<Numeric T>
313constexpr vector3_base<T> cross(const vector3_base<T> &a, const vector3_base<T> &b)
314{
315 return vector3_base<T>(
316 a.y * b.z - a.z * b.y,
317 a.z * b.x - a.x * b.z,
318 a.x * b.y - a.y * b.x);
319}
320
321//
322inline float length(const vector3_base<float> &a)
323{
324 return std::sqrt(x: dot(a, b: a));
325}
326
327inline vector3_base<float> normalize(const vector3_base<float> &v)
328{
329 float divisor = length(a: v);
330 if(divisor == 0.0f)
331 return vector3_base<float>(0.0f, 0.0f, 0.0f);
332 float l = 1.0f / divisor;
333 return vector3_base<float>(v.x * l, v.y * l, v.z * l);
334}
335
336typedef vector3_base<float> vec3;
337typedef vector3_base<bool> bvec3;
338typedef vector3_base<int> ivec3;
339
340// ------------------------------------
341
342template<Numeric T>
343class vector4_base
344{
345public:
346 union
347 {
348 T x, r, h;
349 };
350 union
351 {
352 T y, g, s;
353 };
354 union
355 {
356 T z, b, l;
357 };
358 union
359 {
360 T w, a;
361 };
362
363 constexpr vector4_base() = default;
364 constexpr vector4_base(T nx, T ny, T nz, T nw) :
365 x(nx), y(ny), z(nz), w(nw)
366 {
367 }
368
369 constexpr vector4_base operator+(const vector4_base &vec) const { return vector4_base(x + vec.x, y + vec.y, z + vec.z, w + vec.w); }
370 constexpr vector4_base operator-(const vector4_base &vec) const { return vector4_base(x - vec.x, y - vec.y, z - vec.z, w - vec.w); }
371 constexpr vector4_base operator-() const { return vector4_base(-x, -y, -z, -w); }
372 constexpr vector4_base operator*(const vector4_base &vec) const { return vector4_base(x * vec.x, y * vec.y, z * vec.z, w * vec.w); }
373 constexpr vector4_base operator*(const T rhs) const { return vector4_base(x * rhs, y * rhs, z * rhs, w * rhs); }
374 constexpr vector4_base operator/(const vector4_base &vec) const { return vector4_base(x / vec.x, y / vec.y, z / vec.z, w / vec.w); }
375 constexpr vector4_base operator/(const T vec) const { return vector4_base(x / vec, y / vec, z / vec, w / vec); }
376
377 constexpr vector4_base &operator+=(const vector4_base &vec)
378 {
379 x += vec.x;
380 y += vec.y;
381 z += vec.z;
382 w += vec.w;
383 return *this;
384 }
385 constexpr vector4_base &operator-=(const vector4_base &vec)
386 {
387 x -= vec.x;
388 y -= vec.y;
389 z -= vec.z;
390 w -= vec.w;
391 return *this;
392 }
393 constexpr vector4_base &operator*=(const T rhs)
394 {
395 x *= rhs;
396 y *= rhs;
397 z *= rhs;
398 w *= rhs;
399 return *this;
400 }
401 constexpr vector4_base &operator*=(const vector4_base &vec)
402 {
403 x *= vec.x;
404 y *= vec.y;
405 z *= vec.z;
406 w *= vec.w;
407 return *this;
408 }
409 constexpr vector4_base &operator/=(const T rhs)
410 {
411 x /= rhs;
412 y /= rhs;
413 z /= rhs;
414 w /= rhs;
415 return *this;
416 }
417 constexpr vector4_base &operator/=(const vector4_base &vec)
418 {
419 x /= vec.x;
420 y /= vec.y;
421 z /= vec.z;
422 w /= vec.w;
423 return *this;
424 }
425
426 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
427 constexpr bool operator!=(const vector4_base &vec) const { return x != vec.x || y != vec.y || z != vec.z || w != vec.w; }
428};
429
430typedef vector4_base<float> vec4;
431typedef vector4_base<bool> bvec4;
432typedef vector4_base<int> ivec4;
433typedef vector4_base<uint8_t> ubvec4;
434
435#endif
436