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 | |
13 | template<typename T> |
14 | class vector2_base |
15 | { |
16 | public: |
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 | |
83 | template<typename T> |
84 | constexpr 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 | |
92 | template<typename T> |
93 | inline T distance(const vector2_base<T> a, const vector2_base<T> &b) |
94 | { |
95 | return length(a - b); |
96 | } |
97 | |
98 | template<typename T> |
99 | constexpr 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 | |
104 | inline float length(const vector2_base<float> &a) |
105 | { |
106 | return std::sqrt(x: dot(a, b: a)); |
107 | } |
108 | |
109 | inline float length(const vector2_base<int> &a) |
110 | { |
111 | return std::sqrt(x: dot(a, b: a)); |
112 | } |
113 | |
114 | inline float length_squared(const vector2_base<float> &a) |
115 | { |
116 | return dot(a, b: a); |
117 | } |
118 | |
119 | constexpr 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 | |
131 | template<typename T> |
132 | constexpr 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 | |
139 | inline 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 | |
148 | inline vector2_base<float> direction(float angle) |
149 | { |
150 | return vector2_base<float>(std::cos(x: angle), std::sin(x: angle)); |
151 | } |
152 | |
153 | inline vector2_base<float> random_direction() |
154 | { |
155 | return direction(angle: random_angle()); |
156 | } |
157 | |
158 | typedef vector2_base<float> vec2; |
159 | typedef vector2_base<bool> bvec2; |
160 | typedef vector2_base<int> ivec2; |
161 | |
162 | template<typename T> |
163 | constexpr 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 | // ------------------------------------ |
180 | template<typename T> |
181 | class vector3_base |
182 | { |
183 | public: |
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 | |
258 | template<typename T> |
259 | inline T distance(const vector3_base<T> &a, const vector3_base<T> &b) |
260 | { |
261 | return length(a - b); |
262 | } |
263 | |
264 | template<typename T> |
265 | constexpr 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 | |
270 | template<typename T> |
271 | constexpr 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 | // |
280 | inline float length(const vector3_base<float> &a) |
281 | { |
282 | return std::sqrt(x: dot(a, b: a)); |
283 | } |
284 | |
285 | inline 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 | |
294 | typedef vector3_base<float> vec3; |
295 | typedef vector3_base<bool> bvec3; |
296 | typedef vector3_base<int> ivec3; |
297 | |
298 | // ------------------------------------ |
299 | |
300 | template<typename T> |
301 | class vector4_base |
302 | { |
303 | public: |
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 | |
388 | typedef vector4_base<float> vec4; |
389 | typedef vector4_base<bool> bvec4; |
390 | typedef vector4_base<int> ivec4; |
391 | typedef vector4_base<uint8_t> ubvec4; |
392 | |
393 | #endif |
394 | |