1 /*
2 Copyright (C) 2016 Felipe Ferreira da Silva
3
4 This software is provided 'as-is', without any express or implied warranty. In
5 no event will the authors be held liable for any damages arising from the use of
6 this software.
7
8 Permission is granted to anyone to use this software for any purpose, including
9 commercial applications, and to alter it and redistribute it freely, subject to
10 the following restrictions:
11
12 1. The origin of this software must not be misrepresented; you must not claim
13 that you wrote the original software. If you use this software in a
14 product, an acknowledgment in the product documentation would be
15 appreciated but is not required.
16 2. Altered source versions must be plainly marked as such, and must not be
17 misrepresented as being the original software.
18 3. This notice may not be removed or altered from any source distribution.
19 */
20
21 #include "mathc.h"
22
23 #ifdef __GCWZERO__
24 #undef MMIN
25 #define MMIN fmin
26 #undef MMAX
27 #define MMAX fmax
28 #endif
29
30 /* Utils */
nearly_equal(mfloat_t a,mfloat_t b,mfloat_t epsilon)31 bool nearly_equal(mfloat_t a, mfloat_t b, mfloat_t epsilon)
32 {
33 bool result = false;
34 if (a == b) {
35 result = true;
36 } else {
37 result = MABS(a - b) < epsilon;
38 }
39 return result;
40 }
41
to_radians(mfloat_t degrees)42 mfloat_t to_radians(mfloat_t degrees)
43 {
44 return degrees * MPI / MFLOAT_C(180.0);
45 }
46
to_degrees(mfloat_t radians)47 mfloat_t to_degrees(mfloat_t radians)
48 {
49 return radians * MFLOAT_C(180.0) / MPI;
50 }
51
52 /* Vector 2D */
vec2_is_zero(mfloat_t * a)53 bool vec2_is_zero(mfloat_t *a)
54 {
55 return nearly_equal(a[0], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[1], MFLOAT_C(0.0), MFLT_EPSILON);
56 }
57
vec2_is_near_zero(mfloat_t * a,mfloat_t epsilon)58 bool vec2_is_near_zero(mfloat_t *a, mfloat_t epsilon)
59 {
60 return nearly_equal(a[0], MFLOAT_C(0.0), epsilon) && nearly_equal(a[1], MFLOAT_C(0.0), epsilon);
61 }
62
vec2_is_equal(mfloat_t * a,mfloat_t * b)63 bool vec2_is_equal(mfloat_t *a, mfloat_t *b)
64 {
65 return nearly_equal(a[0], b[0], MFLT_EPSILON) && nearly_equal(a[1], b[1], MFLT_EPSILON);
66 }
67
vec2_is_nearly_equal(mfloat_t * a,mfloat_t * b,mfloat_t epsilon)68 bool vec2_is_nearly_equal(mfloat_t *a, mfloat_t *b, mfloat_t epsilon)
69 {
70 return nearly_equal(a[0], b[0], epsilon) && nearly_equal(a[1], b[1], epsilon);
71 }
72
vec2(mfloat_t * result,mfloat_t x,mfloat_t y)73 mfloat_t *vec2(mfloat_t *result, mfloat_t x, mfloat_t y)
74 {
75 result[0] = x;
76 result[1] = y;
77 return result;
78 }
79
vec2_assign(mfloat_t * result,mfloat_t * a)80 mfloat_t *vec2_assign(mfloat_t *result, mfloat_t *a)
81 {
82 result[0] = a[0];
83 result[1] = a[1];
84 return result;
85 }
86
vec2_assign_vec2i(mfloat_t * result,mint_t * a)87 mfloat_t *vec2_assign_vec2i(mfloat_t *result, mint_t *a)
88 {
89 result[0] = (mfloat_t)a[0];
90 result[1] = (mfloat_t)a[1];
91 return result;
92 }
93
vec2_zero(mfloat_t * result)94 mfloat_t *vec2_zero(mfloat_t *result)
95 {
96 result[0] = MFLOAT_C(0.0);
97 result[1] = MFLOAT_C(0.0);
98 return result;
99 }
100
vec2_one(mfloat_t * result)101 mfloat_t *vec2_one(mfloat_t *result)
102 {
103 result[0] = MFLOAT_C(1.0);
104 result[1] = MFLOAT_C(1.0);
105 return result;
106 }
107
vec2_add(mfloat_t * result,mfloat_t * a,mfloat_t * b)108 mfloat_t *vec2_add(mfloat_t *result, mfloat_t *a, mfloat_t *b)
109 {
110 result[0] = a[0] + b[0];
111 result[1] = a[1] + b[1];
112 return result;
113 }
114
vec2_subtract(mfloat_t * result,mfloat_t * a,mfloat_t * b)115 mfloat_t *vec2_subtract(mfloat_t *result, mfloat_t *a, mfloat_t *b)
116 {
117 result[0] = a[0] - b[0];
118 result[1] = a[1] - b[1];
119 return result;
120 }
121
vec2_scale(mfloat_t * result,mfloat_t * a,mfloat_t scalar)122 mfloat_t *vec2_scale(mfloat_t *result, mfloat_t *a, mfloat_t scalar)
123 {
124 result[0] = a[0] * scalar;
125 result[1] = a[1] * scalar;
126 return result;
127 }
128
vec2_multiply(mfloat_t * result,mfloat_t * a,mfloat_t * b)129 mfloat_t *vec2_multiply(mfloat_t *result, mfloat_t *a, mfloat_t *b)
130 {
131 result[0] = a[0] * b[0];
132 result[1] = a[1] * b[1];
133 return result;
134 }
135
vec2_multiply_mat2(mfloat_t * result,mfloat_t * a,mfloat_t * m)136 mfloat_t *vec2_multiply_mat2(mfloat_t *result, mfloat_t *a, mfloat_t *m)
137 {
138 mfloat_t x = a[0];
139 mfloat_t y = a[1];
140 result[0] = m[0] * x + m[2] * y;
141 result[1] = m[1] * x + m[3] * y;
142 return result;
143 }
144
vec2_divide(mfloat_t * result,mfloat_t * a,mfloat_t * b)145 mfloat_t *vec2_divide(mfloat_t *result, mfloat_t *a, mfloat_t *b)
146 {
147 result[0] = a[0] / b[0];
148 result[1] = a[1] / b[1];
149 return result;
150 }
151
vec2_snap(mfloat_t * result,mfloat_t * a,mfloat_t * b)152 mfloat_t *vec2_snap(mfloat_t *result, mfloat_t *a, mfloat_t *b)
153 {
154 result[0] = MFLOOR(a[0] / b[0]) * b[0];
155 result[1] = MFLOOR(a[1] / b[1]) * b[1];
156 return result;
157 }
158
vec2_negative(mfloat_t * result,mfloat_t * a)159 mfloat_t *vec2_negative(mfloat_t *result, mfloat_t *a)
160 {
161 result[0] = -a[0];
162 result[1] = -a[1];
163 return result;
164 }
165
vec2_inverse(mfloat_t * result,mfloat_t * a)166 mfloat_t *vec2_inverse(mfloat_t *result, mfloat_t *a)
167 {
168 if (!nearly_equal(a[0], MFLOAT_C(0.0), MFLT_EPSILON)) {
169 result[0] = MFLOAT_C(1.0) / a[0];
170 } else {
171 result[0] = MFLOAT_C(0.0);
172 }
173 if (!nearly_equal(a[1], MFLOAT_C(0.0), MFLT_EPSILON)) {
174 result[1] = MFLOAT_C(1.0) / a[1];
175 } else {
176 result[1] = MFLOAT_C(0.0);
177 }
178 return result;
179 }
180
vec2_abs(mfloat_t * result,mfloat_t * a)181 mfloat_t *vec2_abs(mfloat_t *result, mfloat_t *a)
182 {
183 result[0] = MABS(a[0]);
184 result[1] = MABS(a[1]);
185 return result;
186 }
187
vec2_floor(mfloat_t * result,mfloat_t * a)188 mfloat_t *vec2_floor(mfloat_t *result, mfloat_t *a)
189 {
190 result[0] = MFLOOR(a[0]);
191 result[1] = MFLOOR(a[1]);
192 return result;
193 }
194
vec2_ceil(mfloat_t * result,mfloat_t * a)195 mfloat_t *vec2_ceil(mfloat_t *result, mfloat_t *a)
196 {
197 result[0] = MCEIL(a[0]);
198 result[1] = MCEIL(a[1]);
199 return result;
200 }
201
vec2_round(mfloat_t * result,mfloat_t * a)202 mfloat_t *vec2_round(mfloat_t *result, mfloat_t *a)
203 {
204 result[0] = MROUND(a[0]);
205 result[1] = MROUND(a[1]);
206 return result;
207 }
208
vec2_max(mfloat_t * result,mfloat_t * a,mfloat_t * b)209 mfloat_t *vec2_max(mfloat_t *result, mfloat_t *a, mfloat_t *b)
210 {
211 result[0] = MMAX(a[0], b[0]);
212 result[1] = MMAX(a[1], b[1]);
213 return result;
214 }
215
vec2_min(mfloat_t * result,mfloat_t * a,mfloat_t * b)216 mfloat_t *vec2_min(mfloat_t *result, mfloat_t *a, mfloat_t *b)
217 {
218 result[0] = MMIN(a[0], b[0]);
219 result[1] = MMIN(a[1], b[1]);
220 return result;
221 }
222
vec2_clamp(mfloat_t * result,mfloat_t * a,mfloat_t * lower,mfloat_t * higher)223 mfloat_t *vec2_clamp(mfloat_t *result, mfloat_t *a, mfloat_t *lower, mfloat_t *higher)
224 {
225 result[0] = a[0];
226 result[1] = a[1];
227 if (result[0] < lower[0]) {
228 result[0] = lower[0];
229 }
230 if (result[0] > higher[0]) {
231 result[0] = higher[0];
232 }
233 if (result[1] < lower[1]) {
234 result[1] = lower[1];
235 }
236 if (result[1] > higher[1]) {
237 result[1] = higher[1];
238 }
239 return result;
240 }
241
vec2_normalize(mfloat_t * result,mfloat_t * a)242 mfloat_t *vec2_normalize(mfloat_t *result, mfloat_t *a)
243 {
244 mfloat_t length = MSQRT(a[0] * a[0] + a[1] * a[1]);
245 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
246 length = MFLOAT_C(1.0) / length;
247 result[0] = a[0] * length;
248 result[1] = a[1] * length;
249 } else {
250 result[0] = MFLOAT_C(0.0);
251 result[1] = MFLOAT_C(0.0);
252 }
253 return result;
254 }
255
vec2_project(mfloat_t * result,mfloat_t * a,mfloat_t * b)256 mfloat_t *vec2_project(mfloat_t *result, mfloat_t *a, mfloat_t *b)
257 {
258 mfloat_t s = vec2_dot(a, b) / vec2_dot(b, b);
259 result[0] = b[0] * s;
260 result[1] = b[1] * s;
261 return result;
262 }
263
vec2_slide(mfloat_t * result,mfloat_t * a,mfloat_t * b)264 mfloat_t *vec2_slide(mfloat_t *result, mfloat_t *a, mfloat_t *b)
265 {
266 mfloat_t d = vec2_dot(a, b);
267 result[0] = a[0] - b[0] * d;
268 result[1] = a[1] - b[1] * d;
269 return result;
270 }
271
vec2_reflect(mfloat_t * result,mfloat_t * a,mfloat_t * b)272 mfloat_t *vec2_reflect(mfloat_t *result, mfloat_t *a, mfloat_t *b)
273 {
274 mfloat_t d = MFLOAT_C(2.0) * vec2_dot(a, b);
275 result[0] = a[0] - b[0] * d;
276 result[1] = a[1] - b[1] * d;
277 return result;
278 }
279
vec2_tangent(mfloat_t * result,mfloat_t * a)280 mfloat_t *vec2_tangent(mfloat_t *result, mfloat_t *a)
281 {
282 result[0] = a[1];
283 result[1] = -a[0];
284 return result;
285 }
286
vec2_rotate(mfloat_t * result,mfloat_t * a,mfloat_t angle)287 mfloat_t *vec2_rotate(mfloat_t *result, mfloat_t *a, mfloat_t angle)
288 {
289 mfloat_t cs = MCOS(angle);
290 mfloat_t sn = MSIN(angle);
291 mfloat_t x = a[0];
292 mfloat_t y = a[1];
293 result[0] = x * cs - y * sn;
294 result[1] = x * sn + y * cs;
295 return result;
296 }
297
vec2_lerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)298 mfloat_t *vec2_lerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
299 {
300 result[0] = a[0] + (b[0] - a[0]) * p;
301 result[1] = a[1] + (b[1] - a[1]) * p;
302 return result;
303 }
304
vec2_bezier3(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t * c,mfloat_t p)305 mfloat_t *vec2_bezier3(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t *c, mfloat_t p)
306 {
307 mfloat_t tmp_a[VEC2_SIZE];
308 mfloat_t tmp_b[VEC2_SIZE];
309 vec2_lerp(tmp_a, a, b, p);
310 vec2_lerp(tmp_b, b, c, p);
311 vec2_lerp(result, tmp_a, tmp_b, p);
312 return result;
313 }
314
vec2_bezier4(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t * c,mfloat_t * d,mfloat_t p)315 mfloat_t *vec2_bezier4(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t *c, mfloat_t *d, mfloat_t p)
316 {
317 mfloat_t tmp_a[VEC2_SIZE];
318 mfloat_t tmp_b[VEC2_SIZE];
319 mfloat_t tmp_c[VEC2_SIZE];
320 mfloat_t tmp_d[VEC2_SIZE];
321 mfloat_t tmp_e[VEC2_SIZE];
322 vec2_lerp(tmp_a, a, b, p);
323 vec2_lerp(tmp_b, b, c, p);
324 vec2_lerp(tmp_c, c, d, p);
325 vec2_lerp(tmp_d, tmp_a, tmp_b, p);
326 vec2_lerp(tmp_e, tmp_b, tmp_c, p);
327 vec2_lerp(result, tmp_d, tmp_e, p);
328 return result;
329 }
330
vec2_dot(mfloat_t * a,mfloat_t * b)331 mfloat_t vec2_dot(mfloat_t *a, mfloat_t *b)
332 {
333 return a[0] * b[0] + a[1] * b[1];
334 }
335
vec2_angle(mfloat_t * a)336 mfloat_t vec2_angle(mfloat_t *a)
337 {
338 return MATAN2(a[1], a[0]);
339 }
340
vec2_length(mfloat_t * a)341 mfloat_t vec2_length(mfloat_t *a)
342 {
343 return MSQRT(a[0] * a[0] + a[1] * a[1]);
344 }
345
vec2_length_squared(mfloat_t * a)346 mfloat_t vec2_length_squared(mfloat_t *a)
347 {
348 return a[0] * a[0] + a[1] * a[1];
349 }
350
vec2_distance(mfloat_t * a,mfloat_t * b)351 mfloat_t vec2_distance(mfloat_t *a, mfloat_t *b)
352 {
353 return MSQRT((a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]));
354 }
355
vec2_distance_squared(mfloat_t * a,mfloat_t * b)356 mfloat_t vec2_distance_squared(mfloat_t *a, mfloat_t *b)
357 {
358 return (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]);
359 }
360
361 /* Vector 2D Integer */
vec2i_is_zero(mint_t * a)362 bool vec2i_is_zero(mint_t *a)
363 {
364 return a[0] == 0 && a[1] == 0;
365 }
366
vec2i_is_equal(mint_t * a,mint_t * b)367 bool vec2i_is_equal(mint_t *a, mint_t *b)
368 {
369 return a[0] == b[0] && a[1] == b[1];
370 }
371
vec2i(mint_t * result,mint_t x,mint_t y)372 mint_t *vec2i(mint_t *result, mint_t x, mint_t y)
373 {
374 result[0] = x;
375 result[1] = y;
376 return result;
377 }
378
vec2i_assign(mint_t * result,mint_t * a)379 mint_t *vec2i_assign(mint_t *result, mint_t *a)
380 {
381 result[0] = a[0];
382 result[1] = a[1];
383 return result;
384 }
385
vec2i_assign_vec2(mint_t * result,mfloat_t * a)386 mint_t *vec2i_assign_vec2(mint_t *result, mfloat_t *a)
387 {
388 result[0] = (mint_t)a[0];
389 result[1] = (mint_t)a[1];
390 return result;
391 }
392
vec2i_zero(mint_t * result)393 mint_t *vec2i_zero(mint_t *result)
394 {
395 result[0] = 0;
396 result[1] = 0;
397 return result;
398 }
399
vec2i_one(mint_t * result)400 mint_t *vec2i_one(mint_t *result)
401 {
402 result[0] = 1;
403 result[1] = 1;
404 return result;
405 }
406
vec2i_add(mint_t * result,mint_t * a,mint_t * b)407 mint_t *vec2i_add(mint_t *result, mint_t *a, mint_t *b)
408 {
409 result[0] = a[0] + b[0];
410 result[1] = a[1] + b[1];
411 return result;
412 }
413
vec2i_subtract(mint_t * result,mint_t * a,mint_t * b)414 mint_t *vec2i_subtract(mint_t *result, mint_t *a, mint_t *b)
415 {
416 result[0] = a[0] - b[0];
417 result[1] = a[1] - b[1];
418 return result;
419 }
420
vec2i_scale(mint_t * result,mint_t * a,mfloat_t scalar)421 mint_t *vec2i_scale(mint_t *result, mint_t *a, mfloat_t scalar)
422 {
423 result[0] = (mint_t)MROUND(a[0] * scalar);
424 result[1] = (mint_t)MROUND(a[1] * scalar);
425 return result;
426 }
427
vec2i_multiply(mint_t * result,mint_t * a,mint_t * b)428 mint_t *vec2i_multiply(mint_t *result, mint_t *a, mint_t *b)
429 {
430 result[0] = a[0] * b[0];
431 result[1] = a[1] * b[1];
432 return result;
433 }
434
vec2i_multiply_mat2(mint_t * result,mint_t * a,mfloat_t * m)435 mint_t *vec2i_multiply_mat2(mint_t *result, mint_t *a, mfloat_t *m)
436 {
437 mint_t x = a[0];
438 mint_t y = a[1];
439 result[0] = (mint_t)MROUND(m[0] * x + m[2] * y);
440 result[1] = (mint_t)MROUND(m[1] * x + m[3] * y);
441 return result;
442 }
443
vec2i_divide(mint_t * result,mint_t * a,mint_t * b)444 mint_t *vec2i_divide(mint_t *result, mint_t *a, mint_t *b)
445 {
446 result[0] = a[0] / b[0];
447 result[1] = a[1] / b[1];
448 return result;
449 }
450
vec2i_snap(mint_t * result,mint_t * a,mint_t * b)451 mint_t *vec2i_snap(mint_t *result, mint_t *a, mint_t *b)
452 {
453 result[0] = (a[0] / b[0]) * b[0];
454 result[0] = (a[0] / b[0]) * b[0];
455 return result;
456 }
457
vec2i_negative(mint_t * result,mint_t * a)458 mint_t *vec2i_negative(mint_t *result, mint_t *a)
459 {
460 result[0] = -a[0];
461 result[1] = -a[1];
462 return result;
463 }
464
vec2i_inverse(mint_t * result,mint_t * a)465 mint_t *vec2i_inverse(mint_t *result, mint_t *a)
466 {
467 if (a[0] != 0) {
468 result[0] = (mint_t)MROUND(MFLOAT_C(1.0) / a[0]);
469 } else {
470 result[0] = 0;
471 }
472 if (a[1] != 0) {
473 result[1] = (mint_t)MROUND(MFLOAT_C(1.0) / a[1]);
474 } else {
475 result[1] = 0;
476 }
477 return result;
478 }
479
vec2i_abs(mint_t * result,mint_t * a)480 mint_t *vec2i_abs(mint_t *result, mint_t *a)
481 {
482 result[0] = a[0];
483 if (result[0] < 0) {
484 result[0] = -result[0];
485 }
486 result[1] = a[1];
487 if (result[1] < 0) {
488 result[1] = -result[1];
489 }
490 return result;
491 }
492
vec2i_floor(mint_t * result,mfloat_t * a)493 mint_t *vec2i_floor(mint_t *result, mfloat_t *a)
494 {
495 result[0] = (mint_t)MFLOOR(a[0]);
496 result[1] = (mint_t)MFLOOR(a[1]);
497 return result;
498 }
499
vec2i_ceil(mint_t * result,mfloat_t * a)500 mint_t *vec2i_ceil(mint_t *result, mfloat_t *a)
501 {
502 result[0] = (mint_t)MCEIL(a[0]);
503 result[1] = (mint_t)MCEIL(a[1]);
504 return result;
505 }
506
vec2i_round(mint_t * result,mfloat_t * a)507 mint_t *vec2i_round(mint_t *result, mfloat_t *a)
508 {
509 result[0] = (mint_t)MROUND(a[0]);
510 result[1] = (mint_t)MROUND(a[1]);
511 return result;
512 }
513
vec2i_max(mint_t * result,mint_t * a,mint_t * b)514 mint_t *vec2i_max(mint_t *result, mint_t *a, mint_t *b)
515 {
516 result[0] = MMAXI(a[0], b[0]);
517 result[1] = MMAXI(a[1], b[1]);
518 return result;
519 }
520
vec2i_min(mint_t * result,mint_t * a,mint_t * b)521 mint_t *vec2i_min(mint_t *result, mint_t *a, mint_t *b)
522 {
523 result[0] = MMINI(a[0], b[0]);
524 result[1] = MMINI(a[1], b[1]);
525 return result;
526 }
527
vec2i_clamp(mint_t * result,mint_t * a,mint_t * lower,mint_t * higher)528 mint_t *vec2i_clamp(mint_t *result, mint_t *a, mint_t *lower, mint_t *higher)
529 {
530 result[0] = a[0];
531 result[1] = a[1];
532 if (result[0] < lower[0]) {
533 result[0] = lower[0];
534 }
535 if (result[0] > higher[0]) {
536 result[0] = higher[0];
537 }
538 if (result[1] < lower[1]) {
539 result[1] = lower[1];
540 }
541 if (result[1] > higher[1]) {
542 result[1] = higher[1];
543 }
544 return result;
545 }
546
vec2i_normalize(mint_t * result,mint_t * a)547 mint_t *vec2i_normalize(mint_t *result, mint_t *a)
548 {
549 mfloat_t length = MSQRT((mfloat_t)(a[0] * a[0] + a[1] * a[1]));
550 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
551 length = MFLOAT_C(1.0) / length;
552 result[0] = (mint_t)MROUND(a[0] * length);
553 result[1] = (mint_t)MROUND(a[1] * length);
554 } else {
555 result[0] = 0;
556 result[1] = 0;
557 }
558 return result;
559 }
560
vec2i_project(mint_t * result,mint_t * a,mint_t * b)561 mint_t *vec2i_project(mint_t *result, mint_t *a, mint_t *b)
562 {
563 mfloat_t s = (mfloat_t)vec2i_dot(a, b) / vec2i_dot(b, b);
564 result[0] = (mint_t)MROUND(b[0] * s);
565 result[1] = (mint_t)MROUND(b[1] * s);
566 return result;
567 }
568
vec2i_slide(mint_t * result,mint_t * a,mint_t * b)569 mint_t *vec2i_slide(mint_t *result, mint_t *a, mint_t *b)
570 {
571 mfloat_t d = (mfloat_t)vec2i_dot(a, b);
572 result[0] = a[0] - (mint_t)MROUND(b[0] * d);
573 result[1] = a[1] - (mint_t)MROUND(b[1] * d);
574 return result;
575 }
576
vec2i_reflect(mint_t * result,mint_t * a,mint_t * b)577 mint_t *vec2i_reflect(mint_t *result, mint_t *a, mint_t *b)
578 {
579 mfloat_t d = MFLOAT_C(2.0) * vec2i_dot(a, b);
580 result[0] = a[0] - (mint_t)MROUND(b[0] * d);
581 result[1] = a[1] - (mint_t)MROUND(b[1] * d);
582 return result;
583 }
584
vec2i_tangent(mint_t * result,mint_t * a)585 mint_t *vec2i_tangent(mint_t *result, mint_t *a)
586 {
587 result[0] = a[1];
588 result[1] = -a[0];
589 return result;
590 }
591
vec2i_rotate(mint_t * result,mint_t * a,mfloat_t angle)592 mint_t *vec2i_rotate(mint_t *result, mint_t *a, mfloat_t angle)
593 {
594 mfloat_t cs = MCOS(angle);
595 mfloat_t sn = MSIN(angle);
596 mint_t x = a[0];
597 mint_t y = a[1];
598 result[0] = (mint_t)MROUND(x * cs - y * sn);
599 result[1] = (mint_t)MROUND(x * sn + y * cs);
600 return result;
601 }
602
vec2i_lerp(mint_t * result,mint_t * a,mint_t * b,mfloat_t p)603 mint_t *vec2i_lerp(mint_t *result, mint_t *a, mint_t *b, mfloat_t p)
604 {
605 result[0] = a[0] + (mint_t)MROUND((b[0] - a[0]) * p);
606 result[1] = a[1] + (mint_t)MROUND((b[1] - a[1]) * p);
607 return result;
608 }
609
vec2i_bezier3(mint_t * result,mint_t * a,mint_t * b,mint_t * c,mfloat_t p)610 mint_t *vec2i_bezier3(mint_t *result, mint_t *a, mint_t *b, mint_t *c, mfloat_t p)
611 {
612 mint_t tmp_a[VEC2_SIZE];
613 mint_t tmp_b[VEC2_SIZE];
614 vec2i_lerp(tmp_a, a, b, p);
615 vec2i_lerp(tmp_b, b, c, p);
616 vec2i_lerp(result, tmp_a, tmp_b, p);
617 return result;
618 }
619
vec2i_bezier4(mint_t * result,mint_t * a,mint_t * b,mint_t * c,mint_t * d,mfloat_t p)620 mint_t *vec2i_bezier4(mint_t *result, mint_t *a, mint_t *b, mint_t *c, mint_t *d, mfloat_t p)
621 {
622 mint_t tmp_a[VEC2_SIZE];
623 mint_t tmp_b[VEC2_SIZE];
624 mint_t tmp_c[VEC2_SIZE];
625 mint_t tmp_d[VEC2_SIZE];
626 mint_t tmp_e[VEC2_SIZE];
627 vec2i_lerp(tmp_a, a, b, p);
628 vec2i_lerp(tmp_b, b, c, p);
629 vec2i_lerp(tmp_c, c, d, p);
630 vec2i_lerp(tmp_d, tmp_a, tmp_b, p);
631 vec2i_lerp(tmp_e, tmp_b, tmp_c, p);
632 vec2i_lerp(result, tmp_d, tmp_e, p);
633 return result;
634 }
635
vec2i_dot(mint_t * a,mint_t * b)636 mint_t vec2i_dot(mint_t *a, mint_t *b)
637 {
638 return a[0] * b[0] + a[1] * b[1];
639 }
640
vec2i_angle(mint_t * a)641 mfloat_t vec2i_angle(mint_t *a)
642 {
643 return MATAN2((mfloat_t)a[1], (mfloat_t)a[0]);
644 }
645
vec2i_length(mint_t * a)646 mfloat_t vec2i_length(mint_t *a)
647 {
648 return MSQRT((mfloat_t)a[0] * a[0] + a[1] * a[1]);
649 }
650
vec2i_length_squared(mint_t * a)651 mint_t vec2i_length_squared(mint_t *a)
652 {
653 return a[0] * a[0] + a[1] * a[1];
654 }
655
vec2i_distance(mint_t * a,mint_t * b)656 mfloat_t vec2i_distance(mint_t *a, mint_t *b)
657 {
658 return MSQRT((mfloat_t)(a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]));
659 }
660
vec2i_distance_squared(mint_t * a,mint_t * b)661 mint_t vec2i_distance_squared(mint_t *a, mint_t *b)
662 {
663 return (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]);
664 }
665
666 /* Vector 3D */
vec3_is_zero(mfloat_t * a)667 bool vec3_is_zero(mfloat_t *a)
668 {
669 return nearly_equal(a[0], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[1], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[2], MFLOAT_C(0.0), MFLT_EPSILON);
670 }
671
vec3_is_near_zero(mfloat_t * a,mfloat_t epsilon)672 bool vec3_is_near_zero(mfloat_t *a, mfloat_t epsilon)
673 {
674 return nearly_equal(a[0], MFLOAT_C(0.0), epsilon) && nearly_equal(a[1], MFLOAT_C(0.0), epsilon) && nearly_equal(a[2], MFLOAT_C(0.0), epsilon);
675 }
676
vec3_is_equal(mfloat_t * a,mfloat_t * b)677 bool vec3_is_equal(mfloat_t *a, mfloat_t *b)
678 {
679 return nearly_equal(a[0], b[0], MFLT_EPSILON) && nearly_equal(a[1], b[1], MFLT_EPSILON) && nearly_equal(a[2], b[2], MFLT_EPSILON);
680 }
681
vec3_is_nearly_equal(mfloat_t * a,mfloat_t * b,mfloat_t epsilon)682 bool vec3_is_nearly_equal(mfloat_t *a, mfloat_t *b, mfloat_t epsilon)
683 {
684 return nearly_equal(a[0], b[0], epsilon) && nearly_equal(a[1], b[1], epsilon) && nearly_equal(a[2], b[2], epsilon);
685 }
686
vec3(mfloat_t * result,mfloat_t x,mfloat_t y,mfloat_t z)687 mfloat_t *vec3(mfloat_t *result, mfloat_t x, mfloat_t y, mfloat_t z)
688 {
689 result[0] = x;
690 result[1] = y;
691 result[2] = z;
692 return result;
693 }
694
vec3_assign(mfloat_t * result,mfloat_t * a)695 mfloat_t *vec3_assign(mfloat_t *result, mfloat_t *a)
696 {
697 result[0] = a[0];
698 result[1] = a[1];
699 result[2] = a[2];
700 return result;
701 }
702
vec3_assign_vec3i(mfloat_t * result,mint_t * a)703 mfloat_t *vec3_assign_vec3i(mfloat_t *result, mint_t *a)
704 {
705 result[0] = (mfloat_t)a[0];
706 result[1] = (mfloat_t)a[1];
707 result[2] = (mfloat_t)a[2];
708 return result;
709 }
710
vec3_zero(mfloat_t * result)711 mfloat_t *vec3_zero(mfloat_t *result)
712 {
713 result[0] = MFLOAT_C(0.0);
714 result[1] = MFLOAT_C(0.0);
715 result[2] = MFLOAT_C(0.0);
716 return result;
717 }
718
vec3_one(mfloat_t * result)719 mfloat_t *vec3_one(mfloat_t *result)
720 {
721 result[0] = MFLOAT_C(1.0);
722 result[1] = MFLOAT_C(1.0);
723 result[2] = MFLOAT_C(1.0);
724 return result;
725 }
726
vec3_add(mfloat_t * result,mfloat_t * a,mfloat_t * b)727 mfloat_t *vec3_add(mfloat_t *result, mfloat_t *a, mfloat_t *b)
728 {
729 result[0] = a[0] + b[0];
730 result[1] = a[1] + b[1];
731 result[2] = a[2] + b[2];
732 return result;
733 }
734
vec3_subtract(mfloat_t * result,mfloat_t * a,mfloat_t * b)735 mfloat_t *vec3_subtract(mfloat_t *result, mfloat_t *a, mfloat_t *b)
736 {
737 result[0] = a[0] - b[0];
738 result[1] = a[1] - b[1];
739 result[2] = a[2] - b[2];
740 return result;
741 }
742
vec3_scale(mfloat_t * result,mfloat_t * a,mfloat_t scalar)743 mfloat_t *vec3_scale(mfloat_t *result, mfloat_t *a, mfloat_t scalar)
744 {
745 result[0] = a[0] * scalar;
746 result[1] = a[1] * scalar;
747 result[2] = a[2] * scalar;
748 return result;
749 }
750
vec3_multiply(mfloat_t * result,mfloat_t * a,mfloat_t * b)751 mfloat_t *vec3_multiply(mfloat_t *result, mfloat_t *a, mfloat_t *b)
752 {
753 result[0] = a[0] * b[0];
754 result[1] = a[1] * b[1];
755 result[2] = a[2] * b[2];
756 return result;
757 }
758
vec3_multiply_mat3(mfloat_t * result,mfloat_t * a,mfloat_t * m)759 mfloat_t *vec3_multiply_mat3(mfloat_t *result, mfloat_t *a, mfloat_t *m)
760 {
761 mfloat_t x = a[0];
762 mfloat_t y = a[1];
763 mfloat_t z = a[2];
764 result[0] = m[0] * x + m[3] * y + m[6] * z;
765 result[1] = m[1] * x + m[4] * y + m[7] * z;
766 result[2] = m[2] * x + m[5] * y + m[8] * z;
767 return result;
768 }
769
vec3_divide(mfloat_t * result,mfloat_t * a,mfloat_t * b)770 mfloat_t *vec3_divide(mfloat_t *result, mfloat_t *a, mfloat_t *b)
771 {
772 result[0] = a[0] / b[0];
773 result[1] = a[1] / b[1];
774 result[2] = a[2] / b[2];
775 return result;
776 }
777
vec3_snap(mfloat_t * result,mfloat_t * a,mfloat_t * b)778 mfloat_t *vec3_snap(mfloat_t *result, mfloat_t *a, mfloat_t *b)
779 {
780 result[0] = MFLOOR(a[0] / b[0]) * b[0];
781 result[1] = MFLOOR(a[1] / b[1]) * b[1];
782 result[2] = MFLOOR(a[2] / b[2]) * b[2];
783 return result;
784 }
785
vec3_negative(mfloat_t * result,mfloat_t * a)786 mfloat_t *vec3_negative(mfloat_t *result, mfloat_t *a)
787 {
788 result[0] = -a[0];
789 result[1] = -a[1];
790 result[2] = -a[2];
791 return result;
792 }
793
vec3_inverse(mfloat_t * result,mfloat_t * a)794 mfloat_t *vec3_inverse(mfloat_t *result, mfloat_t *a)
795 {
796 if (!nearly_equal(a[0], MFLOAT_C(0.0), MFLT_EPSILON)) {
797 result[0] = MFLOAT_C(1.0) / a[0];
798 } else {
799 result[0] = MFLOAT_C(0.0);
800 }
801 if (!nearly_equal(a[1], MFLOAT_C(0.0), MFLT_EPSILON)) {
802 result[1] = MFLOAT_C(1.0) / a[1];
803 } else {
804 result[1] = MFLOAT_C(0.0);
805 }
806 if (!nearly_equal(a[2], MFLOAT_C(0.0), MFLT_EPSILON)) {
807 result[2] = MFLOAT_C(1.0) / a[2];
808 } else {
809 result[2] = MFLOAT_C(0.0);
810 }
811 return result;
812 }
813
vec3_abs(mfloat_t * result,mfloat_t * a)814 mfloat_t *vec3_abs(mfloat_t *result, mfloat_t *a)
815 {
816 result[0] = MABS(a[0]);
817 result[1] = MABS(a[1]);
818 result[2] = MABS(a[2]);
819 return result;
820 }
821
vec3_floor(mfloat_t * result,mfloat_t * a)822 mfloat_t *vec3_floor(mfloat_t *result, mfloat_t *a)
823 {
824 result[0] = MFLOOR(a[0]);
825 result[1] = MFLOOR(a[1]);
826 result[2] = MFLOOR(a[2]);
827 return result;
828 }
829
vec3_ceil(mfloat_t * result,mfloat_t * a)830 mfloat_t *vec3_ceil(mfloat_t *result, mfloat_t *a)
831 {
832 result[0] = MCEIL(a[0]);
833 result[1] = MCEIL(a[1]);
834 result[2] = MCEIL(a[2]);
835 return result;
836 }
837
vec3_round(mfloat_t * result,mfloat_t * a)838 mfloat_t *vec3_round(mfloat_t *result, mfloat_t *a)
839 {
840 result[0] = MROUND(a[0]);
841 result[1] = MROUND(a[1]);
842 result[2] = MROUND(a[2]);
843 return result;
844 }
845
vec3_max(mfloat_t * result,mfloat_t * a,mfloat_t * b)846 mfloat_t *vec3_max(mfloat_t *result, mfloat_t *a, mfloat_t *b)
847 {
848 result[0] = MMAX(a[0], b[0]);
849 result[1] = MMAX(a[1], b[1]);
850 result[2] = MMAX(a[2], b[2]);
851 return result;
852 }
853
vec3_min(mfloat_t * result,mfloat_t * a,mfloat_t * b)854 mfloat_t *vec3_min(mfloat_t *result, mfloat_t *a, mfloat_t *b)
855 {
856 result[0] = MMIN(a[0], b[0]);
857 result[1] = MMIN(a[1], b[1]);
858 result[2] = MMIN(a[2], b[2]);
859 return result;
860 }
861
vec3_clamp(mfloat_t * result,mfloat_t * a,mfloat_t * lower,mfloat_t * higher)862 mfloat_t *vec3_clamp(mfloat_t *result, mfloat_t *a, mfloat_t *lower, mfloat_t *higher)
863 {
864 result[0] = a[0];
865 result[1] = a[1];
866 result[2] = a[2];
867 if (result[0] < lower[0]) {
868 result[0] = lower[0];
869 }
870 if (result[0] > higher[0]) {
871 result[0] = higher[0];
872 }
873 if (result[1] < lower[1]) {
874 result[1] = lower[1];
875 }
876 if (result[1] > higher[1]) {
877 result[1] = higher[1];
878 }
879 if (result[2] < lower[2]) {
880 result[2] = lower[2];
881 }
882 if (result[2] > higher[2]) {
883 result[2] = higher[2];
884 }
885 return result;
886 }
887
vec3_cross(mfloat_t * result,mfloat_t * a,mfloat_t * b)888 mfloat_t *vec3_cross(mfloat_t *result, mfloat_t *a, mfloat_t *b)
889 {
890 mfloat_t cross[VEC3_SIZE];
891 cross[0] = a[1] * b[2] - a[2] * b[1];
892 cross[1] = a[2] * b[0] - a[0] * b[2];
893 cross[2] = a[0] * b[1] - a[1] * b[0];
894 result[0] = cross[0];
895 result[1] = cross[1];
896 result[2] = cross[2];
897 return result;
898 }
899
vec3_normalize(mfloat_t * result,mfloat_t * a)900 mfloat_t *vec3_normalize(mfloat_t *result, mfloat_t *a)
901 {
902 mfloat_t length = MSQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
903 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
904 length = MFLOAT_C(1.0) / length;
905 result[0] = a[0] * length;
906 result[1] = a[1] * length;
907 result[2] = a[2] * length;
908 } else {
909 result[0] = MFLOAT_C(0.0);
910 result[1] = MFLOAT_C(0.0);
911 result[2] = MFLOAT_C(0.0);
912 }
913 return result;
914 }
915
vec3_project(mfloat_t * result,mfloat_t * a,mfloat_t * b)916 mfloat_t *vec3_project(mfloat_t *result, mfloat_t *a, mfloat_t *b)
917 {
918 mfloat_t s = vec3_dot(a, b) / vec3_dot(b, b);
919 result[0] = b[0] * s;
920 result[1] = b[1] * s;
921 result[2] = b[2] * s;
922 return result;
923 }
924
vec3_slide(mfloat_t * result,mfloat_t * a,mfloat_t * b)925 mfloat_t *vec3_slide(mfloat_t *result, mfloat_t *a, mfloat_t *b)
926 {
927 mfloat_t d = vec3_dot(a, b);
928 result[0] = a[0] - b[0] * d;
929 result[1] = a[1] - b[1] * d;
930 result[2] = a[2] - b[2] * d;
931 return result;
932 }
933
vec3_reflect(mfloat_t * result,mfloat_t * a,mfloat_t * b)934 mfloat_t *vec3_reflect(mfloat_t *result, mfloat_t *a, mfloat_t *b)
935 {
936 mfloat_t d = MFLOAT_C(2.0) * vec3_dot(a, b);
937 result[0] = a[0] - b[0] * d;
938 result[1] = a[1] - b[1] * d;
939 result[2] = a[2] - b[2] * d;
940 return result;
941 }
942
vec3_lerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)943 mfloat_t *vec3_lerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
944 {
945 result[0] = a[0] + (b[0] - a[0]) * p;
946 result[1] = a[1] + (b[1] - a[1]) * p;
947 result[2] = a[2] + (b[2] - a[2]) * p;
948 return result;
949 }
950
vec3_bezier3(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t * c,mfloat_t p)951 mfloat_t *vec3_bezier3(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t *c, mfloat_t p)
952 {
953 mfloat_t tmp_a[VEC3_SIZE];
954 mfloat_t tmp_b[VEC3_SIZE];
955 vec3_lerp(tmp_a, a, b, p);
956 vec3_lerp(tmp_b, b, c, p);
957 vec3_lerp(result, tmp_a, tmp_b, p);
958 return result;
959 }
960
vec3_bezier4(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t * c,mfloat_t * d,mfloat_t p)961 mfloat_t *vec3_bezier4(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t *c, mfloat_t *d, mfloat_t p)
962 {
963 mfloat_t tmp_a[VEC3_SIZE];
964 mfloat_t tmp_b[VEC3_SIZE];
965 mfloat_t tmp_c[VEC3_SIZE];
966 mfloat_t tmp_d[VEC3_SIZE];
967 mfloat_t tmp_e[VEC3_SIZE];
968 vec3_lerp(tmp_a, a, b, p);
969 vec3_lerp(tmp_b, b, c, p);
970 vec3_lerp(tmp_c, c, d, p);
971 vec3_lerp(tmp_d, tmp_a, tmp_b, p);
972 vec3_lerp(tmp_e, tmp_b, tmp_c, p);
973 vec3_lerp(result, tmp_d, tmp_e, p);
974 return result;
975 }
976
vec3_dot(mfloat_t * a,mfloat_t * b)977 mfloat_t vec3_dot(mfloat_t *a, mfloat_t *b)
978 {
979 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
980 }
981
vec3_length(mfloat_t * a)982 mfloat_t vec3_length(mfloat_t *a)
983 {
984 return MSQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
985 }
986
vec3_length_squared(mfloat_t * a)987 mfloat_t vec3_length_squared(mfloat_t *a)
988 {
989 return a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
990 }
991
vec3_distance(mfloat_t * a,mfloat_t * b)992 mfloat_t vec3_distance(mfloat_t *a, mfloat_t *b)
993 {
994 return MSQRT((a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]) + (a[2] - b[2]) * (a[2] - b[2]));
995 }
996
vec3_distance_squared(mfloat_t * a,mfloat_t * b)997 mfloat_t vec3_distance_squared(mfloat_t *a, mfloat_t *b)
998 {
999 return (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]) + (a[2] - b[2]) * (a[2] - b[2]);
1000 }
1001
1002 /* Vector 3D Integer */
vec3i_is_zero(mint_t * a)1003 bool vec3i_is_zero(mint_t *a)
1004 {
1005 return a[0] == 0 && a[1] == 0;
1006 }
1007
vec3i_is_equal(mint_t * a,mint_t * b)1008 bool vec3i_is_equal(mint_t *a, mint_t *b)
1009 {
1010 return a[0] == b[0] && a[1] == b[1];
1011 }
1012
vec3i(mint_t * result,mint_t x,mint_t y,mint_t z)1013 mint_t *vec3i(mint_t *result, mint_t x, mint_t y, mint_t z)
1014 {
1015 result[0] = x;
1016 result[1] = y;
1017 result[2] = z;
1018 return result;
1019 }
1020
vec3i_assign(mint_t * result,mint_t * a)1021 mint_t *vec3i_assign(mint_t *result, mint_t *a)
1022 {
1023 result[0] = a[0];
1024 result[1] = a[1];
1025 result[2] = a[2];
1026 return result;
1027 }
1028
vec3i_assign_vec3(mint_t * result,mfloat_t * a)1029 mint_t *vec3i_assign_vec3(mint_t *result, mfloat_t *a)
1030 {
1031 result[0] = (mint_t)a[0];
1032 result[1] = (mint_t)a[1];
1033 result[2] = (mint_t)a[2];
1034 return result;
1035 }
1036
vec3i_zero(mint_t * result)1037 mint_t *vec3i_zero(mint_t *result)
1038 {
1039 result[0] = 0;
1040 result[1] = 0;
1041 result[2] = 0;
1042 return result;
1043 }
1044
vec3i_one(mint_t * result)1045 mint_t *vec3i_one(mint_t *result)
1046 {
1047 result[0] = 1;
1048 result[1] = 1;
1049 result[2] = 1;
1050 return result;
1051 }
1052
vec3i_add(mint_t * result,mint_t * a,mint_t * b)1053 mint_t *vec3i_add(mint_t *result, mint_t *a, mint_t *b)
1054 {
1055 result[0] = a[0] + b[0];
1056 result[1] = a[1] + b[1];
1057 result[2] = a[2] + b[2];
1058 return result;
1059 }
1060
vec3i_subtract(mint_t * result,mint_t * a,mint_t * b)1061 mint_t *vec3i_subtract(mint_t *result, mint_t *a, mint_t *b)
1062 {
1063 result[0] = a[0] - b[0];
1064 result[1] = a[1] - b[1];
1065 result[2] = a[2] - b[2];
1066 return result;
1067 }
1068
vec3i_scale(mint_t * result,mint_t * a,mfloat_t scalar)1069 mint_t *vec3i_scale(mint_t *result, mint_t *a, mfloat_t scalar)
1070 {
1071 result[0] = (mint_t)MROUND(a[0] * scalar);
1072 result[1] = (mint_t)MROUND(a[1] * scalar);
1073 result[2] = (mint_t)MROUND(a[2] * scalar);
1074 return result;
1075 }
1076
vec3i_multiply(mint_t * result,mint_t * a,mint_t * b)1077 mint_t *vec3i_multiply(mint_t *result, mint_t *a, mint_t *b)
1078 {
1079 result[0] = a[0] * b[0];
1080 result[1] = a[1] * b[1];
1081 result[2] = a[2] * b[2];
1082 return result;
1083 }
1084
vec3i_multiply_mat3(mint_t * result,mint_t * a,mfloat_t * m)1085 mint_t *vec3i_multiply_mat3(mint_t *result, mint_t *a, mfloat_t *m)
1086 {
1087 mint_t x = a[0];
1088 mint_t y = a[1];
1089 mint_t z = a[2];
1090 result[0] = (mint_t)MROUND(m[0] * x + m[3] * y + m[6] * z);
1091 result[1] = (mint_t)MROUND(m[1] * x + m[4] * y + m[7] * z);
1092 result[2] = (mint_t)MROUND(m[2] * x + m[5] * y + m[8] * z);
1093 return result;
1094 }
1095
vec3i_divide(mint_t * result,mint_t * a,mint_t * b)1096 mint_t *vec3i_divide(mint_t *result, mint_t *a, mint_t *b)
1097 {
1098 result[0] = a[0] / b[0];
1099 result[1] = a[1] / b[1];
1100 result[2] = a[2] / b[2];
1101 return result;
1102 }
1103
vec3i_snap(mint_t * result,mint_t * a,mint_t * b)1104 mint_t *vec3i_snap(mint_t *result, mint_t *a, mint_t *b)
1105 {
1106 result[0] = (a[0] / b[0]) * b[0];
1107 result[1] = (a[1] / b[1]) * b[1];
1108 result[2] = (a[2] / b[2]) * b[2];
1109 return result;
1110 }
1111
vec3i_negative(mint_t * result,mint_t * a)1112 mint_t *vec3i_negative(mint_t *result, mint_t *a)
1113 {
1114 result[0] = -a[0];
1115 result[1] = -a[1];
1116 result[2] = -a[2];
1117 return result;
1118 }
1119
vec3i_inverse(mint_t * result,mint_t * a)1120 mint_t *vec3i_inverse(mint_t *result, mint_t *a)
1121 {
1122 if (a[0] != 0) {
1123 result[0] = (mint_t)MROUND(MFLOAT_C(1.0) / (mfloat_t)a[0]);
1124 } else {
1125 result[0] = 0;
1126 }
1127 if (a[1] != 0) {
1128 result[1] = (mint_t)MROUND(MFLOAT_C(1.0) / (mfloat_t)a[1]);
1129 } else {
1130 result[1] = 0;
1131 }
1132 if (a[2] != 0) {
1133 result[2] = (mint_t)MROUND(MFLOAT_C(1.0) / (mfloat_t)a[2]);
1134 } else {
1135 result[2] = 0;
1136 }
1137 return result;
1138 }
1139
vec3i_abs(mint_t * result,mint_t * a)1140 mint_t *vec3i_abs(mint_t *result, mint_t *a)
1141 {
1142 result[0] = a[0];
1143 if (result[0] < 0) {
1144 result[0] = -result[0];
1145 }
1146 result[1] = a[1];
1147 if (result[1] < 0) {
1148 result[1] = -result[1];
1149 }
1150 result[2] = a[2];
1151 if (result[2] < 0) {
1152 result[2] = -result[2];
1153 }
1154 return result;
1155 }
1156
vec3i_floor(mint_t * result,mfloat_t * a)1157 mint_t *vec3i_floor(mint_t *result, mfloat_t *a)
1158 {
1159 result[0] = (mint_t)MFLOOR(a[0]);
1160 result[1] = (mint_t)MFLOOR(a[1]);
1161 result[2] = (mint_t)MFLOOR(a[2]);
1162 return result;
1163 }
1164
vec3i_ceil(mint_t * result,mfloat_t * a)1165 mint_t *vec3i_ceil(mint_t *result, mfloat_t *a)
1166 {
1167 result[0] = (mint_t)MCEIL(a[0]);
1168 result[1] = (mint_t)MCEIL(a[1]);
1169 result[2] = (mint_t)MCEIL(a[2]);
1170 return result;
1171 }
1172
vec3i_round(mint_t * result,mfloat_t * a)1173 mint_t *vec3i_round(mint_t *result, mfloat_t *a)
1174 {
1175 result[0] = (mint_t)MROUND(a[0]);
1176 result[1] = (mint_t)MROUND(a[1]);
1177 result[2] = (mint_t)MROUND(a[2]);
1178 return result;
1179 }
1180
vec3i_max(mint_t * result,mint_t * a,mint_t * b)1181 mint_t *vec3i_max(mint_t *result, mint_t *a, mint_t *b)
1182 {
1183 result[0] = MMAXI(a[0], b[0]);
1184 result[1] = MMAXI(a[1], b[1]);
1185 result[2] = MMAXI(a[2], b[2]);
1186 return result;
1187 }
1188
vec3i_min(mint_t * result,mint_t * a,mint_t * b)1189 mint_t *vec3i_min(mint_t *result, mint_t *a, mint_t *b)
1190 {
1191 result[0] = MMINI(a[0], b[0]);
1192 result[1] = MMINI(a[1], b[1]);
1193 result[2] = MMINI(a[2], b[2]);
1194 return result;
1195 }
1196
vec3i_clamp(mint_t * result,mint_t * a,mint_t * lower,mint_t * higher)1197 mint_t *vec3i_clamp(mint_t *result, mint_t *a, mint_t *lower, mint_t *higher)
1198 {
1199 result[0] = a[0];
1200 result[1] = a[1];
1201 result[2] = a[2];
1202 if (result[0] < lower[0]) {
1203 result[0] = lower[0];
1204 }
1205 if (result[0] > higher[0]) {
1206 result[0] = higher[0];
1207 }
1208 if (result[1] < lower[1]) {
1209 result[1] = lower[1];
1210 }
1211 if (result[1] > higher[1]) {
1212 result[1] = higher[1];
1213 }
1214 if (result[2] < lower[2]) {
1215 result[2] = lower[2];
1216 }
1217 if (result[2] > higher[2]) {
1218 result[2] = higher[2];
1219 }
1220 return result;
1221 }
1222
vec3i_cross(mint_t * result,mint_t * a,mint_t * b)1223 mint_t *vec3i_cross(mint_t *result, mint_t *a, mint_t *b)
1224 {
1225 mint_t cross[VEC3_SIZE];
1226 cross[0] = a[1] * b[2] - a[2] * b[1];
1227 cross[1] = a[2] * b[0] - a[0] * b[2];
1228 cross[2] = a[0] * b[1] - a[1] * b[0];
1229 result[0] = cross[0];
1230 result[1] = cross[1];
1231 result[2] = cross[2];
1232 return result;
1233 }
1234
vec3i_normalize(mint_t * result,mint_t * a)1235 mint_t *vec3i_normalize(mint_t *result, mint_t *a)
1236 {
1237 mfloat_t length = MSQRT((mfloat_t)a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
1238 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
1239 length = MFLOAT_C(1.0) / length;
1240 result[0] = (mint_t)MROUND(a[0] * length);
1241 result[1] = (mint_t)MROUND(a[1] * length);
1242 result[2] = (mint_t)MROUND(a[2] * length);
1243 } else {
1244 result[0] = 0;
1245 result[1] = 0;
1246 result[2] = 0;
1247 }
1248 return result;
1249 }
1250
vec3i_project(mint_t * result,mint_t * a,mint_t * b)1251 mint_t *vec3i_project(mint_t *result, mint_t *a, mint_t *b)
1252 {
1253 mfloat_t s = (mfloat_t)vec3i_dot(a, b) / vec3i_dot(b, b);
1254 result[0] = (mint_t)MROUND(b[0] * s);
1255 result[1] = (mint_t)MROUND(b[1] * s);
1256 result[2] = (mint_t)MROUND(b[2] * s);
1257 return result;
1258 }
1259
vec3i_slide(mint_t * result,mint_t * a,mint_t * b)1260 mint_t *vec3i_slide(mint_t *result, mint_t *a, mint_t *b)
1261 {
1262 mfloat_t d = (mfloat_t)vec3i_dot(a, b);
1263 result[0] = (mint_t)MROUND(a[0] - b[0] * d);
1264 result[1] = (mint_t)MROUND(a[1] - b[1] * d);
1265 result[2] = (mint_t)MROUND(a[2] - b[2] * d);
1266 return result;
1267 }
1268
vec3i_reflect(mint_t * result,mint_t * a,mint_t * b)1269 mint_t *vec3i_reflect(mint_t *result, mint_t *a, mint_t *b)
1270 {
1271 mfloat_t d = MFLOAT_C(2.0) * vec3i_dot(a, b);
1272 result[0] = a[0] - (mint_t)MROUND(b[0] * d);
1273 result[1] = a[1] - (mint_t)MROUND(b[1] * d);
1274 result[2] = a[2] - (mint_t)MROUND(b[2] * d);
1275 return result;
1276 }
1277
vec3i_lerp(mint_t * result,mint_t * a,mint_t * b,mfloat_t p)1278 mint_t *vec3i_lerp(mint_t *result, mint_t *a, mint_t *b, mfloat_t p)
1279 {
1280 result[0] = a[0] + (mint_t)MROUND((b[0] - a[0]) * p);
1281 result[1] = a[1] + (mint_t)MROUND((b[1] - a[1]) * p);
1282 result[2] = a[2] + (mint_t)MROUND((b[2] - a[2]) * p);
1283 return result;
1284 }
1285
vec3i_bezier3(mint_t * result,mint_t * a,mint_t * b,mint_t * c,mfloat_t p)1286 mint_t *vec3i_bezier3(mint_t *result, mint_t *a, mint_t *b, mint_t *c, mfloat_t p)
1287 {
1288 mint_t tmp_a[VEC3_SIZE];
1289 mint_t tmp_b[VEC3_SIZE];
1290 vec3i_lerp(tmp_a, a, b, p);
1291 vec3i_lerp(tmp_b, b, c, p);
1292 vec3i_lerp(result, tmp_a, tmp_b, p);
1293 return result;
1294 }
1295
vec3i_bezier4(mint_t * result,mint_t * a,mint_t * b,mint_t * c,mint_t * d,mfloat_t p)1296 mint_t *vec3i_bezier4(mint_t *result, mint_t *a, mint_t *b, mint_t *c, mint_t *d, mfloat_t p)
1297 {
1298 mint_t tmp_a[VEC3_SIZE];
1299 mint_t tmp_b[VEC3_SIZE];
1300 mint_t tmp_c[VEC3_SIZE];
1301 mint_t tmp_d[VEC3_SIZE];
1302 mint_t tmp_e[VEC3_SIZE];
1303 vec3i_lerp(tmp_a, a, b, p);
1304 vec3i_lerp(tmp_b, b, c, p);
1305 vec3i_lerp(tmp_c, c, d, p);
1306 vec3i_lerp(tmp_d, tmp_a, tmp_b, p);
1307 vec3i_lerp(tmp_e, tmp_b, tmp_c, p);
1308 vec3i_lerp(result, tmp_d, tmp_e, p);
1309 return result;
1310 }
1311
vec3i_dot(mint_t * a,mint_t * b)1312 mint_t vec3i_dot(mint_t *a, mint_t *b)
1313 {
1314 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
1315 }
1316
vec3i_length(mint_t * a)1317 mfloat_t vec3i_length(mint_t *a)
1318 {
1319 return MSQRT((mfloat_t)a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
1320 }
1321
vec3i_length_squared(mint_t * a)1322 mint_t vec3i_length_squared(mint_t *a)
1323 {
1324 return a[0] * a[0] + a[1] * a[1] + a[2] * a[2];
1325 }
1326
vec3i_distance(mint_t * a,mint_t * b)1327 mfloat_t vec3i_distance(mint_t *a, mint_t *b)
1328 {
1329 return MSQRT((mfloat_t)(a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]) + (a[2] - b[2]) * (a[2] - b[2]));
1330 }
1331
vec3i_distance_squared(mint_t * a,mint_t * b)1332 mint_t vec3i_distance_squared(mint_t *a, mint_t *b)
1333 {
1334 return (a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]) + (a[2] - b[2]) * (a[2] - b[2]);
1335 }
1336
1337 /* Vector 4D */
vec4_is_zero(mfloat_t * a)1338 bool vec4_is_zero(mfloat_t *a)
1339 {
1340 return nearly_equal(a[0], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[1], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[2], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[3], MFLOAT_C(0.0), MFLT_EPSILON);
1341 }
1342
vec4_is_near_zero(mfloat_t * a,mfloat_t epsilon)1343 bool vec4_is_near_zero(mfloat_t *a, mfloat_t epsilon)
1344 {
1345 return nearly_equal(a[0], MFLOAT_C(0.0), epsilon) && nearly_equal(a[1], MFLOAT_C(0.0), epsilon) && nearly_equal(a[2], MFLOAT_C(0.0), epsilon) && nearly_equal(a[3], MFLOAT_C(0.0), epsilon);
1346 }
1347
vec4_is_equal(mfloat_t * a,mfloat_t * b)1348 bool vec4_is_equal(mfloat_t *a, mfloat_t *b)
1349 {
1350 return nearly_equal(a[0], b[0], MFLT_EPSILON) && nearly_equal(a[1], b[1], MFLT_EPSILON) && nearly_equal(a[2], b[2], MFLT_EPSILON) && nearly_equal(a[3], b[3], MFLT_EPSILON);
1351 }
1352
vec4_is_nearly_equal(mfloat_t * a,mfloat_t * b,mfloat_t epsilon)1353 bool vec4_is_nearly_equal(mfloat_t *a, mfloat_t *b, mfloat_t epsilon)
1354 {
1355 return nearly_equal(a[0], b[0], epsilon) && nearly_equal(a[1], b[1], epsilon) && nearly_equal(a[2], b[2], epsilon) && nearly_equal(a[3], b[3], epsilon);
1356 }
1357
vec4(mfloat_t * result,mfloat_t x,mfloat_t y,mfloat_t z,mfloat_t w)1358 mfloat_t *vec4(mfloat_t *result, mfloat_t x, mfloat_t y, mfloat_t z, mfloat_t w)
1359 {
1360 result[0] = x;
1361 result[1] = y;
1362 result[2] = z;
1363 result[3] = w;
1364 return result;
1365 }
1366
vec4_assign(mfloat_t * result,mfloat_t * a)1367 mfloat_t *vec4_assign(mfloat_t *result, mfloat_t *a)
1368 {
1369 result[0] = a[0];
1370 result[1] = a[1];
1371 result[2] = a[2];
1372 result[3] = a[3];
1373 return result;
1374 }
1375
vec4_assign_vec4i(mfloat_t * result,mint_t * a)1376 mfloat_t *vec4_assign_vec4i(mfloat_t *result, mint_t *a)
1377 {
1378 result[0] = (mfloat_t)a[0];
1379 result[1] = (mfloat_t)a[1];
1380 result[2] = (mfloat_t)a[2];
1381 result[3] = (mfloat_t)a[3];
1382 return result;
1383 }
1384
vec4_zero(mfloat_t * result)1385 mfloat_t *vec4_zero(mfloat_t *result)
1386 {
1387 result[0] = MFLOAT_C(0.0);
1388 result[1] = MFLOAT_C(0.0);
1389 result[2] = MFLOAT_C(0.0);
1390 result[3] = MFLOAT_C(0.0);
1391 return result;
1392 }
1393
vec4_one(mfloat_t * result)1394 mfloat_t *vec4_one(mfloat_t *result)
1395 {
1396 result[0] = MFLOAT_C(1.0);
1397 result[1] = MFLOAT_C(1.0);
1398 result[2] = MFLOAT_C(1.0);
1399 result[3] = MFLOAT_C(1.0);
1400 return result;
1401 }
1402
vec4_add(mfloat_t * result,mfloat_t * a,mfloat_t * b)1403 mfloat_t *vec4_add(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1404 {
1405 result[0] = a[0] + b[0];
1406 result[1] = a[1] + b[1];
1407 result[2] = a[2] + b[2];
1408 result[3] = a[3] + b[3];
1409 return result;
1410 }
1411
vec4_subtract(mfloat_t * result,mfloat_t * a,mfloat_t * b)1412 mfloat_t *vec4_subtract(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1413 {
1414 result[0] = a[0] - b[0];
1415 result[1] = a[1] - b[1];
1416 result[2] = a[2] - b[2];
1417 result[3] = a[3] - b[3];
1418 return result;
1419 }
1420
vec4_scale(mfloat_t * result,mfloat_t * a,mfloat_t scalar)1421 mfloat_t *vec4_scale(mfloat_t *result, mfloat_t *a, mfloat_t scalar)
1422 {
1423 result[0] = a[0] * scalar;
1424 result[1] = a[1] * scalar;
1425 result[2] = a[2] * scalar;
1426 result[3] = a[3] * scalar;
1427 return result;
1428 }
1429
vec4_multiply(mfloat_t * result,mfloat_t * a,mfloat_t * b)1430 mfloat_t *vec4_multiply(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1431 {
1432 result[0] = a[0] * b[0];
1433 result[1] = a[1] * b[1];
1434 result[2] = a[2] * b[2];
1435 result[3] = a[3] * b[3];
1436 return result;
1437 }
1438
vec4_multiply_mat4(mfloat_t * result,mfloat_t * a,mfloat_t * m)1439 mfloat_t *vec4_multiply_mat4(mfloat_t *result, mfloat_t *a, mfloat_t *m)
1440 {
1441 mfloat_t x = a[0];
1442 mfloat_t y = a[1];
1443 mfloat_t z = a[2];
1444 mfloat_t w = a[3];
1445 result[0] = m[0] * x + m[4] * y + m[8] * z + m[12] * w;
1446 result[1] = m[1] * x + m[5] * y + m[9] * z + m[13] * w;
1447 result[2] = m[2] * x + m[6] * y + m[10] * z + m[14] * w;
1448 result[3] = m[3] * x + m[7] * y + m[11] * z + m[15] * w;
1449 return result;
1450 }
1451
vec4_divide(mfloat_t * result,mfloat_t * a,mfloat_t * b)1452 mfloat_t *vec4_divide(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1453 {
1454 result[0] = a[0] / b[0];
1455 result[1] = a[1] / b[1];
1456 result[2] = a[2] / b[2];
1457 result[3] = a[3] / b[3];
1458 return result;
1459 }
1460
vec4_snap(mfloat_t * result,mfloat_t * a,mfloat_t * b)1461 mfloat_t *vec4_snap(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1462 {
1463 result[0] = MFLOOR(a[0] / b[0]) * b[0];
1464 result[1] = MFLOOR(a[1] / b[1]) * b[1];
1465 result[2] = MFLOOR(a[2] / b[2]) * b[2];
1466 result[3] = MFLOOR(a[3] / b[3]) * b[3];
1467 return result;
1468 }
1469
vec4_negative(mfloat_t * result,mfloat_t * a)1470 mfloat_t *vec4_negative(mfloat_t *result, mfloat_t *a)
1471 {
1472 result[0] = -a[0];
1473 result[1] = -a[1];
1474 result[2] = -a[2];
1475 result[3] = -a[3];
1476 return result;
1477 }
1478
vec4_inverse(mfloat_t * result,mfloat_t * a)1479 mfloat_t *vec4_inverse(mfloat_t *result, mfloat_t *a)
1480 {
1481 if (!nearly_equal(a[0], MFLOAT_C(0.0), MFLT_EPSILON)) {
1482 result[0] = MFLOAT_C(1.0) / a[0];
1483 } else {
1484 result[0] = MFLOAT_C(0.0);
1485 }
1486 if (!nearly_equal(a[1], MFLOAT_C(0.0), MFLT_EPSILON)) {
1487 result[1] = MFLOAT_C(1.0) / a[1];
1488 } else {
1489 result[1] = MFLOAT_C(0.0);
1490 }
1491 if (!nearly_equal(a[2], MFLOAT_C(0.0), MFLT_EPSILON)) {
1492 result[2] = MFLOAT_C(1.0) / a[2];
1493 } else {
1494 result[2] = MFLOAT_C(0.0);
1495 }
1496 if (!nearly_equal(a[3], MFLOAT_C(0.0), MFLT_EPSILON)) {
1497 result[3] = MFLOAT_C(1.0) / a[3];
1498 } else {
1499 result[3] = MFLOAT_C(0.0);
1500 }
1501 return result;
1502 }
1503
vec4_abs(mfloat_t * result,mfloat_t * a)1504 mfloat_t *vec4_abs(mfloat_t *result, mfloat_t *a)
1505 {
1506 result[0] = MABS(a[0]);
1507 result[1] = MABS(a[1]);
1508 result[2] = MABS(a[2]);
1509 result[3] = MABS(a[3]);
1510 return result;
1511 }
1512
vec4_floor(mfloat_t * result,mfloat_t * a)1513 mfloat_t *vec4_floor(mfloat_t *result, mfloat_t *a)
1514 {
1515 result[0] = MFLOOR(a[0]);
1516 result[1] = MFLOOR(a[1]);
1517 result[2] = MFLOOR(a[2]);
1518 result[3] = MFLOOR(a[3]);
1519 return result;
1520 }
1521
vec4_ceil(mfloat_t * result,mfloat_t * a)1522 mfloat_t *vec4_ceil(mfloat_t *result, mfloat_t *a)
1523 {
1524 result[0] = MCEIL(a[0]);
1525 result[1] = MCEIL(a[1]);
1526 result[2] = MCEIL(a[2]);
1527 result[3] = MCEIL(a[3]);
1528 return result;
1529 }
1530
vec4_round(mfloat_t * result,mfloat_t * a)1531 mfloat_t *vec4_round(mfloat_t *result, mfloat_t *a)
1532 {
1533 result[0] = MROUND(a[0]);
1534 result[1] = MROUND(a[1]);
1535 result[2] = MROUND(a[2]);
1536 result[3] = MROUND(a[3]);
1537 return result;
1538 }
1539
vec4_max(mfloat_t * result,mfloat_t * a,mfloat_t * b)1540 mfloat_t *vec4_max(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1541 {
1542 result[0] = MMAX(a[0], b[0]);
1543 result[1] = MMAX(a[1], b[1]);
1544 result[2] = MMAX(a[2], b[2]);
1545 result[3] = MMAX(a[3], b[3]);
1546 return result;
1547 }
1548
vec4_min(mfloat_t * result,mfloat_t * a,mfloat_t * b)1549 mfloat_t *vec4_min(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1550 {
1551 result[0] = MMIN(a[0], b[0]);
1552 result[1] = MMIN(a[1], b[1]);
1553 result[2] = MMIN(a[2], b[2]);
1554 result[3] = MMIN(a[3], b[3]);
1555 return result;
1556 }
1557
vec4_clamp(mfloat_t * result,mfloat_t * a,mfloat_t * lower,mfloat_t * higher)1558 mfloat_t *vec4_clamp(mfloat_t *result, mfloat_t *a, mfloat_t *lower, mfloat_t *higher)
1559 {
1560 result[0] = a[0];
1561 result[1] = a[1];
1562 result[2] = a[2];
1563 result[3] = a[3];
1564 if (result[0] < lower[0]) {
1565 result[0] = lower[0];
1566 }
1567 if (result[0] > higher[0]) {
1568 result[0] = higher[0];
1569 }
1570 if (result[1] < lower[1]) {
1571 result[1] = lower[1];
1572 }
1573 if (result[1] > higher[1]) {
1574 result[1] = higher[1];
1575 }
1576 if (result[2] < lower[2]) {
1577 result[2] = lower[2];
1578 }
1579 if (result[2] > higher[2]) {
1580 result[2] = higher[2];
1581 }
1582 if (result[3] < lower[3]) {
1583 result[3] = lower[3];
1584 }
1585 if (result[3] > higher[3]) {
1586 result[3] = higher[3];
1587 }
1588 return result;
1589 }
1590
vec4_normalize(mfloat_t * result,mfloat_t * a)1591 mfloat_t *vec4_normalize(mfloat_t *result, mfloat_t *a)
1592 {
1593 mfloat_t length = MSQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]);
1594 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
1595 length = MFLOAT_C(1.0) / length;
1596 result[0] = a[0] * length;
1597 result[1] = a[1] * length;
1598 result[2] = a[2] * length;
1599 result[3] = a[3] * length;
1600 } else {
1601 result[0] = MFLOAT_C(0.0);
1602 result[1] = MFLOAT_C(0.0);
1603 result[2] = MFLOAT_C(0.0);
1604 result[3] = MFLOAT_C(0.0);
1605 }
1606 return result;
1607 }
1608
vec4_lerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)1609 mfloat_t *vec4_lerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
1610 {
1611 result[0] = a[0] + (b[0] - a[0]) * p;
1612 result[1] = a[1] + (b[1] - a[1]) * p;
1613 result[2] = a[2] + (b[2] - a[2]) * p;
1614 result[3] = a[3] + (b[3] - a[3]) * p;
1615 return result;
1616 }
1617
1618 /* Vector 4D Integer */
vec4i_is_zero(mint_t * a)1619 bool vec4i_is_zero(mint_t *a)
1620 {
1621 return a[0] == 0 && a[1] == 0 && a[2] == 0 && a[3] == 0;
1622 }
1623
vec4i_is_equal(mint_t * a,mint_t * b)1624 bool vec4i_is_equal(mint_t *a, mint_t *b)
1625 {
1626 return a[0] == b[0] && a[1] == b[1] && a[2] == b[2] && a[3] == b[3];
1627 }
1628
vec4i(mint_t * result,mint_t x,mint_t y,mint_t z,mint_t w)1629 mint_t *vec4i(mint_t *result, mint_t x, mint_t y, mint_t z, mint_t w)
1630 {
1631 result[0] = x;
1632 result[1] = y;
1633 result[2] = z;
1634 result[3] = w;
1635 return result;
1636 }
1637
vec4i_assign(mint_t * result,mint_t * a)1638 mint_t *vec4i_assign(mint_t *result, mint_t *a)
1639 {
1640 result[0] = a[0];
1641 result[1] = a[1];
1642 result[2] = a[2];
1643 result[3] = a[3];
1644 return result;
1645 }
1646
vec4i_assign_vec4(mint_t * result,mfloat_t * a)1647 mint_t *vec4i_assign_vec4(mint_t *result, mfloat_t *a)
1648 {
1649 result[0] = (mint_t)MROUND(a[0]);
1650 result[1] = (mint_t)MROUND(a[1]);
1651 result[2] = (mint_t)MROUND(a[2]);
1652 result[3] = (mint_t)MROUND(a[3]);
1653 return result;
1654 }
1655
vec4i_zero(mint_t * result)1656 mint_t *vec4i_zero(mint_t *result)
1657 {
1658 result[0] = 0;
1659 result[1] = 0;
1660 result[2] = 0;
1661 result[3] = 0;
1662 return result;
1663 }
1664
vec4i_one(mint_t * result)1665 mint_t *vec4i_one(mint_t *result)
1666 {
1667 result[0] = 1;
1668 result[1] = 1;
1669 result[2] = 1;
1670 result[3] = 1;
1671 return result;
1672 }
1673
vec4i_add(mint_t * result,mint_t * a,mint_t * b)1674 mint_t *vec4i_add(mint_t *result, mint_t *a, mint_t *b)
1675 {
1676 result[0] = a[0] + b[0];
1677 result[1] = a[1] + b[1];
1678 result[2] = a[2] + b[2];
1679 result[3] = a[3] + b[3];
1680 return result;
1681 }
1682
vec4i_subtract(mint_t * result,mint_t * a,mint_t * b)1683 mint_t *vec4i_subtract(mint_t *result, mint_t *a, mint_t *b)
1684 {
1685 result[0] = a[0] - b[0];
1686 result[1] = a[1] - b[1];
1687 result[2] = a[2] - b[2];
1688 result[3] = a[3] - b[3];
1689 return result;
1690 }
1691
vec4i_scale(mint_t * result,mint_t * a,mfloat_t scalar)1692 mint_t *vec4i_scale(mint_t *result, mint_t *a, mfloat_t scalar)
1693 {
1694 result[0] = (mint_t)MROUND(a[0] * scalar);
1695 result[1] = (mint_t)MROUND(a[1] * scalar);
1696 result[2] = (mint_t)MROUND(a[2] * scalar);
1697 result[3] = (mint_t)MROUND(a[3] * scalar);
1698 return result;
1699 }
1700
vec4i_multiply(mint_t * result,mint_t * a,mint_t * b)1701 mint_t *vec4i_multiply(mint_t *result, mint_t *a, mint_t *b)
1702 {
1703 result[0] = a[0] * b[0];
1704 result[1] = a[1] * b[1];
1705 result[2] = a[2] * b[2];
1706 result[3] = a[3] * b[3];
1707 return result;
1708 }
1709
vec4i_multiply_mat4(mint_t * result,mint_t * a,mfloat_t * m)1710 mint_t *vec4i_multiply_mat4(mint_t *result, mint_t *a, mfloat_t *m)
1711 {
1712 mint_t x = a[0];
1713 mint_t y = a[1];
1714 mint_t z = a[2];
1715 mint_t w = a[3];
1716 result[0] = (mint_t)MROUND(m[0] * x + m[4] * y + m[8] * z + m[12] * w);
1717 result[1] = (mint_t)MROUND(m[1] * x + m[5] * y + m[9] * z + m[13] * w);
1718 result[2] = (mint_t)MROUND(m[2] * x + m[6] * y + m[10] * z + m[14] * w);
1719 result[3] = (mint_t)MROUND(m[3] * x + m[7] * y + m[11] * z + m[15] * w);
1720 return result;
1721 }
1722
vec4i_divide(mint_t * result,mint_t * a,mint_t * b)1723 mint_t *vec4i_divide(mint_t *result, mint_t *a, mint_t *b)
1724 {
1725 result[0] = a[0] / b[0];
1726 result[1] = a[1] / b[1];
1727 result[2] = a[2] / b[2];
1728 result[3] = a[3] / b[3];
1729 return result;
1730 }
1731
vec4i_snap(mint_t * result,mint_t * a,mint_t * b)1732 mint_t *vec4i_snap(mint_t *result, mint_t *a, mint_t *b)
1733 {
1734 result[0] = (a[0] / b[0]) * b[0];
1735 result[1] = (a[1] / b[1]) * b[1];
1736 result[2] = (a[2] / b[2]) * b[2];
1737 result[3] = (a[3] / b[3]) * b[3];
1738 return result;
1739 }
1740
vec4i_negative(mint_t * result,mint_t * a)1741 mint_t *vec4i_negative(mint_t *result, mint_t *a)
1742 {
1743 result[0] = -a[0];
1744 result[1] = -a[1];
1745 result[2] = -a[2];
1746 result[3] = -a[3];
1747 return result;
1748 }
1749
vec4i_inverse(mint_t * result,mint_t * a)1750 mint_t *vec4i_inverse(mint_t *result, mint_t *a)
1751 {
1752 if (a[0] != 0) {
1753 result[0] = (mint_t)MROUND(MFLOAT_C(1.0) / (mfloat_t)a[0]);
1754 } else {
1755 result[0] = 0;
1756 }
1757 if (a[1] != 0) {
1758 result[1] = (mint_t)MROUND(MFLOAT_C(1.0) / (mfloat_t)a[1]);
1759 } else {
1760 result[1] = 0;
1761 }
1762 if (a[2] != 0) {
1763 result[2] = (mint_t)MROUND(MFLOAT_C(1.0) / (mfloat_t)a[2]);
1764 } else {
1765 result[2] = 0;
1766 }
1767 if (a[3] != 0) {
1768 result[3] = (mint_t)MROUND(MFLOAT_C(1.0) / (mfloat_t)a[3]);
1769 } else {
1770 result[3] = 0;
1771 }
1772 return result;
1773 }
1774
vec4i_abs(mint_t * result,mint_t * a)1775 mint_t *vec4i_abs(mint_t *result, mint_t *a)
1776 {
1777 result[0] = a[0];
1778 if (result[0] < 0) {
1779 result[0] = -result[0];
1780 }
1781 result[1] = a[1];
1782 if (result[1] < 0) {
1783 result[1] = -result[1];
1784 }
1785 result[2] = a[2];
1786 if (result[2] < 0) {
1787 result[2] = -result[2];
1788 }
1789 result[3] = a[3];
1790 if (result[3] < 0) {
1791 result[3] = -result[3];
1792 }return result;
1793 }
1794
vec4i_floor(mint_t * result,mfloat_t * a)1795 mint_t *vec4i_floor(mint_t *result, mfloat_t *a)
1796 {
1797 result[0] = (mint_t)MFLOOR(a[0]);
1798 result[1] = (mint_t)MFLOOR(a[1]);
1799 result[2] = (mint_t)MFLOOR(a[2]);
1800 result[3] = (mint_t)MFLOOR(a[3]);
1801 return result;
1802 }
1803
vec4i_ceil(mint_t * result,mfloat_t * a)1804 mint_t *vec4i_ceil(mint_t *result, mfloat_t *a)
1805 {
1806 result[0] = (mint_t)MCEIL(a[0]);
1807 result[1] = (mint_t)MCEIL(a[1]);
1808 result[2] = (mint_t)MCEIL(a[2]);
1809 result[3] = (mint_t)MCEIL(a[3]);
1810 return result;
1811 }
1812
vec4i_round(mint_t * result,mfloat_t * a)1813 mint_t *vec4i_round(mint_t *result, mfloat_t *a)
1814 {
1815 result[0] = (mint_t)MROUND(a[0]);
1816 result[1] = (mint_t)MROUND(a[1]);
1817 result[2] = (mint_t)MROUND(a[2]);
1818 result[3] = (mint_t)MROUND(a[3]);
1819 return result;
1820 }
1821
vec4i_max(mint_t * result,mint_t * a,mint_t * b)1822 mint_t *vec4i_max(mint_t *result, mint_t *a, mint_t *b)
1823 {
1824 result[0] = MMAXI(a[0], b[0]);
1825 result[1] = MMAXI(a[1], b[1]);
1826 result[2] = MMAXI(a[2], b[2]);
1827 result[3] = MMAXI(a[3], b[3]);
1828 return result;
1829 }
1830
vec4i_min(mint_t * result,mint_t * a,mint_t * b)1831 mint_t *vec4i_min(mint_t *result, mint_t *a, mint_t *b)
1832 {
1833 result[0] = MMINI(a[0], b[0]);
1834 result[1] = MMINI(a[1], b[1]);
1835 result[2] = MMINI(a[2], b[2]);
1836 result[3] = MMINI(a[3], b[3]);
1837 return result;
1838 }
1839
vec4i_clamp(mint_t * result,mint_t * a,mint_t * lower,mint_t * higher)1840 mint_t *vec4i_clamp(mint_t *result, mint_t *a, mint_t *lower, mint_t *higher)
1841 {
1842 result[0] = a[0];
1843 result[1] = a[1];
1844 result[2] = a[2];
1845 result[3] = a[3];
1846 if (result[0] < lower[0]) {
1847 result[0] = lower[0];
1848 }
1849 if (result[0] > higher[0]) {
1850 result[0] = higher[0];
1851 }
1852 if (result[1] < lower[1]) {
1853 result[1] = lower[1];
1854 }
1855 if (result[1] > higher[1]) {
1856 result[1] = higher[1];
1857 }
1858 if (result[2] < lower[2]) {
1859 result[2] = lower[2];
1860 }
1861 if (result[2] > higher[2]) {
1862 result[2] = higher[2];
1863 }
1864 if (result[3] < lower[3]) {
1865 result[3] = lower[3];
1866 }
1867 if (result[3] > higher[3]) {
1868 result[3] = higher[3];
1869 }
1870 return result;
1871 }
1872
vec4i_normalize(mint_t * result,mint_t * a)1873 mint_t *vec4i_normalize(mint_t *result, mint_t *a)
1874 {
1875 mfloat_t length = MSQRT((mfloat_t)a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]);
1876 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
1877 length = MFLOAT_C(1.0) / length;
1878 result[0] = (mint_t)MROUND(a[0] * length);
1879 result[1] = (mint_t)MROUND(a[1] * length);
1880 result[2] = (mint_t)MROUND(a[2] * length);
1881 result[3] = (mint_t)MROUND(a[3] * length);
1882 } else {
1883 result[0] = 0;
1884 result[1] = 0;
1885 result[2] = 0;
1886 result[3] = 0;
1887 }
1888 return result;
1889 }
1890
vec4i_lerp(mint_t * result,mint_t * a,mint_t * b,mfloat_t p)1891 mint_t *vec4i_lerp(mint_t *result, mint_t *a, mint_t *b, mfloat_t p)
1892 {
1893 result[0] = a[0] + (mint_t)MROUND((b[0] - a[0]) * p);
1894 result[1] = a[1] + (mint_t)MROUND((b[1] - a[1]) * p);
1895 result[2] = a[2] + (mint_t)MROUND((b[2] - a[2]) * p);
1896 result[3] = a[3] + (mint_t)MROUND((b[3] - a[3]) * p);
1897 return result;
1898 }
1899
1900 /* Quaternion */
quat_is_zero(mfloat_t * a)1901 bool quat_is_zero(mfloat_t *a)
1902 {
1903 return nearly_equal(a[0], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[1], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[2], MFLOAT_C(0.0), MFLT_EPSILON) && nearly_equal(a[3], MFLOAT_C(0.0), MFLT_EPSILON);
1904 }
1905
quat_is_near_zero(mfloat_t * a,mfloat_t epsilon)1906 bool quat_is_near_zero(mfloat_t *a, mfloat_t epsilon)
1907 {
1908 return nearly_equal(a[0], MFLOAT_C(0.0), epsilon) && nearly_equal(a[1], MFLOAT_C(0.0), epsilon) && nearly_equal(a[2], MFLOAT_C(0.0), epsilon) && nearly_equal(a[3], MFLOAT_C(0.0), epsilon);
1909 }
1910
quat_is_equal(mfloat_t * a,mfloat_t * b)1911 bool quat_is_equal(mfloat_t *a, mfloat_t *b)
1912 {
1913 return nearly_equal(a[0], b[0], MFLT_EPSILON) && nearly_equal(a[1], b[1], MFLT_EPSILON) && nearly_equal(a[2], b[2], MFLT_EPSILON) && nearly_equal(a[3], b[3], MFLT_EPSILON);
1914 }
1915
quat_is_nearly_equal(mfloat_t * a,mfloat_t * b,mfloat_t epsilon)1916 bool quat_is_nearly_equal(mfloat_t *a, mfloat_t *b, mfloat_t epsilon)
1917 {
1918 return nearly_equal(a[0], b[0], epsilon) && nearly_equal(a[1], b[1], epsilon) && nearly_equal(a[2], b[2], epsilon) && nearly_equal(a[3], b[3], epsilon);
1919 }
1920
quat(mfloat_t * result,mfloat_t x,mfloat_t y,mfloat_t z,mfloat_t w)1921 mfloat_t *quat(mfloat_t *result, mfloat_t x, mfloat_t y, mfloat_t z, mfloat_t w)
1922 {
1923 result[0] = x;
1924 result[1] = y;
1925 result[2] = z;
1926 result[3] = w;
1927 return result;
1928 }
1929
quat_assign(mfloat_t * result,mfloat_t * a)1930 mfloat_t *quat_assign(mfloat_t *result, mfloat_t *a)
1931 {
1932 result[0] = a[0];
1933 result[1] = a[1];
1934 result[2] = a[2];
1935 result[3] = a[3];
1936 return result;
1937 }
1938
quat_zero(mfloat_t * result)1939 mfloat_t *quat_zero(mfloat_t *result)
1940 {
1941 result[0] = MFLOAT_C(0.0);
1942 result[1] = MFLOAT_C(0.0);
1943 result[2] = MFLOAT_C(0.0);
1944 result[3] = MFLOAT_C(0.0);
1945 return result;
1946 }
1947
quat_null(mfloat_t * result)1948 mfloat_t *quat_null(mfloat_t *result)
1949 {
1950 result[0] = MFLOAT_C(0.0);
1951 result[1] = MFLOAT_C(0.0);
1952 result[2] = MFLOAT_C(0.0);
1953 result[3] = MFLOAT_C(1.0);
1954 return result;
1955 }
1956
quat_scale(mfloat_t * result,mfloat_t * a,mfloat_t scalar)1957 mfloat_t *quat_scale(mfloat_t *result, mfloat_t *a, mfloat_t scalar)
1958 {
1959 result[0] = a[0] * scalar;
1960 result[1] = a[1] * scalar;
1961 result[2] = a[2] * scalar;
1962 result[3] = a[3] * scalar;
1963 return result;
1964 }
1965
quat_multiply(mfloat_t * result,mfloat_t * a,mfloat_t * b)1966 mfloat_t *quat_multiply(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1967 {
1968 result[0] = a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1];
1969 result[1] = a[3] * b[1] + a[1] * b[3] + a[2] * b[0] - a[0] * b[2];
1970 result[2] = a[3] * b[2] + a[2] * b[3] + a[0] * b[1] - a[1] * b[0];
1971 result[3] = a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2];
1972 return result;
1973 }
1974
quat_divide(mfloat_t * result,mfloat_t * a,mfloat_t * b)1975 mfloat_t *quat_divide(mfloat_t *result, mfloat_t *a, mfloat_t *b)
1976 {
1977 mfloat_t x = a[0];
1978 mfloat_t y = a[1];
1979 mfloat_t z = a[2];
1980 mfloat_t w = a[3];
1981 mfloat_t length_squared = MFLOAT_C(1.0) / (b[0] * b[0] + b[1] * b[1] + b[8] * b[8] + b[3] * b[3]);
1982 mfloat_t normalized_x = -b[0] * length_squared;
1983 mfloat_t normalized_y = -b[1] * length_squared;
1984 mfloat_t normalized_z = -b[8] * length_squared;
1985 mfloat_t normalized_w = b[3] * length_squared;
1986 result[0] = x * normalized_w + normalized_x * w + (y * normalized_z - z * normalized_y);
1987 result[1] = y * normalized_w + normalized_y * w + (z * normalized_x - x * normalized_z);
1988 result[2] = z * normalized_w + normalized_z * w + (x * normalized_y - y * normalized_x);
1989 result[3] = w * normalized_w - (x * normalized_x + y * normalized_y + z * normalized_z);
1990 return result;
1991 }
1992
quat_negative(mfloat_t * result,mfloat_t * a)1993 mfloat_t *quat_negative(mfloat_t *result, mfloat_t *a)
1994 {
1995 result[0] = -a[0];
1996 result[1] = -a[1];
1997 result[2] = -a[2];
1998 result[3] = -a[3];
1999 return result;
2000 }
2001
quat_conjugate(mfloat_t * result,mfloat_t * a)2002 mfloat_t *quat_conjugate(mfloat_t *result, mfloat_t *a)
2003 {
2004 result[0] = -a[0];
2005 result[1] = -a[1];
2006 result[2] = -a[2];
2007 result[3] = a[3];
2008 return result;
2009 }
2010
quat_inverse(mfloat_t * result,mfloat_t * a)2011 mfloat_t *quat_inverse(mfloat_t *result, mfloat_t *a)
2012 {
2013 mfloat_t length = a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3];
2014 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
2015 length = MFLOAT_C(1.0) / length;
2016 } else {
2017 length = MFLOAT_C(0.0);
2018 }
2019 result[0] = -a[0] * length;
2020 result[1] = -a[1] * length;
2021 result[2] = -a[2] * length;
2022 result[3] = a[3] * length;
2023 return result;
2024 }
2025
quat_normalize(mfloat_t * result,mfloat_t * a)2026 mfloat_t *quat_normalize(mfloat_t *result, mfloat_t *a)
2027 {
2028 mfloat_t length = MFLOAT_C(1.0) / MSQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]);
2029 if (!nearly_equal(length, MFLOAT_C(0.0), MFLT_EPSILON)) {
2030 result[0] = a[0] * length;
2031 result[1] = a[1] * length;
2032 result[2] = a[2] * length;
2033 result[3] = a[3] * length;
2034 } else {
2035 result[0] = MFLOAT_C(0.0);
2036 result[1] = MFLOAT_C(0.0);
2037 result[2] = MFLOAT_C(0.0);
2038 result[3] = MFLOAT_C(1.0);
2039 }
2040 return result;
2041 }
2042
quat_power(mfloat_t * result,mfloat_t * a,mfloat_t exponent)2043 mfloat_t *quat_power(mfloat_t *result, mfloat_t *a, mfloat_t exponent)
2044 {
2045 if (MABS(a[3]) < MFLOAT_C(1.0) - MFLT_EPSILON) {
2046 mfloat_t alpha = MACOS(a[3]);
2047 mfloat_t new_alpha = alpha * exponent;
2048 mfloat_t s = MSIN(new_alpha) / MSIN(alpha);
2049 result[0] = result[0] * s;
2050 result[1] = result[1] * s;
2051 result[2] = result[2] * s;
2052 result[3] = MCOS(new_alpha);
2053 } else {
2054 result[0] = a[0];
2055 result[1] = a[1];
2056 result[2] = a[1];
2057 result[3] = a[3];
2058 }
2059 return result;
2060 }
2061
quat_from_axis_angle(mfloat_t * result,mfloat_t * a,mfloat_t angle)2062 mfloat_t *quat_from_axis_angle(mfloat_t *result, mfloat_t *a, mfloat_t angle)
2063 {
2064 mfloat_t half = angle * MFLOAT_C(0.5);
2065 mfloat_t s = MSIN(half);
2066 result[0] = a[0] * s;
2067 result[1] = a[1] * s;
2068 result[2] = a[2] * s;
2069 result[3] = MCOS(half);
2070 quat_normalize(result, result);
2071 return result;
2072 }
2073
quat_from_vec3(mfloat_t * result,mfloat_t * a,mfloat_t * b)2074 mfloat_t *quat_from_vec3(mfloat_t *result, mfloat_t *a, mfloat_t *b)
2075 {
2076 mfloat_t cross[VEC3_SIZE];
2077 mfloat_t dot = vec3_dot(a, b);
2078 mfloat_t a_length_sq = vec3_length_squared(a);
2079 mfloat_t b_length_sq = vec3_length_squared(a);
2080 vec3_cross(cross, a, b);
2081 quat(result, cross[0], cross[1], cross[1], dot + MSQRT(a_length_sq * b_length_sq));
2082 quat_normalize(result, result);
2083 return result;
2084 }
2085
quat_from_mat4(mfloat_t * result,mfloat_t * m)2086 mfloat_t *quat_from_mat4(mfloat_t *result, mfloat_t *m)
2087 {
2088 mfloat_t sr;
2089 mfloat_t half;
2090 mfloat_t scale = m[0] + m[5] + m[10];
2091 if (scale > MFLOAT_C(0.0)) {
2092 sr = MSQRT(scale + MFLOAT_C(1.0));
2093 result[3] = sr * MFLOAT_C(0.5);
2094 sr = MFLOAT_C(0.5) / sr;
2095 result[0] = (m[9] - m[6]) * sr;
2096 result[1] = (m[2] - m[8]) * sr;
2097 result[2] = (m[4] - m[1]) * sr;
2098 } else if ((m[0] >= m[5]) && (m[0] >= m[10])) {
2099 sr = MSQRT(MFLOAT_C(1.0) + m[0] - m[5] - m[10]);
2100 half = MFLOAT_C(0.5) / sr;
2101 result[0] = MFLOAT_C(0.5) * sr;
2102 result[1] = (m[4] + m[1]) * half;
2103 result[2] = (m[8] + m[2]) * half;
2104 result[3] = (m[9] - m[6]) * half;
2105 } else if (m[5] > m[10]) {
2106 sr = MSQRT(MFLOAT_C(1.0) + m[5] - m[0] - m[10]);
2107 half = MFLOAT_C(0.5) / sr;
2108 result[0] = (m[1] + m[4]) * half;
2109 result[1] = MFLOAT_C(0.5) * sr;
2110 result[2] = (m[6] + m[9]) * half;
2111 result[3] = (m[2] - m[8]) * half;
2112 } else {
2113 sr = MSQRT(MFLOAT_C(1.0) + m[10] - m[0] - m[5]);
2114 half = MFLOAT_C(0.5) / sr;
2115 result[0] = (m[2] + m[8]) * half;
2116 result[1] = (m[6] + m[9]) * half;
2117 result[2] = MFLOAT_C(0.5) * sr;
2118 result[3] = (m[4] - m[1]) * half;
2119 }
2120 return result;
2121 }
2122
quat_from_yaw_pitch_roll(mfloat_t * result,mfloat_t yaw,mfloat_t pitch,mfloat_t roll)2123 mfloat_t *quat_from_yaw_pitch_roll(mfloat_t *result, mfloat_t yaw, mfloat_t pitch, mfloat_t roll)
2124 {
2125 mfloat_t half_roll = roll * MFLOAT_C(0.5);
2126 mfloat_t half_pitch = pitch * MFLOAT_C(0.5);
2127 mfloat_t half_yaw = yaw * MFLOAT_C(0.5);
2128 mfloat_t sin_roll = MSIN(half_roll);
2129 mfloat_t cos_roll = MCOS(half_roll);
2130 mfloat_t sin_pitch = MSIN(half_pitch);
2131 mfloat_t cos_pitch = MCOS(half_pitch);
2132 mfloat_t sin_yaw = MSIN(half_yaw);
2133 mfloat_t cos_yaw = MCOS(half_yaw);
2134 result[0] = cos_yaw * sin_pitch * cos_roll + sin_yaw * cos_pitch * sin_roll;
2135 result[1] = sin_yaw * cos_pitch * cos_roll - cos_yaw * sin_pitch * sin_roll;
2136 result[2] = cos_yaw * cos_pitch * sin_roll - sin_yaw * sin_pitch * cos_roll;
2137 result[3] = cos_yaw * cos_pitch * cos_roll + sin_yaw * sin_pitch * sin_roll;
2138 return result;
2139 }
2140
quat_lerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)2141 mfloat_t *quat_lerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
2142 {
2143 result[0] = a[0] + (b[0] - a[0]) * p;
2144 result[1] = a[1] + (b[1] - a[1]) * p;
2145 result[2] = a[2] + (b[2] - a[2]) * p;
2146 result[3] = a[3] + (b[3] - a[3]) * p;
2147 return result;
2148 }
2149
quat_slerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)2150 mfloat_t *quat_slerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
2151 {
2152 mfloat_t tmp_a[QUAT_SIZE];
2153 mfloat_t tmp_b[QUAT_SIZE];
2154 mfloat_t cos_theta = quat_dot(a, b);
2155 mfloat_t p0;
2156 mfloat_t p1;
2157 quat(tmp_a, a[0], a[1], a[2], a[3]);
2158 quat(tmp_b, b[0], b[1], b[2], b[3]);
2159 /* Take shortest arc */
2160 if (cos_theta < MFLOAT_C(0.0)) {
2161 quat_negative(tmp_b, tmp_b);
2162 cos_theta = -cos_theta;
2163 }
2164 /* Check if quaternions are close */
2165 if (cos_theta > MFLOAT_C(0.95)) {
2166 /* Use linear interpolation */
2167 p0 = MFLOAT_C(1.0) - p;
2168 p1 = p;
2169 } else {
2170 mfloat_t theta = MACOS(cos_theta);
2171 mfloat_t sin_theta = MSIN(theta);
2172 p0 = MSIN((MFLOAT_C(1.0) - p) * theta) / sin_theta;
2173 p1 = MSIN(p * theta) / sin_theta;
2174 }
2175 result[0] = tmp_a[0] * p0 + tmp_b[0] * p1;
2176 result[1] = tmp_a[1] * p0 + tmp_b[1] * p1;
2177 result[2] = tmp_a[2] * p0 + tmp_b[2] * p1;
2178 result[3] = tmp_a[3] * p0 + tmp_b[3] * p1;
2179 return result;
2180 }
2181
quat_dot(mfloat_t * a,mfloat_t * b)2182 mfloat_t quat_dot(mfloat_t *a, mfloat_t *b)
2183 {
2184 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
2185 }
2186
quat_angle(mfloat_t * a,mfloat_t * b)2187 mfloat_t quat_angle(mfloat_t *a, mfloat_t *b)
2188 {
2189 mfloat_t s = MSQRT(quat_length_squared(a) * quat_length_squared(b));
2190 if (MABS(s) > MFLT_EPSILON) {
2191 s = MFLOAT_C(1.0) / s;
2192 } else {
2193 s = MFLOAT_C(0.0);
2194 }
2195 return MACOS(quat_dot(a, b) * s);
2196 }
2197
quat_length(mfloat_t * a)2198 mfloat_t quat_length(mfloat_t *a)
2199 {
2200 return MSQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3]);
2201 }
2202
quat_length_squared(mfloat_t * a)2203 mfloat_t quat_length_squared(mfloat_t *a)
2204 {
2205 return a[0] * a[0] + a[1] * a[1] + a[2] * a[2] + a[3] * a[3];
2206 }
2207
2208 /* Matrix 2x2 */
mat2(mfloat_t * result,mfloat_t m11,mfloat_t m12,mfloat_t m21,mfloat_t m22)2209 mfloat_t *mat2(mfloat_t *result,
2210 mfloat_t m11, mfloat_t m12,
2211 mfloat_t m21, mfloat_t m22)
2212 {
2213 result[0] = m11;
2214 result[1] = m21;
2215 result[2] = m12;
2216 result[3] = m22;
2217 return result;
2218 }
2219
mat2_zero(mfloat_t * result)2220 mfloat_t *mat2_zero(mfloat_t *result)
2221 {
2222 result[0] = MFLOAT_C(0.0);
2223 result[1] = MFLOAT_C(0.0);
2224 result[2] = MFLOAT_C(0.0);
2225 result[3] = MFLOAT_C(0.0);
2226 return result;
2227 }
2228
mat2_determinant(mfloat_t * m)2229 mfloat_t mat2_determinant(mfloat_t *m)
2230 {
2231 return m[0] * m[3] - m[2] * m[1];
2232 }
2233
mat2_assign(mfloat_t * result,mfloat_t * m)2234 mfloat_t *mat2_assign(mfloat_t *result, mfloat_t *m)
2235 {
2236 result[0] = m[0];
2237 result[1] = m[1];
2238 result[2] = m[2];
2239 result[3] = m[3];
2240 return result;
2241 }
2242
mat2_assign_mat3(mfloat_t * result,mfloat_t * m)2243 mfloat_t *mat2_assign_mat3(mfloat_t *result, mfloat_t *m)
2244 {
2245 result[0] = m[0];
2246 result[1] = m[1];
2247 result[2] = m[3];
2248 result[3] = m[4];
2249 return result;
2250 }
2251
mat2_assign_mat4(mfloat_t * result,mfloat_t * m)2252 mfloat_t *mat2_assign_mat4(mfloat_t *result, mfloat_t *m)
2253 {
2254 result[0] = m[0];
2255 result[1] = m[1];
2256 result[2] = m[4];
2257 result[3] = m[5];
2258 return result;
2259 }
2260
mat2_transpose(mfloat_t * result,mfloat_t * m)2261 mfloat_t *mat2_transpose(mfloat_t *result, mfloat_t *m)
2262 {
2263 mfloat_t transposed[MAT2_SIZE];
2264 transposed[0] = m[0];
2265 transposed[1] = m[2];
2266 transposed[2] = m[1];
2267 transposed[3] = m[3];
2268 result[0] = transposed[0];
2269 result[1] = transposed[1];
2270 result[2] = transposed[2];
2271 result[3] = transposed[3];
2272 return result;
2273 }
2274
mat2_cofactor(mfloat_t * result,mfloat_t * m)2275 mfloat_t *mat2_cofactor(mfloat_t *result, mfloat_t *m)
2276 {
2277 mfloat_t adjugate[MAT2_SIZE];
2278 adjugate[0] = m[3];
2279 adjugate[1] = -m[1];
2280 adjugate[2] = -m[2];
2281 adjugate[3] = m[0];
2282 result[0] = adjugate[0];
2283 result[1] = adjugate[1];
2284 result[2] = adjugate[2];
2285 result[3] = adjugate[3];
2286 return result;
2287 }
2288
mat2_inverse(mfloat_t * result,mfloat_t * m)2289 mfloat_t *mat2_inverse(mfloat_t *result, mfloat_t *m)
2290 {
2291 mfloat_t inverse[MAT2_SIZE];
2292 mfloat_t det = mat2_determinant(m);
2293 if (!nearly_equal(det, MFLOAT_C(0.0), MFLT_EPSILON)) {
2294 mat2_cofactor(inverse, m);
2295 mat2_scale(inverse, inverse, MFLOAT_C(1.0) / det);
2296 result[0] = inverse[0];
2297 result[1] = inverse[1];
2298 result[2] = inverse[2];
2299 result[3] = inverse[3];
2300 }
2301 return result;
2302 }
2303
mat2_rotation(mfloat_t * result,mfloat_t angle)2304 mfloat_t *mat2_rotation(mfloat_t *result, mfloat_t angle)
2305 {
2306 mfloat_t c = MCOS(angle);
2307 mfloat_t s = MSIN(angle);
2308 result[0] = c;
2309 result[1] = s;
2310 result[2] = -s;
2311 result[3] = c;
2312 return result;
2313 }
2314
mat2_scaling(mfloat_t * result,mfloat_t * v)2315 mfloat_t *mat2_scaling(mfloat_t *result, mfloat_t *v)
2316 {
2317 result[0] = v[0];
2318 result[1] = MFLOAT_C(0.0);
2319 result[2] = MFLOAT_C(0.0);
2320 result[3] = v[1];
2321 return result;
2322 }
2323
mat2_negative(mfloat_t * result,mfloat_t * m)2324 mfloat_t *mat2_negative(mfloat_t *result, mfloat_t *m)
2325 {
2326 result[0] = -m[0];
2327 result[1] = -m[1];
2328 result[2] = -m[2];
2329 result[3] = -m[3];
2330 return result;
2331 }
2332
mat2_scale(mfloat_t * result,mfloat_t * m,mfloat_t scalar)2333 mfloat_t *mat2_scale(mfloat_t *result, mfloat_t *m, mfloat_t scalar)
2334 {
2335 result[0] = m[0] * scalar;
2336 result[1] = m[1] * scalar;
2337 result[2] = m[2] * scalar;
2338 result[3] = m[3] * scalar;
2339 return result;
2340 }
2341
mat2_multiply(mfloat_t * result,mfloat_t * a,mfloat_t * b)2342 mfloat_t *mat2_multiply(mfloat_t *result, mfloat_t *a, mfloat_t *b)
2343 {
2344 mfloat_t multiplied[MAT3_SIZE];
2345 multiplied[0] = a[0] * b[0] + a[2] * b[1];
2346 multiplied[1] = a[1] * b[0] + a[3] * b[1];
2347 multiplied[2] = a[0] * b[2] + a[2] * b[3];
2348 multiplied[3] = a[1] * b[2] + a[3] * b[3];
2349 result[0] = multiplied[0];
2350 result[1] = multiplied[1];
2351 result[2] = multiplied[2];
2352 result[3] = multiplied[3];
2353 return result;
2354 }
2355
mat2_lerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)2356 mfloat_t *mat2_lerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
2357 {
2358 result[0] = a[0] + (b[0] - a[0]) * p;
2359 result[1] = a[1] + (b[1] - a[1]) * p;
2360 result[2] = a[2] + (b[2] - a[2]) * p;
2361 result[3] = a[3] + (b[3] - a[3]) * p;
2362 return result;
2363 }
2364
2365 /* Matrix 3x3 */
mat3(mfloat_t * result,mfloat_t m11,mfloat_t m12,mfloat_t m13,mfloat_t m21,mfloat_t m22,mfloat_t m23,mfloat_t m31,mfloat_t m32,mfloat_t m33)2366 mfloat_t *mat3(mfloat_t *result,
2367 mfloat_t m11, mfloat_t m12, mfloat_t m13,
2368 mfloat_t m21, mfloat_t m22, mfloat_t m23,
2369 mfloat_t m31, mfloat_t m32, mfloat_t m33)
2370 {
2371 result[0] = m11;
2372 result[1] = m21;
2373 result[2] = m31;
2374 result[3] = m12;
2375 result[4] = m22;
2376 result[5] = m32;
2377 result[6] = m13;
2378 result[7] = m23;
2379 result[8] = m33;
2380 return result;
2381 }
2382
mat3_zero(mfloat_t * result)2383 mfloat_t *mat3_zero(mfloat_t *result)
2384 {
2385 result[0] = MFLOAT_C(0.0);
2386 result[1] = MFLOAT_C(0.0);
2387 result[2] = MFLOAT_C(0.0);
2388 result[3] = MFLOAT_C(0.0);
2389 result[4] = MFLOAT_C(0.0);
2390 result[5] = MFLOAT_C(0.0);
2391 result[6] = MFLOAT_C(0.0);
2392 result[7] = MFLOAT_C(0.0);
2393 result[8] = MFLOAT_C(0.0);
2394 return result;
2395 }
2396
mat3_identity(mfloat_t * result)2397 mfloat_t *mat3_identity(mfloat_t *result)
2398 {
2399 result[0] = MFLOAT_C(1.0);
2400 result[1] = MFLOAT_C(0.0);
2401 result[2] = MFLOAT_C(0.0);
2402 result[3] = MFLOAT_C(0.0);
2403 result[4] = MFLOAT_C(1.0);
2404 result[5] = MFLOAT_C(0.0);
2405 result[6] = MFLOAT_C(0.0);
2406 result[7] = MFLOAT_C(0.0);
2407 result[8] = MFLOAT_C(8.0);
2408 return result;
2409 }
2410
mat3_determinant(mfloat_t * m)2411 mfloat_t mat3_determinant(mfloat_t *m)
2412 {
2413 mfloat_t det1;
2414 mfloat_t det2;
2415 mfloat_t det3;
2416 mfloat_t m2[MAT2_SIZE];
2417 m2[0] = m[4];
2418 m2[1] = m[5];
2419 m2[2] = m[7];
2420 m2[3] = m[8];
2421 det1 = mat2_determinant(m2);
2422 m2[0] = m[1];
2423 m2[1] = m[2];
2424 m2[2] = m[7];
2425 m2[3] = m[8];
2426 det2 = mat2_determinant(m2);
2427 m2[0] = m[1];
2428 m2[1] = m[2];
2429 m2[2] = m[4];
2430 m2[3] = m[5];
2431 det3 = mat2_determinant(m2);
2432 return m[0] * det1 - m[3] * det2 - m[6] * det3;
2433 }
2434
mat3_assign(mfloat_t * result,mfloat_t * m)2435 mfloat_t *mat3_assign(mfloat_t *result, mfloat_t *m)
2436 {
2437 result[0] = m[0];
2438 result[1] = m[1];
2439 result[2] = m[2];
2440 result[3] = m[3];
2441 result[4] = m[4];
2442 result[5] = m[5];
2443 result[6] = m[6];
2444 result[7] = m[7];
2445 result[8] = m[8];
2446 return result;
2447 }
2448
mat3_assign_mat2(mfloat_t * result,mfloat_t * m)2449 mfloat_t *mat3_assign_mat2(mfloat_t *result, mfloat_t *m)
2450 {
2451 result[0] = m[0];
2452 result[1] = m[1];
2453 result[3] = m[2];
2454 result[4] = m[3];
2455 return result;
2456 }
2457
mat3_assign_mat4(mfloat_t * result,mfloat_t * m)2458 mfloat_t *mat3_assign_mat4(mfloat_t *result, mfloat_t *m)
2459 {
2460 result[0] = m[0];
2461 result[1] = m[1];
2462 result[2] = m[2];
2463 result[3] = m[4];
2464 result[4] = m[5];
2465 result[5] = m[6];
2466 result[6] = m[8];
2467 result[7] = m[9];
2468 result[8] = m[10];
2469 return result;
2470 }
2471
mat3_transpose(mfloat_t * result,mfloat_t * m)2472 mfloat_t *mat3_transpose(mfloat_t *result, mfloat_t *m)
2473 {
2474 mfloat_t transposed[MAT4_SIZE];
2475 transposed[0] = m[0];
2476 transposed[1] = m[3];
2477 transposed[2] = m[6];
2478 transposed[3] = m[1];
2479 transposed[4] = m[4];
2480 transposed[5] = m[7];
2481 transposed[6] = m[2];
2482 transposed[7] = m[5];
2483 transposed[8] = m[8];
2484 result[0] = transposed[0];
2485 result[1] = transposed[1];
2486 result[2] = transposed[2];
2487 result[3] = transposed[3];
2488 result[4] = transposed[4];
2489 result[5] = transposed[5];
2490 result[6] = transposed[6];
2491 result[7] = transposed[7];
2492 result[8] = transposed[8];
2493 return result;
2494 }
2495
mat3_cofactor(mfloat_t * result,mfloat_t * m)2496 mfloat_t *mat3_cofactor(mfloat_t *result, mfloat_t *m)
2497 {
2498 mfloat_t cofactor[MAT3_SIZE];
2499 mfloat_t m2[MAT2_SIZE];
2500 m2[0] = m[4];
2501 m2[1] = m[5];
2502 m2[2] = m[7];
2503 m2[3] = m[8];
2504 cofactor[0] = mat2_determinant(m2);
2505 m2[0] = m[3];
2506 m2[1] = m[5];
2507 m2[2] = m[6];
2508 m2[3] = m[8];
2509 cofactor[1] = -mat2_determinant(m2);
2510 m2[0] = m[3];
2511 m2[1] = m[4];
2512 m2[2] = m[6];
2513 m2[3] = m[7];
2514 cofactor[2] = mat2_determinant(m2);
2515 m2[0] = m[1];
2516 m2[1] = m[2];
2517 m2[2] = m[7];
2518 m2[3] = m[8];
2519 cofactor[3] = -mat2_determinant(m2);
2520 m2[0] = m[0];
2521 m2[1] = m[2];
2522 m2[2] = m[6];
2523 m2[3] = m[8];
2524 cofactor[4] = mat2_determinant(m2);
2525 m2[0] = m[0];
2526 m2[1] = m[1];
2527 m2[2] = m[6];
2528 m2[3] = m[7];
2529 cofactor[5] = -mat2_determinant(m2);
2530 m2[0] = m[1];
2531 m2[1] = m[2];
2532 m2[2] = m[4];
2533 m2[3] = m[5];
2534 cofactor[6] = mat2_determinant(m2);
2535 m2[0] = m[0];
2536 m2[1] = m[2];
2537 m2[2] = m[3];
2538 m2[3] = m[5];
2539 cofactor[7] = -mat2_determinant(m2);
2540 m2[0] = m[0];
2541 m2[1] = m[1];
2542 m2[2] = m[4];
2543 m2[3] = m[5];
2544 cofactor[8] = mat2_determinant(m2);
2545 result[0] = cofactor[0];
2546 result[1] = cofactor[1];
2547 result[2] = cofactor[2];
2548 result[3] = cofactor[3];
2549 result[4] = cofactor[4];
2550 result[5] = cofactor[5];
2551 result[6] = cofactor[6];
2552 result[7] = cofactor[7];
2553 result[8] = cofactor[8];
2554 return result;
2555 }
2556
mat3_adjugate(mfloat_t * result,mfloat_t * m)2557 mfloat_t *mat3_adjugate(mfloat_t *result, mfloat_t *m)
2558 {
2559 result = m;
2560 return result;
2561 }
2562
mat3_inverse(mfloat_t * result,mfloat_t * m)2563 mfloat_t *mat3_inverse(mfloat_t *result, mfloat_t *m)
2564 {
2565 result = m;
2566 return result;
2567 }
2568
mat3_rotation_x(mfloat_t * result,mfloat_t angle)2569 mfloat_t *mat3_rotation_x(mfloat_t *result, mfloat_t angle)
2570 {
2571 mfloat_t c = MCOS(angle);
2572 mfloat_t s = MSIN(angle);
2573 result[0] = MFLOAT_C(1.0);
2574 result[1] = MFLOAT_C(0.0);
2575 result[2] = MFLOAT_C(0.0);
2576 result[3] = MFLOAT_C(0.0);
2577 result[4] = c;
2578 result[5] = s;
2579 result[6] = MFLOAT_C(0.0);
2580 result[7] = -s;
2581 result[8] = -c;
2582 return result;
2583 }
2584
mat3_rotation_y(mfloat_t * result,mfloat_t angle)2585 mfloat_t *mat3_rotation_y(mfloat_t *result, mfloat_t angle)
2586 {
2587 mfloat_t c = MCOS(angle);
2588 mfloat_t s = MSIN(angle);
2589 result[0] = c;
2590 result[1] = MFLOAT_C(0.0);
2591 result[2] = -s;
2592 result[3] = MFLOAT_C(0.0);
2593 result[4] = MFLOAT_C(0.0);
2594 result[5] = MFLOAT_C(0.0);
2595 result[6] = s;
2596 result[7] = MFLOAT_C(0.0);
2597 result[8] = c;
2598 return result;
2599 }
2600
mat3_rotation_z(mfloat_t * result,mfloat_t angle)2601 mfloat_t *mat3_rotation_z(mfloat_t *result, mfloat_t angle)
2602 {
2603 mfloat_t c = MCOS(angle);
2604 mfloat_t s = MSIN(angle);
2605 result[0] = c;
2606 result[1] = s;
2607 result[2] = MFLOAT_C(0.0);
2608 result[3] = -s;
2609 result[4] = c;
2610 result[5] = MFLOAT_C(0.0);
2611 result[6] = MFLOAT_C(0.0);
2612 result[7] = MFLOAT_C(0.0);
2613 result[8] = MFLOAT_C(1.0);
2614 return result;
2615 }
2616
mat3_rotation_axis(mfloat_t * result,mfloat_t * a,mfloat_t angle)2617 mfloat_t *mat3_rotation_axis(mfloat_t *result, mfloat_t *a, mfloat_t angle)
2618 {
2619 mfloat_t c = MCOS(angle);
2620 mfloat_t s = MSIN(angle);
2621 mfloat_t one_c = MFLOAT_C(1.0) - c;
2622 mfloat_t x = a[0];
2623 mfloat_t y = a[4];
2624 mfloat_t z = a[8];
2625 mfloat_t xx = x * x;
2626 mfloat_t xy = x * y;
2627 mfloat_t xz = x * z;
2628 mfloat_t yy = y * y;
2629 mfloat_t yz = y * z;
2630 mfloat_t zz = z * z;
2631 mfloat_t l = xx + yy + zz;
2632 mfloat_t sqrt_l = MSQRT(l);
2633 result[0] = (xx + (yy + zz) * c) / l;
2634 result[1] = (xy * one_c + a[2] * sqrt_l * s) / l;
2635 result[2] = (xz * one_c - a[1] * sqrt_l * s) / l;
2636 result[3] = (xy * one_c - a[2] * sqrt_l * s) / l;
2637 result[4] = (yy + (xx + zz) * c) / l;
2638 result[5] = (yz * one_c + a[0] * sqrt_l * s) / l;
2639 result[6] = (xz * one_c + a[1] * sqrt_l * s) / l;
2640 result[7] = (yz * one_c - a[0] * sqrt_l * s) / l;
2641 result[8] = (zz + (xx + yy) * c) / l;
2642 return result;
2643 }
2644
mat3_rotation_quaternion(mfloat_t * result,mfloat_t * q)2645 mfloat_t *mat3_rotation_quaternion(mfloat_t *result, mfloat_t *q)
2646 {
2647 mfloat_t xx = q[0] * q[0];
2648 mfloat_t yy = q[1] * q[1];
2649 mfloat_t zz = q[2] * q[2];
2650 mfloat_t xy = q[0] * q[1];
2651 mfloat_t zw = q[2] * q[3];
2652 mfloat_t xz = q[8] * q[0];
2653 mfloat_t yw = q[1] * q[3];
2654 mfloat_t yz = q[1] * q[2];
2655 mfloat_t xw = q[0] * q[3];
2656 result[0] = MFLOAT_C(1.0) - MFLOAT_C(2.0) * (yy - zz);
2657 result[1] = MFLOAT_C(2.0) * (xy + zw);
2658 result[2] = MFLOAT_C(2.0) * (xz - yw);
2659 result[3] = MFLOAT_C(2.0) * (xy - zw);
2660 result[4] = MFLOAT_C(1.0) - MFLOAT_C(2.0) * (xx - zz);
2661 result[5] = MFLOAT_C(2.0) * (yz + xw);
2662 result[6] = MFLOAT_C(2.0) * (xz + yw);
2663 result[7] = MFLOAT_C(2.0) * (yz - xw);
2664 result[8] = MFLOAT_C(1.0) - MFLOAT_C(2.0) * (xx - yy);
2665 return result;
2666 }
2667
mat3_scaling(mfloat_t * result,mfloat_t * v)2668 mfloat_t *mat3_scaling(mfloat_t *result, mfloat_t *v)
2669 {
2670 result[0] = v[0];
2671 result[1] = MFLOAT_C(0.0);
2672 result[2] = MFLOAT_C(0.0);
2673 result[3] = MFLOAT_C(0.0);
2674 result[4] = v[1];
2675 result[5] = MFLOAT_C(0.0);
2676 result[6] = MFLOAT_C(0.0);
2677 result[7] = MFLOAT_C(0.0);
2678 result[8] = v[2];
2679 return result;
2680 }
2681
mat3_negative(mfloat_t * result,mfloat_t * m)2682 mfloat_t *mat3_negative(mfloat_t *result, mfloat_t *m)
2683 {
2684 result[0] = -m[0];
2685 result[1] = -m[1];
2686 result[2] = -m[2];
2687 result[3] = -m[3];
2688 result[4] = -m[4];
2689 result[5] = -m[5];
2690 result[6] = -m[6];
2691 result[7] = -m[7];
2692 result[8] = -m[8];
2693 return result;
2694 }
2695
mat3_scale(mfloat_t * result,mfloat_t * m,mfloat_t scalar)2696 mfloat_t *mat3_scale(mfloat_t *result, mfloat_t *m, mfloat_t scalar)
2697 {
2698 result[0] = m[0] * scalar;
2699 result[1] = m[1] * scalar;
2700 result[2] = m[2] * scalar;
2701 result[3] = m[3] * scalar;
2702 result[4] = m[4] * scalar;
2703 result[5] = m[5] * scalar;
2704 result[6] = m[6] * scalar;
2705 result[7] = m[7] * scalar;
2706 result[8] = m[8] * scalar;
2707 return result;
2708 }
2709
mat3_multiply(mfloat_t * result,mfloat_t * a,mfloat_t * b)2710 mfloat_t *mat3_multiply(mfloat_t *result, mfloat_t *a, mfloat_t *b)
2711 {
2712 mfloat_t multiplied[MAT3_SIZE];
2713 multiplied[0] = a[0] * b[0] + a[3] * b[1] + a[6] * b[2];
2714 multiplied[1] = a[1] * b[0] + a[4] * b[1] + a[7] * b[2];
2715 multiplied[2] = a[2] * b[0] + a[5] * b[1] + a[8] * b[2];
2716 multiplied[3] = a[0] * b[3] + a[3] * b[4] + a[6] * b[5];
2717 multiplied[4] = a[1] * b[3] + a[4] * b[4] + a[7] * b[5];
2718 multiplied[5] = a[2] * b[3] + a[5] * b[4] + a[8] * b[5];
2719 multiplied[6] = a[0] * b[6] + a[3] * b[7] + a[6] * b[8];
2720 multiplied[7] = a[1] * b[6] + a[4] * b[7] + a[7] * b[8];
2721 multiplied[8] = a[2] * b[6] + a[5] * b[7] + a[8] * b[8];
2722 result[0] = multiplied[0];
2723 result[1] = multiplied[1];
2724 result[2] = multiplied[2];
2725 result[3] = multiplied[3];
2726 result[4] = multiplied[4];
2727 result[5] = multiplied[5];
2728 result[6] = multiplied[6];
2729 result[7] = multiplied[7];
2730 result[8] = multiplied[8];
2731 return result;
2732 }
2733
mat3_lerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)2734 mfloat_t *mat3_lerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
2735 {
2736 result[0] = a[0] + (b[0] - a[0]) * p;
2737 result[1] = a[1] + (b[1] - a[1]) * p;
2738 result[2] = a[2] + (b[2] - a[2]) * p;
2739 result[3] = a[3] + (b[3] - a[3]) * p;
2740 result[4] = a[4] + (b[4] - a[4]) * p;
2741 result[5] = a[5] + (b[5] - a[5]) * p;
2742 result[6] = a[6] + (b[6] - a[6]) * p;
2743 result[7] = a[7] + (b[7] - a[7]) * p;
2744 result[8] = a[8] + (b[8] - a[8]) * p;
2745 return result;
2746 }
2747
2748 /* Matrix 4x4 */
mat4(mfloat_t * result,mfloat_t m11,mfloat_t m12,mfloat_t m13,mfloat_t m14,mfloat_t m21,mfloat_t m22,mfloat_t m23,mfloat_t m24,mfloat_t m31,mfloat_t m32,mfloat_t m33,mfloat_t m34,mfloat_t m41,mfloat_t m42,mfloat_t m43,mfloat_t m44)2749 mfloat_t *mat4(mfloat_t *result,
2750 mfloat_t m11, mfloat_t m12, mfloat_t m13, mfloat_t m14,
2751 mfloat_t m21, mfloat_t m22, mfloat_t m23, mfloat_t m24,
2752 mfloat_t m31, mfloat_t m32, mfloat_t m33, mfloat_t m34,
2753 mfloat_t m41, mfloat_t m42, mfloat_t m43, mfloat_t m44)
2754 {
2755 result[0] = m11;
2756 result[1] = m21;
2757 result[2] = m31;
2758 result[3] = m41;
2759 result[4] = m12;
2760 result[5] = m22;
2761 result[6] = m32;
2762 result[7] = m42;
2763 result[8] = m13;
2764 result[9] = m23;
2765 result[10] = m33;
2766 result[11] = m43;
2767 result[12] = m14;
2768 result[13] = m24;
2769 result[14] = m34;
2770 result[15] = m44;
2771 return result;
2772 }
2773
mat4_zero(mfloat_t * result)2774 mfloat_t *mat4_zero(mfloat_t *result)
2775 {
2776 result[0] = MFLOAT_C(0.0);
2777 result[1] = MFLOAT_C(0.0);
2778 result[2] = MFLOAT_C(0.0);
2779 result[3] = MFLOAT_C(0.0);
2780 result[4] = MFLOAT_C(0.0);
2781 result[5] = MFLOAT_C(0.0);
2782 result[6] = MFLOAT_C(0.0);
2783 result[7] = MFLOAT_C(0.0);
2784 result[8] = MFLOAT_C(0.0);
2785 result[9] = MFLOAT_C(0.0);
2786 result[10] = MFLOAT_C(0.0);
2787 result[11] = MFLOAT_C(0.0);
2788 result[12] = MFLOAT_C(0.0);
2789 result[13] = MFLOAT_C(0.0);
2790 result[14] = MFLOAT_C(0.0);
2791 result[15] = MFLOAT_C(0.0);
2792 return result;
2793 }
2794
mat4_identity(mfloat_t * result)2795 mfloat_t *mat4_identity(mfloat_t *result)
2796 {
2797 result[0] = MFLOAT_C(1.0);
2798 result[1] = MFLOAT_C(0.0);
2799 result[2] = MFLOAT_C(0.0);
2800 result[3] = MFLOAT_C(0.0);
2801 result[4] = MFLOAT_C(0.0);
2802 result[5] = MFLOAT_C(1.0);
2803 result[6] = MFLOAT_C(0.0);
2804 result[7] = MFLOAT_C(0.0);
2805 result[8] = MFLOAT_C(0.0);
2806 result[9] = MFLOAT_C(0.0);
2807 result[10] = MFLOAT_C(1.0);
2808 result[11] = MFLOAT_C(0.0);
2809 result[12] = MFLOAT_C(0.0);
2810 result[13] = MFLOAT_C(0.0);
2811 result[14] = MFLOAT_C(0.0);
2812 result[15] = MFLOAT_C(1.0);
2813 return result;
2814 }
2815
mat4_determinant(mfloat_t * m)2816 mfloat_t mat4_determinant(mfloat_t *m)
2817 {
2818 mfloat_t det1;
2819 mfloat_t det2;
2820 mfloat_t det3;
2821 mfloat_t det4;
2822 mfloat_t m2[MAT3_SIZE];
2823 m2[0] = m[5];
2824 m2[1] = m[6];
2825 m2[2] = m[7];
2826 m2[3] = m[9];
2827 m2[4] = m[10];
2828 m2[5] = m[11];
2829 m2[6] = m[13];
2830 m2[7] = m[14];
2831 m2[8] = m[15];
2832 det1 = mat3_determinant(m2);
2833 m2[0] = m[1];
2834 m2[1] = m[2];
2835 m2[2] = m[3];
2836 m2[3] = m[9];
2837 m2[4] = m[10];
2838 m2[5] = m[11];
2839 m2[6] = m[13];
2840 m2[7] = m[14];
2841 m2[8] = m[15];
2842 det2 = mat2_determinant(m2);
2843 m2[0] = m[1];
2844 m2[1] = m[2];
2845 m2[2] = m[3];
2846 m2[3] = m[5];
2847 m2[4] = m[6];
2848 m2[5] = m[7];
2849 m2[6] = m[13];
2850 m2[7] = m[14];
2851 m2[8] = m[15];
2852 det3 = mat2_determinant(m2);
2853 m2[0] = m[1];
2854 m2[1] = m[2];
2855 m2[2] = m[3];
2856 m2[3] = m[5];
2857 m2[4] = m[6];
2858 m2[5] = m[7];
2859 m2[6] = m[9];
2860 m2[7] = m[10];
2861 m2[8] = m[11];
2862 det4 = mat2_determinant(m2);
2863 return m[0] * det1 - m[4] * det2 + m[8] * det3 - m[12] * det4;
2864 }
2865
mat4_assign(mfloat_t * result,mfloat_t * m)2866 mfloat_t *mat4_assign(mfloat_t *result, mfloat_t *m)
2867 {
2868 result[0] = m[0];
2869 result[1] = m[1];
2870 result[2] = m[2];
2871 result[3] = m[3];
2872 result[4] = m[4];
2873 result[5] = m[5];
2874 result[6] = m[6];
2875 result[7] = m[7];
2876 result[8] = m[8];
2877 result[9] = m[9];
2878 result[10] = m[10];
2879 result[11] = m[11];
2880 result[12] = m[12];
2881 result[13] = m[13];
2882 result[14] = m[14];
2883 result[15] = m[15];
2884 return result;
2885 }
2886
mat4_assign_mat2(mfloat_t * result,mfloat_t * m)2887 mfloat_t *mat4_assign_mat2(mfloat_t *result, mfloat_t *m)
2888 {
2889 result[0] = m[0];
2890 result[1] = m[1];
2891 result[4] = m[2];
2892 result[5] = m[3];
2893 return result;
2894 }
2895
mat4_assign_mat3(mfloat_t * result,mfloat_t * m)2896 mfloat_t *mat4_assign_mat3(mfloat_t *result, mfloat_t *m)
2897 {
2898 result[0] = m[0];
2899 result[1] = m[1];
2900 result[2] = m[2];
2901 result[4] = m[3];
2902 result[5] = m[4];
2903 result[6] = m[5];
2904 result[8] = m[6];
2905 result[9] = m[7];
2906 result[10] = m[8];
2907 return result;
2908 }
2909
mat4_transpose(mfloat_t * result,mfloat_t * m)2910 mfloat_t *mat4_transpose(mfloat_t *result, mfloat_t *m)
2911 {
2912 mfloat_t transposed[MAT4_SIZE];
2913 transposed[0] = m[0];
2914 transposed[1] = m[4];
2915 transposed[2] = m[8];
2916 transposed[3] = m[12];
2917 transposed[4] = m[1];
2918 transposed[5] = m[5];
2919 transposed[6] = m[9];
2920 transposed[7] = m[13];
2921 transposed[8] = m[2];
2922 transposed[9] = m[6];
2923 transposed[10] = m[10];
2924 transposed[11] = m[14];
2925 transposed[12] = m[3];
2926 transposed[13] = m[7];
2927 transposed[14] = m[11];
2928 transposed[15] = m[15];
2929 result[0] = transposed[0];
2930 result[1] = transposed[1];
2931 result[2] = transposed[2];
2932 result[3] = transposed[3];
2933 result[4] = transposed[4];
2934 result[5] = transposed[5];
2935 result[6] = transposed[6];
2936 result[7] = transposed[7];
2937 result[8] = transposed[8];
2938 result[9] = transposed[9];
2939 result[10] = transposed[10];
2940 result[11] = transposed[11];
2941 result[12] = transposed[12];
2942 result[13] = transposed[13];
2943 result[14] = transposed[14];
2944 result[15] = transposed[15];
2945 return result;
2946 }
2947
mat4_adjugate(mfloat_t * result,mfloat_t * m)2948 mfloat_t *mat4_adjugate(mfloat_t *result, mfloat_t *m)
2949 {
2950 result = m;
2951 return result;
2952 }
2953
mat4_inverse(mfloat_t * result,mfloat_t * m)2954 mfloat_t *mat4_inverse(mfloat_t *result, mfloat_t *m)
2955 {
2956 mfloat_t inverse[MAT4_SIZE];
2957 mfloat_t det;
2958 inverse[0] = m[5] * m[10] * m[15] -
2959 m[5] * m[11] * m[14] -
2960 m[9] * m[6] * m[15] +
2961 m[9] * m[7] * m[14] +
2962 m[13] * m[6] * m[11] -
2963 m[13] * m[7] * m[10];
2964 inverse[4] = -m[4] * m[10] * m[15] +
2965 m[4] * m[11] * m[14] +
2966 m[8] * m[6] * m[15] -
2967 m[8] * m[7] * m[14] -
2968 m[12] * m[6] * m[11] +
2969 m[12] * m[7] * m[10];
2970 inverse[8] = m[4] * m[9] * m[15] -
2971 m[4] * m[11] * m[13] -
2972 m[8] * m[5] * m[15] +
2973 m[8] * m[7] * m[13] +
2974 m[12] * m[5] * m[11] -
2975 m[12] * m[7] * m[9];
2976 inverse[12] = -m[4] * m[9] * m[14] +
2977 m[4] * m[10] * m[13] +
2978 m[8] * m[5] * m[14] -
2979 m[8] * m[6] * m[13] -
2980 m[12] * m[5] * m[10] +
2981 m[12] * m[6] * m[9];
2982 inverse[1] = -m[1] * m[10] * m[15] +
2983 m[1] * m[11] * m[14] +
2984 m[9] * m[2] * m[15] -
2985 m[9] * m[3] * m[14] -
2986 m[13] * m[2] * m[11] +
2987 m[13] * m[3] * m[10];
2988 inverse[5] = m[0] * m[10] * m[15] -
2989 m[0] * m[11] * m[14] -
2990 m[8] * m[2] * m[15] +
2991 m[8] * m[3] * m[14] +
2992 m[12] * m[2] * m[11] -
2993 m[12] * m[3] * m[10];
2994 inverse[9] = -m[0] * m[9] * m[15] +
2995 m[0] * m[11] * m[13] +
2996 m[8] * m[1] * m[15] -
2997 m[8] * m[3] * m[13] -
2998 m[12] * m[1] * m[11] +
2999 m[12] * m[3] * m[9];
3000 inverse[13] = m[0] * m[9] * m[14] -
3001 m[0] * m[10] * m[13] -
3002 m[8] * m[1] * m[14] +
3003 m[8] * m[2] * m[13] +
3004 m[12] * m[1] * m[10] -
3005 m[12] * m[2] * m[9];
3006 inverse[2] = m[1] * m[6] * m[15] -
3007 m[1] * m[7] * m[14] -
3008 m[5] * m[2] * m[15] +
3009 m[5] * m[3] * m[14] +
3010 m[13] * m[2] * m[7] -
3011 m[13] * m[3] * m[6];
3012 inverse[6] = -m[0] * m[6] * m[15] +
3013 m[0] * m[7] * m[14] +
3014 m[4] * m[2] * m[15] -
3015 m[4] * m[3] * m[14] -
3016 m[12] * m[2] * m[7] +
3017 m[12] * m[3] * m[6];
3018 inverse[10] = m[0] * m[5] * m[15] -
3019 m[0] * m[7] * m[13] -
3020 m[4] * m[1] * m[15] +
3021 m[4] * m[3] * m[13] +
3022 m[12] * m[1] * m[7] -
3023 m[12] * m[3] * m[5];
3024 inverse[14] = -m[0] * m[5] * m[14] +
3025 m[0] * m[6] * m[13] +
3026 m[4] * m[1] * m[14] -
3027 m[4] * m[2] * m[13] -
3028 m[12] * m[1] * m[6] +
3029 m[12] * m[2] * m[5];
3030 inverse[3] = -m[1] * m[6] * m[11] +
3031 m[1] * m[7] * m[10] +
3032 m[5] * m[2] * m[11] -
3033 m[5] * m[3] * m[10] -
3034 m[9] * m[2] * m[7] +
3035 m[9] * m[3] * m[6];
3036 inverse[7] = m[0] * m[6] * m[11] -
3037 m[0] * m[7] * m[10] -
3038 m[4] * m[2] * m[11] +
3039 m[4] * m[3] * m[10] +
3040 m[8] * m[2] * m[7] -
3041 m[8] * m[3] * m[6];
3042 inverse[11] = -m[0] * m[5] * m[11] +
3043 m[0] * m[7] * m[9] +
3044 m[4] * m[1] * m[11] -
3045 m[4] * m[3] * m[9] -
3046 m[8] * m[1] * m[7] +
3047 m[8] * m[3] * m[5];
3048 inverse[15] = m[0] * m[5] * m[10] -
3049 m[0] * m[6] * m[9] -
3050 m[4] * m[1] * m[10] +
3051 m[4] * m[2] * m[9] +
3052 m[8] * m[1] * m[6] -
3053 m[8] * m[2] * m[5];
3054 det = m[0] * inverse[0] + m[1] * inverse[4] + m[2] * inverse[8] + m[3] * inverse[12];
3055 if (!nearly_equal(det, MFLOAT_C(0.0), MFLT_EPSILON)) {
3056 det = MFLOAT_C(1.0) / det;
3057 }
3058 result[0] = inverse[0] * det;
3059 result[1] = inverse[1] * det;
3060 result[2] = inverse[2] * det;
3061 result[3] = inverse[3] * det;
3062 result[4] = inverse[4] * det;
3063 result[5] = inverse[5] * det;
3064 result[6] = inverse[6] * det;
3065 result[7] = inverse[7] * det;
3066 result[8] = inverse[8] * det;
3067 result[9] = inverse[9] * det;
3068 result[10] = inverse[10] * det;
3069 result[11] = inverse[11] * det;
3070 result[12] = inverse[12] * det;
3071 result[13] = inverse[13] * det;
3072 result[14] = inverse[14] * det;
3073 result[15] = inverse[15] * det;
3074 return result;
3075 }
3076
mat4_ortho(mfloat_t * result,mfloat_t l,mfloat_t r,mfloat_t b,mfloat_t t,mfloat_t n,mfloat_t f)3077 mfloat_t *mat4_ortho(mfloat_t *result, mfloat_t l, mfloat_t r, mfloat_t b, mfloat_t t, mfloat_t n, mfloat_t f)
3078 {
3079 result[0] = MFLOAT_C(2.0) / (r - l);
3080 result[1] = MFLOAT_C(0.0);
3081 result[2] = MFLOAT_C(0.0);
3082 result[3] = MFLOAT_C(0.0);
3083 result[4] = MFLOAT_C(0.0);
3084 result[5] = MFLOAT_C(2.0) / (t - b);
3085 result[6] = MFLOAT_C(0.0);
3086 result[7] = MFLOAT_C(0.0);
3087 result[8] = MFLOAT_C(0.0);
3088 result[9] = MFLOAT_C(0.0);
3089 result[10] = -MFLOAT_C(2.0) / (f - n);
3090 result[11] = MFLOAT_C(0.0);
3091 result[12] = -((r + l) / (r - l));
3092 result[13] = -((t + b) / (t - b));
3093 result[14] = -((f + n) / (f - n));
3094 result[15] = MFLOAT_C(1.0);
3095 return result;
3096 }
3097
mat4_perspective(mfloat_t * result,mfloat_t fov_y,mfloat_t aspect,mfloat_t n,mfloat_t f)3098 mfloat_t *mat4_perspective(mfloat_t *result, mfloat_t fov_y, mfloat_t aspect, mfloat_t n, mfloat_t f)
3099 {
3100 mfloat_t tan_half_fov_y = MFLOAT_C(1.0) / MTAN(fov_y * MFLOAT_C(0.5));
3101 result[0] = MFLOAT_C(1.0) / aspect * tan_half_fov_y;
3102 result[1] = MFLOAT_C(0.0);
3103 result[2] = MFLOAT_C(0.0);
3104 result[3] = MFLOAT_C(0.0);
3105 result[4] = MFLOAT_C(0.0);
3106 result[5] = MFLOAT_C(1.0) / tan_half_fov_y;
3107 result[6] = MFLOAT_C(0.0);
3108 result[7] = MFLOAT_C(0.0);
3109 result[8] = MFLOAT_C(0.0);
3110 result[9] = MFLOAT_C(0.0);
3111 result[10] = f / (n - f);
3112 result[11] = -MFLOAT_C(1.0);
3113 result[12] = MFLOAT_C(0.0);
3114 result[13] = MFLOAT_C(0.0);
3115 result[14] = -(f * n) / (f - n);
3116 result[15] = MFLOAT_C(0.0);
3117 return result;
3118 }
3119
mat4_perspective_fov(mfloat_t * result,mfloat_t fov,mfloat_t w,mfloat_t h,mfloat_t n,mfloat_t f)3120 mfloat_t *mat4_perspective_fov(mfloat_t *result, mfloat_t fov, mfloat_t w, mfloat_t h, mfloat_t n, mfloat_t f)
3121 {
3122 mfloat_t h2 = MCOS(fov * MFLOAT_C(0.5)) / MSIN(fov * MFLOAT_C(0.5));
3123 mfloat_t w2 = h2 * h / w;
3124 result[0] = w2;
3125 result[1] = MFLOAT_C(0.0);
3126 result[2] = MFLOAT_C(0.0);
3127 result[3] = MFLOAT_C(0.0);
3128 result[4] = MFLOAT_C(0.0);
3129 result[5] = h2;
3130 result[6] = MFLOAT_C(0.0);
3131 result[7] = MFLOAT_C(0.0);
3132 result[8] = MFLOAT_C(0.0);
3133 result[9] = MFLOAT_C(0.0);
3134 result[10] = f / (n - f);
3135 result[11] = -MFLOAT_C(1.0);
3136 result[12] = MFLOAT_C(0.0);
3137 result[13] = MFLOAT_C(0.0);
3138 result[14] = -(f * n) / (f - n);
3139 result[15] = MFLOAT_C(0.0);
3140 return result;
3141 }
3142
mat4_perspective_infinite(mfloat_t * result,mfloat_t fov_y,mfloat_t aspect,mfloat_t n)3143 mfloat_t *mat4_perspective_infinite(mfloat_t *result, mfloat_t fov_y, mfloat_t aspect, mfloat_t n)
3144 {
3145 mfloat_t range = MTAN(fov_y * MFLOAT_C(0.5)) * n;
3146 mfloat_t left = -range * aspect;
3147 mfloat_t right = range * aspect;
3148 mfloat_t top = range;
3149 mfloat_t bottom = -range;
3150 result[0] = MFLOAT_C(2.0) * n / (right - left);
3151 result[1] = MFLOAT_C(0.0);
3152 result[2] = MFLOAT_C(0.0);
3153 result[3] = MFLOAT_C(0.0);
3154 result[4] = MFLOAT_C(0.0);
3155 result[5] = MFLOAT_C(2.0) * n / (top - bottom);
3156 result[6] = MFLOAT_C(0.0);
3157 result[7] = MFLOAT_C(0.0);
3158 result[8] = MFLOAT_C(0.0);
3159 result[9] = MFLOAT_C(0.0);
3160 result[10] = -MFLOAT_C(1.0);
3161 result[11] = -MFLOAT_C(1.0);
3162 result[12] = MFLOAT_C(0.0);
3163 result[13] = MFLOAT_C(0.0);
3164 result[14] = -MFLOAT_C(2.0) * n;
3165 result[15] = MFLOAT_C(0.0);
3166 return result;
3167 }
3168
mat4_rotation_x(mfloat_t * result,mfloat_t angle)3169 mfloat_t *mat4_rotation_x(mfloat_t *result, mfloat_t angle)
3170 {
3171 mfloat_t c = MCOS(angle);
3172 mfloat_t s = MSIN(angle);
3173 result[0] = MFLOAT_C(1.0);
3174 result[1] = MFLOAT_C(0.0);
3175 result[2] = MFLOAT_C(0.0);
3176 result[3] = MFLOAT_C(0.0);
3177 result[4] = MFLOAT_C(0.0);
3178 result[5] = c;
3179 result[6] = s;
3180 result[7] = MFLOAT_C(0.0);
3181 result[8] = MFLOAT_C(0.0);
3182 result[9] = -s;
3183 result[10] = c;
3184 result[11] = MFLOAT_C(0.0);
3185 result[12] = MFLOAT_C(0.0);
3186 result[13] = MFLOAT_C(0.0);
3187 result[14] = MFLOAT_C(0.0);
3188 result[15] = MFLOAT_C(1.0);
3189 return result;
3190 }
3191
mat4_rotation_y(mfloat_t * result,mfloat_t angle)3192 mfloat_t *mat4_rotation_y(mfloat_t *result, mfloat_t angle)
3193 {
3194 mfloat_t c = MCOS(angle);
3195 mfloat_t s = MSIN(angle);
3196 result[0] = c;
3197 result[1] = MFLOAT_C(0.0);
3198 result[2] = -s;
3199 result[3] = MFLOAT_C(0.0);
3200 result[4] = MFLOAT_C(0.0);
3201 result[5] = MFLOAT_C(0.0);
3202 result[6] = MFLOAT_C(0.0);
3203 result[7] = MFLOAT_C(0.0);
3204 result[8] = s;
3205 result[9] = MFLOAT_C(0.0);
3206 result[10] = c;
3207 result[11] = MFLOAT_C(0.0);
3208 result[12] = MFLOAT_C(0.0);
3209 result[13] = MFLOAT_C(0.0);
3210 result[14] = MFLOAT_C(0.0);
3211 result[15] = MFLOAT_C(1.0);
3212 return result;
3213 }
3214
mat4_rotation_z(mfloat_t * result,mfloat_t angle)3215 mfloat_t *mat4_rotation_z(mfloat_t *result, mfloat_t angle)
3216 {
3217 mfloat_t c = MCOS(angle);
3218 mfloat_t s = MSIN(angle);
3219 result[0] = c;
3220 result[1] = s;
3221 result[2] = MFLOAT_C(0.0);
3222 result[3] = MFLOAT_C(0.0);
3223 result[4] = -s;
3224 result[5] = c;
3225 result[6] = MFLOAT_C(0.0);
3226 result[7] = MFLOAT_C(0.0);
3227 result[8] = MFLOAT_C(0.0);
3228 result[9] = MFLOAT_C(0.0);
3229 result[10] = MFLOAT_C(1.0);
3230 result[11] = MFLOAT_C(0.0);
3231 result[12] = MFLOAT_C(0.0);
3232 result[13] = MFLOAT_C(0.0);
3233 result[14] = MFLOAT_C(0.0);
3234 result[15] = MFLOAT_C(1.0);
3235 return result;
3236 }
3237
mat4_rotation_axis(mfloat_t * result,mfloat_t * a,mfloat_t angle)3238 mfloat_t *mat4_rotation_axis(mfloat_t *result, mfloat_t *a, mfloat_t angle)
3239 {
3240 mfloat_t c = MCOS(angle);
3241 mfloat_t s = MSIN(angle);
3242 mfloat_t one_c = MFLOAT_C(1.0) - c;
3243 mfloat_t x = a[0];
3244 mfloat_t y = a[4];
3245 mfloat_t z = a[8];
3246 mfloat_t xx = x * x;
3247 mfloat_t xy = x * y;
3248 mfloat_t xz = x * z;
3249 mfloat_t yy = y * y;
3250 mfloat_t yz = y * z;
3251 mfloat_t zz = z * z;
3252 mfloat_t l = xx + yy + zz;
3253 mfloat_t sqrt_l = MSQRT(l);
3254 result[0] = (xx + (yy + zz) * c) / l;
3255 result[1] = (xy * one_c + a[2] * sqrt_l * s) / l;
3256 result[2] = (xz * one_c - a[1] * sqrt_l * s) / l;
3257 result[3] = MFLOAT_C(0.0);
3258 result[4] = (xy * one_c - a[2] * sqrt_l * s) / l;
3259 result[5] = (yy + (xx + zz) * c) / l;
3260 result[6] = (yz * one_c + a[0] * sqrt_l * s) / l;
3261 result[7] = MFLOAT_C(0.0);
3262 result[8] = (xz * one_c + a[1] * sqrt_l * s) / l;
3263 result[9] = (yz * one_c - a[0] * sqrt_l * s) / l;
3264 result[10] = (zz + (xx + yy) * c) / l;
3265 result[11] = MFLOAT_C(0.0);
3266 result[12] = MFLOAT_C(0.0);
3267 result[13] = MFLOAT_C(0.0);
3268 result[14] = MFLOAT_C(0.0);
3269 result[15] = MFLOAT_C(1.0);
3270 return result;
3271 }
3272
mat4_rotation_quaternion(mfloat_t * result,mfloat_t * q)3273 mfloat_t *mat4_rotation_quaternion(mfloat_t *result, mfloat_t *q)
3274 {
3275 mfloat_t xx = q[0] * q[0];
3276 mfloat_t yy = q[1] * q[1];
3277 mfloat_t zz = q[2] * q[2];
3278 mfloat_t xy = q[0] * q[1];
3279 mfloat_t zw = q[2] * q[3];
3280 mfloat_t xz = q[8] * q[0];
3281 mfloat_t yw = q[1] * q[3];
3282 mfloat_t yz = q[1] * q[2];
3283 mfloat_t xw = q[0] * q[3];
3284 result[0] = MFLOAT_C(1.0) - MFLOAT_C(2.0) * (yy - zz);
3285 result[1] = MFLOAT_C(2.0) * (xy + zw);
3286 result[2] = MFLOAT_C(2.0) * (xz - yw);
3287 result[3] = MFLOAT_C(0.0);
3288 result[4] = MFLOAT_C(2.0) * (xy - zw);
3289 result[5] = MFLOAT_C(1.0) - MFLOAT_C(2.0) * (xx - zz);
3290 result[6] = MFLOAT_C(2.0) * (yz + xw);
3291 result[7] = MFLOAT_C(0.0);
3292 result[8] = MFLOAT_C(2.0) * (xz + yw);
3293 result[9] = MFLOAT_C(2.0) * (yz - xw);
3294 result[10] = MFLOAT_C(1.0) - MFLOAT_C(2.0) * (xx - yy);
3295 result[11] = MFLOAT_C(0.0);
3296 result[12] = MFLOAT_C(0.0);
3297 result[13] = MFLOAT_C(0.0);
3298 result[14] = MFLOAT_C(0.0);
3299 result[15] = MFLOAT_C(1.0);
3300 return result;
3301 }
3302
mat4_look_at(mfloat_t * result,mfloat_t * position,mfloat_t * target,mfloat_t * up)3303 mfloat_t *mat4_look_at(mfloat_t *result, mfloat_t *position, mfloat_t *target, mfloat_t *up)
3304 {
3305 mfloat_t tmp_forward[VEC3_SIZE];
3306 mfloat_t tmp_side[VEC3_SIZE];
3307 mfloat_t tmp_up[VEC3_SIZE];
3308 vec3_subtract(tmp_forward, target, position);
3309 vec3_normalize(tmp_forward, tmp_forward);
3310 vec3_cross(tmp_side, tmp_forward, up);
3311 vec3_normalize(tmp_side, tmp_side);
3312 vec3_cross(tmp_up, tmp_side, tmp_forward);
3313 result[0] = tmp_side[0];
3314 result[1] = tmp_up[0];
3315 result[2] = -tmp_forward[0];
3316 result[3] = MFLOAT_C(0.0);
3317 result[4] = tmp_side[1];
3318 result[5] = tmp_up[1];
3319 result[6] = -tmp_forward[1];
3320 result[7] = MFLOAT_C(0.0);
3321 result[8] = tmp_side[2];
3322 result[9] = tmp_up[2];
3323 result[10] = -tmp_forward[2];
3324 result[11] = MFLOAT_C(0.0);
3325 result[12] = -vec3_dot(tmp_side, position);
3326 result[13] = -vec3_dot(tmp_up, position);
3327 result[14] = vec3_dot(tmp_forward, position);
3328 result[15] = MFLOAT_C(1.0);
3329 return result;
3330 }
3331
mat4_translation(mfloat_t * result,mfloat_t * v)3332 mfloat_t *mat4_translation(mfloat_t *result, mfloat_t *v)
3333 {
3334 result[0] = MFLOAT_C(1.0);
3335 result[1] = MFLOAT_C(0.0);
3336 result[2] = MFLOAT_C(0.0);
3337 result[3] = MFLOAT_C(0.0);
3338 result[4] = MFLOAT_C(0.0);
3339 result[5] = MFLOAT_C(1.0);
3340 result[6] = MFLOAT_C(0.0);
3341 result[7] = MFLOAT_C(0.0);
3342 result[8] = MFLOAT_C(0.0);
3343 result[9] = MFLOAT_C(0.0);
3344 result[10] = MFLOAT_C(1.0);
3345 result[11] = MFLOAT_C(0.0);
3346 result[12] = v[0];
3347 result[13] = v[1];
3348 result[14] = v[2];
3349 result[15] = MFLOAT_C(1.0);
3350 return result;
3351 }
3352
mat4_scaling(mfloat_t * result,mfloat_t * v)3353 mfloat_t *mat4_scaling(mfloat_t *result, mfloat_t *v)
3354 {
3355 result[0] = v[0];
3356 result[1] = MFLOAT_C(0.0);
3357 result[2] = MFLOAT_C(0.0);
3358 result[3] = MFLOAT_C(0.0);
3359 result[4] = MFLOAT_C(0.0);
3360 result[5] = v[1];
3361 result[6] = MFLOAT_C(0.0);
3362 result[7] = MFLOAT_C(0.0);
3363 result[8] = MFLOAT_C(0.0);
3364 result[9] = MFLOAT_C(0.0);
3365 result[10] = v[2];
3366 result[11] = MFLOAT_C(0.0);
3367 result[12] = MFLOAT_C(0.0);
3368 result[13] = MFLOAT_C(0.0);
3369 result[14] = MFLOAT_C(0.0);
3370 result[15] = MFLOAT_C(1.0);
3371 return result;
3372 }
3373
mat4_negative(mfloat_t * result,mfloat_t * m)3374 mfloat_t *mat4_negative(mfloat_t *result, mfloat_t *m)
3375 {
3376 result[0] = -m[0];
3377 result[1] = -m[1];
3378 result[2] = -m[2];
3379 result[3] = -m[3];
3380 result[4] = -m[4];
3381 result[5] = -m[5];
3382 result[6] = -m[6];
3383 result[7] = -m[7];
3384 result[8] = -m[8];
3385 result[9] = -m[9];
3386 result[10] = -m[10];
3387 result[11] = -m[11];
3388 result[12] = -m[12];
3389 result[13] = -m[13];
3390 result[14] = -m[14];
3391 result[15] = -m[15];
3392 return result;
3393 }
3394
mat4_scale(mfloat_t * result,mfloat_t * m,mfloat_t scalar)3395 mfloat_t *mat4_scale(mfloat_t *result, mfloat_t *m, mfloat_t scalar)
3396 {
3397 result[0] = m[0] * scalar;
3398 result[1] = m[1] * scalar;
3399 result[2] = m[2] * scalar;
3400 result[3] = m[3] * scalar;
3401 result[4] = m[4] * scalar;
3402 result[5] = m[5] * scalar;
3403 result[6] = m[6] * scalar;
3404 result[7] = m[7] * scalar;
3405 result[8] = m[8] * scalar;
3406 result[9] = m[9] * scalar;
3407 result[10] = m[10] * scalar;
3408 result[11] = m[11] * scalar;
3409 result[12] = m[12] * scalar;
3410 result[13] = m[13] * scalar;
3411 result[14] = m[14] * scalar;
3412 result[15] = m[15] * scalar;
3413 return result;
3414 }
3415
mat4_multiply(mfloat_t * result,mfloat_t * a,mfloat_t * b)3416 mfloat_t *mat4_multiply(mfloat_t *result, mfloat_t *a, mfloat_t *b)
3417 {
3418 mfloat_t multiplied[MAT4_SIZE];
3419 multiplied[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2] + a[12] * b[3];
3420 multiplied[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2] + a[13] * b[3];
3421 multiplied[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2] + a[14] * b[3];
3422 multiplied[3] = a[3] * b[0] + a[7] * b[1] + a[11] * b[2] + a[15] * b[3];
3423 multiplied[4] = a[0] * b[4] + a[4] * b[5] + a[8] * b[6] + a[12] * b[7];
3424 multiplied[5] = a[1] * b[4] + a[5] * b[5] + a[9] * b[6] + a[13] * b[7];
3425 multiplied[6] = a[2] * b[4] + a[6] * b[5] + a[10] * b[6] + a[14] * b[7];
3426 multiplied[7] = a[3] * b[4] + a[7] * b[5] + a[11] * b[6] + a[15] * b[7];
3427 multiplied[8] = a[0] * b[8] + a[4] * b[9] + a[8] * b[10] + a[12] * b[11];
3428 multiplied[9] = a[1] * b[8] + a[5] * b[9] + a[9] * b[10] + a[13] * b[11];
3429 multiplied[10] = a[2] * b[8] + a[6] * b[9] + a[10] * b[10] + a[14] * b[11];
3430 multiplied[11] = a[3] * b[8] + a[7] * b[9] + a[11] * b[10] + a[15] * b[11];
3431 multiplied[12] = a[0] * b[12] + a[4] * b[13] + a[8] * b[14] + a[12] * b[15];
3432 multiplied[13] = a[1] * b[12] + a[5] * b[13] + a[9] * b[14] + a[13] * b[15];
3433 multiplied[14] = a[2] * b[12] + a[6] * b[13] + a[10] * b[14] + a[14] * b[15];
3434 multiplied[15] = a[3] * b[12] + a[7] * b[13] + a[11] * b[14] + a[15] * b[15];
3435 result[0] = multiplied[0];
3436 result[1] = multiplied[1];
3437 result[2] = multiplied[2];
3438 result[3] = multiplied[3];
3439 result[4] = multiplied[4];
3440 result[5] = multiplied[5];
3441 result[6] = multiplied[6];
3442 result[7] = multiplied[7];
3443 result[8] = multiplied[8];
3444 result[9] = multiplied[9];
3445 result[10] = multiplied[10];
3446 result[11] = multiplied[11];
3447 result[12] = multiplied[12];
3448 result[13] = multiplied[13];
3449 result[14] = multiplied[14];
3450 result[15] = multiplied[15];
3451 return result;
3452 }
3453
mat4_lerp(mfloat_t * result,mfloat_t * a,mfloat_t * b,mfloat_t p)3454 mfloat_t *mat4_lerp(mfloat_t *result, mfloat_t *a, mfloat_t *b, mfloat_t p)
3455 {
3456 result[0] = a[0] + (b[0] - a[0]) * p;
3457 result[1] = a[1] + (b[1] - a[1]) * p;
3458 result[2] = a[2] + (b[2] - a[2]) * p;
3459 result[3] = a[3] + (b[3] - a[3]) * p;
3460 result[4] = a[4] + (b[4] - a[4]) * p;
3461 result[5] = a[5] + (b[5] - a[5]) * p;
3462 result[6] = a[6] + (b[6] - a[6]) * p;
3463 result[7] = a[7] + (b[7] - a[7]) * p;
3464 result[8] = a[8] + (b[8] - a[8]) * p;
3465 result[9] = a[9] + (b[9] - a[9]) * p;
3466 result[10] = a[10] + (b[10] - a[10]) * p;
3467 result[11] = a[11] + (b[11] - a[11]) * p;
3468 result[12] = a[12] + (b[12] - a[12]) * p;
3469 result[13] = a[13] + (b[13] - a[13]) * p;
3470 result[14] = a[14] + (b[14] - a[14]) * p;
3471 result[15] = a[15] + (b[15] - a[15]) * p;
3472 return result;
3473 }
3474
3475 #ifndef MATHC_NO_EASING_FUNCTIONS
3476 /* Easing functions */
quadratic_ease_in(mfloat_t p)3477 mfloat_t quadratic_ease_in(mfloat_t p)
3478 {
3479 return p * p;
3480 }
3481
quadratic_ease_out(mfloat_t p)3482 mfloat_t quadratic_ease_out(mfloat_t p)
3483 {
3484 return -(p * (p - MFLOAT_C(2.0)));
3485 }
3486
quadratic_ease_in_out(mfloat_t p)3487 mfloat_t quadratic_ease_in_out(mfloat_t p)
3488 {
3489 mfloat_t f = MFLOAT_C(0.0);
3490 if (p < MFLOAT_C(0.5)) {
3491 f = MFLOAT_C(2.0) * p * p;
3492 } else {
3493 f = (-MFLOAT_C(2.0) * p * p) + (MFLOAT_C(4.0) * p) - MFLOAT_C(1.0);
3494 }
3495 return f;
3496 }
3497
cubic_ease_in(mfloat_t p)3498 mfloat_t cubic_ease_in(mfloat_t p)
3499 {
3500 return p * p * p;
3501 }
3502
cubic_ease_out(mfloat_t p)3503 mfloat_t cubic_ease_out(mfloat_t p)
3504 {
3505 mfloat_t f = (p - MFLOAT_C(1.0));
3506 return f * f * f + MFLOAT_C(1.0);
3507 }
3508
cubic_ease_in_out(mfloat_t p)3509 mfloat_t cubic_ease_in_out(mfloat_t p)
3510 {
3511 mfloat_t f = MFLOAT_C(0.0);
3512 if (p < MFLOAT_C(0.5)) {
3513 f = MFLOAT_C(4.0) * p * p * p;
3514 } else {
3515 f = ((MFLOAT_C(2.0) * p) - MFLOAT_C(2.0));
3516 f = MFLOAT_C(0.5) * f * f * f + MFLOAT_C(1.0);
3517 }
3518 return f;
3519 }
3520
quartic_ease_in(mfloat_t p)3521 mfloat_t quartic_ease_in(mfloat_t p)
3522 {
3523 return p * p * p * p;
3524 }
3525
quartic_ease_out(mfloat_t p)3526 mfloat_t quartic_ease_out(mfloat_t p)
3527 {
3528 mfloat_t f = (p - MFLOAT_C(1.0));
3529 return f * f * f * (MFLOAT_C(1.0) - p) + MFLOAT_C(1.0);
3530 }
3531
quartic_ease_in_out(mfloat_t p)3532 mfloat_t quartic_ease_in_out(mfloat_t p)
3533 {
3534 mfloat_t f = MFLOAT_C(0.0);
3535 if (p < MFLOAT_C(0.5)) {
3536 f = MFLOAT_C(8.0) * p * p * p * p;
3537 } else {
3538 f = (p - MFLOAT_C(1.0));
3539 f = -MFLOAT_C(8.0) * f * f * f * f + MFLOAT_C(1.0);
3540 }
3541 return f;
3542 }
3543
quintic_ease_in(mfloat_t p)3544 mfloat_t quintic_ease_in(mfloat_t p)
3545 {
3546 return p * p * p * p * p;
3547 }
3548
quintic_ease_out(mfloat_t p)3549 mfloat_t quintic_ease_out(mfloat_t p)
3550 {
3551 mfloat_t f = (p - MFLOAT_C(1.0));
3552 return f * f * f * f * f + MFLOAT_C(1.0);
3553 }
3554
quintic_ease_in_out(mfloat_t p)3555 mfloat_t quintic_ease_in_out(mfloat_t p)
3556 {
3557 mfloat_t f = MFLOAT_C(0.0);
3558 if (p < MFLOAT_C(0.5)) {
3559 f = MFLOAT_C(16.0) * p * p * p * p * p;
3560 } else {
3561 f = ((MFLOAT_C(2.0) * p) - MFLOAT_C(2.0));
3562 f = MFLOAT_C(0.5) * f * f * f * f * f + MFLOAT_C(1.0);
3563 }
3564 return f;
3565 }
3566
sine_ease_in(mfloat_t p)3567 mfloat_t sine_ease_in(mfloat_t p)
3568 {
3569 return MSIN((p - MFLOAT_C(1.0)) * MPI_2) + MFLOAT_C(1.0);
3570 }
3571
sine_ease_out(mfloat_t p)3572 mfloat_t sine_ease_out(mfloat_t p)
3573 {
3574 return MSIN(p * MPI_2);
3575 }
3576
sine_ease_in_out(mfloat_t p)3577 mfloat_t sine_ease_in_out(mfloat_t p)
3578 {
3579 return MFLOAT_C(0.5) * (MFLOAT_C(1.0) - MCOS(p * MPI));
3580 }
3581
circular_ease_in(mfloat_t p)3582 mfloat_t circular_ease_in(mfloat_t p)
3583 {
3584 return MFLOAT_C(1.0) - MSQRT(MFLOAT_C(1.0) - (p * p));
3585 }
3586
circular_ease_out(mfloat_t p)3587 mfloat_t circular_ease_out(mfloat_t p)
3588 {
3589 return MSQRT((MFLOAT_C(2.0) - p) * p);
3590 }
3591
circular_ease_in_out(mfloat_t p)3592 mfloat_t circular_ease_in_out(mfloat_t p)
3593 {
3594 mfloat_t f = MFLOAT_C(0.0);
3595 if (p < MFLOAT_C(0.5)) {
3596 f = MFLOAT_C(0.5) * (MFLOAT_C(1.0) - MSQRT(MFLOAT_C(1.0) - MFLOAT_C(4.0) * (p * p)));
3597 } else {
3598 f = MFLOAT_C(0.5) * (MSQRT(-((MFLOAT_C(2.0) * p) - MFLOAT_C(3.0)) * ((MFLOAT_C(2.0) * p) - MFLOAT_C(1.0))) + MFLOAT_C(1.0));
3599 }
3600 return f;
3601 }
3602
exponential_ease_in(mfloat_t p)3603 mfloat_t exponential_ease_in(mfloat_t p)
3604 {
3605 mfloat_t f = p;
3606 if (p != MFLOAT_C(0.0)) {
3607 f = MPOW(MFLOAT_C(2.0), MFLOAT_C(10.0) * (p - MFLOAT_C(1.0)));
3608 }
3609 return f;
3610 }
3611
exponential_ease_out(mfloat_t p)3612 mfloat_t exponential_ease_out(mfloat_t p)
3613 {
3614 mfloat_t f = p;
3615 if (p != MFLOAT_C(1.0)) {
3616 f = MFLOAT_C(1.0) - MPOW(MFLOAT_C(2.0), -MFLOAT_C(10.0) * p);
3617 }
3618 return f;
3619 }
3620
exponential_ease_in_out(mfloat_t p)3621 mfloat_t exponential_ease_in_out(mfloat_t p)
3622 {
3623 mfloat_t f = p;
3624 if (p < MFLOAT_C(0.5)) {
3625 f = MFLOAT_C(0.5) * MPOW(MFLOAT_C(2.0), (MFLOAT_C(20.0) * p) - MFLOAT_C(10.0));
3626 } else {
3627 f = -MFLOAT_C(0.5) * MPOW(MFLOAT_C(2.0), (-MFLOAT_C(20.0) * p) + MFLOAT_C(10.0)) + MFLOAT_C(1.0);
3628 }
3629 return f;
3630 }
3631
elastic_ease_in(mfloat_t p)3632 mfloat_t elastic_ease_in(mfloat_t p)
3633 {
3634 return MSIN(MFLOAT_C(13.0) * MPI_2 * p) * MPOW(MFLOAT_C(2.0), MFLOAT_C(10.0) * (p - MFLOAT_C(1.0)));
3635 }
3636
elastic_ease_out(mfloat_t p)3637 mfloat_t elastic_ease_out(mfloat_t p)
3638 {
3639 return MSIN(-MFLOAT_C(13.0) * MPI_2 * (p + MFLOAT_C(1.0))) * MPOW(MFLOAT_C(2.0), -MFLOAT_C(10.0) * p) + MFLOAT_C(1.0);
3640 }
3641
elastic_ease_in_out(mfloat_t p)3642 mfloat_t elastic_ease_in_out(mfloat_t p)
3643 {
3644 mfloat_t f = MFLOAT_C(0.0);
3645 if (p < MFLOAT_C(0.5)) {
3646 f = MFLOAT_C(0.5) * MSIN(MFLOAT_C(13.0) * MPI_2 * (MFLOAT_C(2.0) * p)) * MPOW(MFLOAT_C(2.0), MFLOAT_C(10.0) * ((MFLOAT_C(2.0) * p) - MFLOAT_C(1.0)));
3647 } else {
3648 f = MFLOAT_C(0.5) * (MSIN(-MFLOAT_C(13.0) * MPI_2 * ((MFLOAT_C(2.0) * p - MFLOAT_C(1.0)) + MFLOAT_C(1.0))) * MPOW(MFLOAT_C(2.0), -MFLOAT_C(10.0) * (MFLOAT_C(2.0) * p - MFLOAT_C(1.0))) + MFLOAT_C(2.0));
3649 }
3650 return f;
3651 }
3652
back_ease_in(mfloat_t p)3653 mfloat_t back_ease_in(mfloat_t p)
3654 {
3655 return p * p * p - p * MSIN(p * MPI);
3656 }
3657
back_ease_out(mfloat_t p)3658 mfloat_t back_ease_out(mfloat_t p)
3659 {
3660 mfloat_t f = (MFLOAT_C(1.0) - p);
3661 return MFLOAT_C(1.0) - (f * f * f - f * MSIN(f * MPI));
3662 }
3663
back_ease_in_out(mfloat_t p)3664 mfloat_t back_ease_in_out(mfloat_t p)
3665 {
3666 mfloat_t f = MFLOAT_C(0.0);
3667 if (p < MFLOAT_C(0.5)) {
3668 f = MFLOAT_C(2.0) * p;
3669 f = MFLOAT_C(0.5) * (f * f * f - f * MSIN(f * MPI));
3670 } else {
3671 f = (MFLOAT_C(1.0) - (MFLOAT_C(2.0) * p - MFLOAT_C(1.0)));
3672 f = MFLOAT_C(0.5) * (MFLOAT_C(1.0) - (f * f * f - f * MSIN(f * MPI))) + MFLOAT_C(0.5);
3673 }
3674 return f;
3675 }
3676
bounce_ease_in(mfloat_t p)3677 mfloat_t bounce_ease_in(mfloat_t p)
3678 {
3679 return MFLOAT_C(1.0) - bounce_ease_out(MFLOAT_C(1.0) - p);
3680 }
3681
bounce_ease_out(mfloat_t p)3682 mfloat_t bounce_ease_out(mfloat_t p)
3683 {
3684 mfloat_t f = MFLOAT_C(0.0);
3685 if (p < MFLOAT_C(4.0) / MFLOAT_C(11.0)) {
3686 f = (MFLOAT_C(121.0) * p * p) / MFLOAT_C(16.0);
3687 } else if (p < MFLOAT_C(8.0) / MFLOAT_C(11.0)) {
3688 f = (MFLOAT_C(363.0) / MFLOAT_C(40.0) * p * p) - (MFLOAT_C(99.0) / MFLOAT_C(10.0) * p) + MFLOAT_C(17.0) / MFLOAT_C(5.0);
3689 } else if (p < MFLOAT_C(9.0) / MFLOAT_C(10.0)) {
3690 f = (MFLOAT_C(4356.0) / MFLOAT_C(361.0) * p * p) - (MFLOAT_C(35442.0) / MFLOAT_C(1805.0) * p) + MFLOAT_C(16061.0) / MFLOAT_C(1805.0);
3691 } else {
3692 f = (MFLOAT_C(54.0) / MFLOAT_C(5.0) * p * p) - (MFLOAT_C(513.0) / MFLOAT_C(25.0) * p) + MFLOAT_C(268.0) / MFLOAT_C(25.0);
3693 }
3694 return f;
3695 }
3696
bounce_ease_in_out(mfloat_t p)3697 mfloat_t bounce_ease_in_out(mfloat_t p)
3698 {
3699 mfloat_t f = MFLOAT_C(0.0);
3700 if (p < MFLOAT_C(0.5)) {
3701 f = MFLOAT_C(0.5) * bounce_ease_in(p * MFLOAT_C(2.0));
3702 } else {
3703 f = MFLOAT_C(0.5) * bounce_ease_out(p * MFLOAT_C(2.0) - MFLOAT_C(1.0)) + MFLOAT_C(0.5);
3704 }
3705 return f;
3706 }
3707 #endif
3708