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