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