Untitled
Loading...
Searching...
No Matches
mathf.h
Go to the documentation of this file.
1
2#ifndef MATHF_H
3#define MATHF_H
4
5#include <math.h>
6#include "types.h"
7#include <stdio.h>
8
9
10
11static inline int round_up_power_of_two(int a, int max)
12{
13
14 int p2 = 2;
15
16 for (int i = 0; i < max; i++)
17 {
18 p2 *= p2;
19 if (p2 > a)
20 {
21 break;
22 }
23 }
24 return p2;
25}
26
27static inline float smoothstep(float a, float b, float c)
28{
29
30 float t = (c - a) / (b - a);
31 if (t < 0.0f) t = 0.0f;
32 if (t > 1.0f) t = 1.0f;
33
34 return t * t * (3.0f - 2.0f * t);
35
36}
37
38static inline vec2 vec2_normalize(vec2 v) {
39 float len = sqrtf(v.x*v.x + v.y*v.y);
40 if (len == 0.0f) return v;
41 v.x /= len;
42 v.y /= len;
43
44 return v;
45}
46static inline vec3 vec3_normalize(vec3 v) {
47 float len = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
48 if (len == 0.0f) return v;
49 v.x /= len;
50 v.y /= len;
51 v.z /= len;
52
53 return v;
54}
55static inline vec4 vec4_normalize(vec4 v) {
56
57 float len = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z + v.w*v.w);
58
59 if (len == 0.0f) return v;
60
61 v.x /= len;
62 v.y /= len;
63 v.z /= len;
64 v.w /= len;
65
66 return v;
67
68}
69static inline vec3 ivec3_normalize(ivec3 a)
70{
71 vec3 v = {(float)a.x, (float)a.y, (float)a.z};
72 float len = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
73 if (len == 0.0f) return v;
74 v.x /= len;
75 v.y /= len;
76 v.z /= len;
77
78 return v;
79}
80static inline vec3 vec3_cross(vec3 a, vec3 b) {
81 vec3 out;
82 out.x = a.y*b.z - a.z*b.y;
83 out.y = a.z*b.x - a.x*b.z;
84 out.z = a.x*b.y - a.y*b.x;
85 return out;
86}
87
88static inline mat3 rotation_matrix_r(vec3 right, vec3 up, vec3 forward)
89{
90
91 mat3 a;
92 a.a0 = right.x;
93 a.a1 = right.y;
94 a.a2 = right.z;
95 a.b0 = up.x;
96 a.b1 = up.y;
97 a.b2 = up.z;
98 a.c0 = forward.x;
99 a.c1 = forward.y;
100 a.c2 = forward.z;
101
102 return a;
103
104}
105
106
107
108static inline bool m9_inverse(float inverse[9][9], float mat[9][9])
109{
110
111 double temp[9][2 * 9];
112
113 for (int i = 0; i < 9; i++) {
114 for (int j = 0; j < 9; j++) {
115 temp[i][j] = mat[i][j];
116 temp[i][j + 9] = (i == j) ? 1.0 : 0.0;
117 }
118 }
119
120 for (int i = 0; i < 9; i++) {
121 int maxRow = i;
122 for (int k = i + 1; k < 9; k++) {
123 if (fabs(temp[k][i]) > fabs(temp[maxRow][i])) {
124 maxRow = k;
125 }
126 }
127
128 if (maxRow != i) {
129 for (int k = 0; k < 2 * 9; k++) {
130 double t = temp[i][k];
131 temp[i][k] = temp[maxRow][k];
132 temp[maxRow][k] = t;
133 }
134 }
135
136 if (fabs(temp[i][i]) < 1e-9) {
137 printf("Matrix is singular and cannot be inverted.\n");
138 return 0;
139 }
140
141 double pivot = temp[i][i];
142 for (int k = 0; k < 2 * 9; k++) {
143 temp[i][k] /= pivot;
144 }
145 for (int j = 0; j < 9; j++) {
146 if (j != i) {
147 double factor = temp[j][i];
148 for (int k = 0; k < 2 * 9; k++) {
149 temp[j][k] -= factor * temp[i][k];
150 }
151 }
152 }
153 }
154
155 for (int i = 0; i < 9; i++) {
156 for (int j = 0; j < 9; j++) {
157 inverse[i][j] = temp[i][j + 9];
158 }
159 }
160
161 return 1;
162}
163
164static inline bool m4_inverse(float out[16], const float m[16]) {
165 float inv[16], det;
166 int i;
167
168 inv[0] = m[5]*m[10]*m[15] - m[5]*m[11]*m[14] -
169 m[9]*m[6]*m[15] + m[9]*m[7]*m[14] +
170 m[13]*m[6]*m[11] - m[13]*m[7]*m[10];
171
172 inv[4] = -m[4]*m[10]*m[15] + m[4]*m[11]*m[14] +
173 m[8]*m[6]*m[15] - m[8]*m[7]*m[14] -
174 m[12]*m[6]*m[11] + m[12]*m[7]*m[10];
175
176 inv[8] = m[4]*m[9]*m[15] - m[4]*m[11]*m[13] -
177 m[8]*m[5]*m[15] + m[8]*m[7]*m[13] +
178 m[12]*m[5]*m[11] - m[12]*m[7]*m[9];
179
180 inv[12] = -m[4]*m[9]*m[14] + m[4]*m[10]*m[13] +
181 m[8]*m[5]*m[14] - m[8]*m[6]*m[13] -
182 m[12]*m[5]*m[10] + m[12]*m[6]*m[9];
183
184 inv[1] = -m[1]*m[10]*m[15] + m[1]*m[11]*m[14] +
185 m[9]*m[2]*m[15] - m[9]*m[3]*m[14] -
186 m[13]*m[2]*m[11] + m[13]*m[3]*m[10];
187
188 inv[5] = m[0]*m[10]*m[15] - m[0]*m[11]*m[14] -
189 m[8]*m[2]*m[15] + m[8]*m[3]*m[14] +
190 m[12]*m[2]*m[11] - m[12]*m[3]*m[10];
191
192 inv[9] = -m[0]*m[9]*m[15] + m[0]*m[11]*m[13] +
193 m[8]*m[1]*m[15] - m[8]*m[3]*m[13] -
194 m[12]*m[1]*m[11] + m[12]*m[3]*m[9];
195
196 inv[13] = m[0]*m[9]*m[14] - m[0]*m[10]*m[13] -
197 m[8]*m[1]*m[14] + m[8]*m[2]*m[13] +
198 m[12]*m[1]*m[10] - m[12]*m[2]*m[9];
199
200 inv[2] = m[1]*m[6]*m[15] - m[1]*m[7]*m[14] -
201 m[5]*m[2]*m[15] + m[5]*m[3]*m[14] +
202 m[13]*m[2]*m[7] - m[13]*m[3]*m[6];
203
204 inv[6] = -m[0]*m[6]*m[15] + m[0]*m[7]*m[14] +
205 m[4]*m[2]*m[15] - m[4]*m[3]*m[14] -
206 m[12]*m[2]*m[7] + m[12]*m[3]*m[6];
207
208 inv[10] = m[0]*m[5]*m[15] - m[0]*m[7]*m[13] -
209 m[4]*m[1]*m[15] + m[4]*m[3]*m[13] +
210 m[12]*m[1]*m[7] - m[12]*m[3]*m[5];
211
212 inv[14] = -m[0]*m[5]*m[14] + m[0]*m[6]*m[13] +
213 m[4]*m[1]*m[14] - m[4]*m[2]*m[13] -
214 m[12]*m[1]*m[6] + m[12]*m[2]*m[5];
215
216 inv[3] = -m[1]*m[6]*m[11] + m[1]*m[7]*m[10] +
217 m[5]*m[2]*m[11] - m[5]*m[3]*m[10] -
218 m[9]*m[2]*m[7] + m[9]*m[3]*m[6];
219
220 inv[7] = m[0]*m[6]*m[11] - m[0]*m[7]*m[10] -
221 m[4]*m[2]*m[11] + m[4]*m[3]*m[10] +
222 m[8]*m[2]*m[7] - m[8]*m[3]*m[6];
223
224 inv[11] = -m[0]*m[5]*m[11] + m[0]*m[7]*m[9] +
225 m[4]*m[1]*m[11] - m[4]*m[3]*m[9] -
226 m[8]*m[1]*m[7] + m[8]*m[3]*m[5];
227
228 inv[15] = m[0]*m[5]*m[10] - m[0]*m[6]*m[9] -
229 m[4]*m[1]*m[10] + m[4]*m[2]*m[9] +
230 m[8]*m[1]*m[6] - m[8]*m[2]*m[5];
231
232 det = m[0]*inv[0] + m[1]*inv[4] + m[2]*inv[8] + m[3]*inv[12];
233
234 if (det == 0)
235 return false;
236
237 det = 1.0 / det;
238 for (i = 0; i < 16; i++)
239 out[i] = inv[i] * det;
240
241 return true;
242}
243
244static inline vec2 vec2_scale(vec2 a, float b)
245{
246 return (vec2){a.x * b, a.y * b};
247}
248static inline vec3 vec3_scale(vec3 a, float b)
249{
250 return (vec3){a.x * b, a.y * b, a.z * b};
251}
252static inline vec4 vec4_scale(vec4 a, float b)
253{
254 return (vec4){a.x * b, a.y * b, a.z * b, a.w * b};
255}
256
257static inline float ivec3_dist(ivec3 a, ivec3 b)
258{
259 int ix = a.x - b.x, iy = a.y - b.y, iz = a.z - b.z;
260 return sqrtf(ix * ix + iy * iy + iz * iz);
261}
262static inline ivec3 ivec3_sub(ivec3 a, ivec3 b)
263{
264 return (ivec3){a.x - b.x, a.y - b.y, a.z - b.z};
265}
266static inline vec3 vec3_sub(vec3 a, vec3 b)
267{
268 return (vec3){a.x - b.x, a.y - b.y, a.z - b.z};
269}
270static inline vec2 vec2_sub(vec2 a, vec2 b)
271{
272 return (vec2){a.x - b.x, a.y - b.y};
273}
274static inline vec2 vec2_add(vec2 a, vec2 b)
275{
276 return (vec2){a.x + b.x, a.y + b.y};
277}
278static inline ivec3 ivec3_add(ivec3 a, ivec3 b)
279{
280 return (ivec3){a.x + b.x, a.y + b.y, a.z + b.z};
281}
282static inline vec3 ivec3_scale(ivec3 a, float b)
283{
284 return (vec3){a.x * b, a.y * b, a.z * b};
285}
286static inline vec3 vec3_add(vec3 a, vec3 b)
287{
288 return (vec3){a.x + b.x, a.y + b.y, a.z + b.z};
289}
290static inline vec4 vec4_add(vec4 a, vec4 b)
291{
292 return (vec4){a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w};
293}
294static inline vec4 vec4_sub(vec4 a, vec4 b)
295{
296 return (vec4){a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w};
297}
298static inline vec3 vec3_mul(vec3 a, vec3 b)
299{
300 return (vec3){a.x * b.x, a.y * b.y, a.z * b.z};
301}
302static inline vec4 vec4_mul(vec4 a, vec4 b)
303{
304 return (vec4){a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w};
305}
306static inline vec3 vec3_div(vec3 a, vec3 b)
307{
308 return (vec3){a.x / b.x, a.y / b.y, a.z / b.z};
309}
310static inline float vec3_dist2(vec3 a, vec3 b) {
311 vec3 d = vec3_sub(a, b);
312 return d.x*d.x + d.y*d.y + d.z*d.z;
313}
314static inline float vec3_dist2x2(vec3 a, vec3 b) {
315 vec3 d = vec3_sub(a, b);
316 return sqrtf(d.x*d.x + d.y*d.y + d.z*d.z);
317}
318static inline float vec3_dist(vec3 a, vec3 b) {
319 vec3 d = vec3_sub(a, b);
320 return fabsf(d.x) + fabsf(d.y) + fabsf(d.z);
321}
322static inline float vec3_dot(vec3 a, vec3 b) {
323 return a.x*b.x + a.y*b.y + a.z*b.z;
324}
325static inline float vec2_dot(vec2 a, vec2 b) {
326 return a.x*b.x + a.y*b.y;
327}
328static inline float vec3_length(vec3 v) {
329 return sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
330}
331
332static inline float vec4_length(vec4 v) {
333 return sqrtf(v.x*v.x + v.y*v.y + v.z*v.z + v.w * v.w);
334}
335
336static inline float vec2_length(vec2 v) {
337 return sqrtf(v.x*v.x + v.y*v.y);
338}
339
340static inline mat4 mat4_inverse(mat4 in) {
341
342 vec3 a = { in.a0, in.a1, in.a2 };
343 vec3 b = { in.b0, in.b1, in.b2 };
344 vec3 c = { in.c0, in.c1, in.c2 };
345 vec3 d = { in.d0, in.d1, in.d2 };
346
347 vec3 cross_bc = vec3_cross(b, c);
348 float det = vec3_dot(a, cross_bc);
349 float invDet = 1.0f / det;
350 vec3 ia = vec3_cross(b, c);
351 vec3 ib = vec3_cross(c, a);
352 vec3 ic = vec3_cross(a, b);
353
354 ia.x *= invDet; ia.y *= invDet; ia.z *= invDet;
355 ib.x *= invDet; ib.y *= invDet; ib.z *= invDet;
356 ic.x *= invDet; ic.y *= invDet; ic.z *= invDet;
357
358 vec3 id = {
359 -vec3_dot(ia, d),
360 -vec3_dot(ib, d),
361 -vec3_dot(ic, d)
362 };
363
364 mat4 out;
365 out.a0=ia.x; out.a1=ia.y; out.a2=ia.z; out.a3=0.0f;
366 out.b0=ib.x; out.b1=ib.y; out.b2=ib.z; out.b3=0.0f;
367 out.c0=ic.x; out.c1=ic.y; out.c2=ic.z; out.c3=0.0f;
368 out.d0=id.x; out.d1=id.y; out.d2=id.z; out.d3=1.0f;
369
370 return out;
371}
372static inline float lerp(float a, float b, float t) {
373 return a + (b - a) * t;
374}
375static inline vec3 vec3_lerp(vec3 a, vec3 b, float t) {
376 return vec3_add(a, vec3_scale(vec3_sub(b, a), t));
377}
378
379static inline float clamp(float a, float b, float c){
380
381 return fmaxf(b, fminf(a, c));
382}
383static inline vec4 mul_m4_v4(mat4 m, vec4 v) {
384 vec4 out;
385 out.x = m.a0 * v.x + m.b0 * v.y + m.c0 * v.z + m.d0 * v.w;
386 out.y = m.a1 * v.x + m.b1 * v.y + m.c1 * v.z + m.d1 * v.w;
387 out.z = m.a2 * v.x + m.b2 * v.y + m.c2 * v.z + m.d2 * v.w;
388 out.w = m.a3 * v.x + m.b3 * v.y + m.c3 * v.z + m.d3 * v.w;
389 return out;
390}
391
392static inline vec4 mul_m4_v4r(mat4 m, vec4 v) {
393 vec4 out;
394 out.x = m.a0 * v.x + m.a1 * v.y + m.a2 * v.z + m.a3 * v.w;
395 out.y = m.b0 * v.x + m.b1 * v.y + m.b2 * v.z + m.b3 * v.w;
396 out.z = m.c0 * v.x + m.c1 * v.y + m.c2 * v.z + m.c3 * v.w;
397 out.w = m.d0 * v.x + m.d1 * v.y + m.d2 * v.z + m.d3 * v.w;
398 return out;
399}
400
401static inline mat4 m4_mul(mat4 a, mat4 b) {
402 mat4 out;
403 vec4 b0 = { b.a0, b.a1, b.a2, b.a3 };
404 vec4 b1 = { b.b0, b.b1, b.b2, b.b3 };
405 vec4 b2 = { b.c0, b.c1, b.c2, b.c3 };
406 vec4 b3 = { b.d0, b.d1, b.d2, b.d3 };
407
408 vec4 r0 = mul_m4_v4(a, b0);
409 vec4 r1 = mul_m4_v4(a, b1);
410 vec4 r2 = mul_m4_v4(a, b2);
411 vec4 r3 = mul_m4_v4(a, b3);
412
413 out.a0 = r0.x; out.a1 = r0.y; out.a2 = r0.z; out.a3 = r0.w;
414 out.b0 = r1.x; out.b1 = r1.y; out.b2 = r1.z; out.b3 = r1.w;
415 out.c0 = r2.x; out.c1 = r2.y; out.c2 = r2.z; out.c3 = r2.w;
416 out.d0 = r3.x; out.d1 = r3.y; out.d2 = r3.z; out.d3 = r3.w;
417
418 return out;
419}
420
421static inline void mat4_mul(float out[16], const float a[16], const float b[16]) {
422 for (int row = 0; row < 4; ++row) {
423 for (int col = 0; col < 4; ++col) {
424 out[col * 4 + row] =
425 a[0 * 4 + row] * b[col * 4 + 0] +
426 a[1 * 4 + row] * b[col * 4 + 1] +
427 a[2 * 4 + row] * b[col * 4 + 2] +
428 a[3 * 4 + row] * b[col * 4 + 3];
429 }
430 }
431}
432
433#define MAX(a, b) (a > b ? (a) : (b))
434
435#define MIN(a, b) (a < b ? (a) : (b))
436
437
438static inline uint32_t expand_bits(uint32_t v)
439{
440 v = (v * 0x00010001u) & 0xFF0000FFu;
441 v = (v * 0x00000101u) & 0x0F00F00Fu;
442 v = (v * 0x00000011u) & 0xC30C30C3u;
443 v = (v * 0x00000005u) & 0x49249249u;
444 return v;
445}
446
447static inline uint32_t morton(uint32_t x, uint32_t y, uint32_t z)
448{
449 return expand_bits(x) | (expand_bits(y) << 1) | (expand_bits(z) << 2);
450}
451
452#endif
shared uint temp[1024]
vec3 vec3_cross(vec3 a, vec3 b)
Definition mathf.h:80
float vec3_dist(vec3 a, vec3 b)
Definition mathf.h:318
vec3 vec3_lerp(vec3 a, vec3 b, float t)
Definition mathf.h:375
float vec3_dot(vec3 a, vec3 b)
Definition mathf.h:322
float vec2_dot(vec2 a, vec2 b)
Definition mathf.h:325
mat3 rotation_matrix_r(vec3 right, vec3 up, vec3 forward)
Definition mathf.h:88
int round_up_power_of_two(int a, int max)
Definition mathf.h:11
bool m9_inverse(float inverse[9][9], float mat[9][9])
Definition mathf.h:108
ivec3 ivec3_sub(ivec3 a, ivec3 b)
Definition mathf.h:262
vec3 ivec3_scale(ivec3 a, float b)
Definition mathf.h:282
vec4 vec4_add(vec4 a, vec4 b)
Definition mathf.h:290
vec2 vec2_normalize(vec2 v)
Definition mathf.h:38
vec3 vec3_normalize(vec3 v)
Definition mathf.h:46
vec3 ivec3_normalize(ivec3 a)
Definition mathf.h:69
mat4 m4_mul(mat4 a, mat4 b)
Definition mathf.h:401
float ivec3_dist(ivec3 a, ivec3 b)
Definition mathf.h:257
vec3 vec3_scale(vec3 a, float b)
Definition mathf.h:248
vec4 mul_m4_v4(mat4 m, vec4 v)
Definition mathf.h:383
void mat4_mul(float out[16], const float a[16], const float b[16])
Definition mathf.h:421
float smoothstep(float a, float b, float c)
Definition mathf.h:27
vec3 vec3_mul(vec3 a, vec3 b)
Definition mathf.h:298
uint32_t morton(uint32_t x, uint32_t y, uint32_t z)
Definition mathf.h:447
vec3 vec3_div(vec3 a, vec3 b)
Definition mathf.h:306
vec4 mul_m4_v4r(mat4 m, vec4 v)
Definition mathf.h:392
vec4 vec4_mul(vec4 a, vec4 b)
Definition mathf.h:302
vec2 vec2_add(vec2 a, vec2 b)
Definition mathf.h:274
vec2 vec2_scale(vec2 a, float b)
Definition mathf.h:244
vec4 vec4_sub(vec4 a, vec4 b)
Definition mathf.h:294
vec3 vec3_sub(vec3 a, vec3 b)
Definition mathf.h:266
float vec4_length(vec4 v)
Definition mathf.h:332
vec3 vec3_add(vec3 a, vec3 b)
Definition mathf.h:286
vec4 vec4_normalize(vec4 v)
Definition mathf.h:55
vec2 vec2_sub(vec2 a, vec2 b)
Definition mathf.h:270
float lerp(float a, float b, float t)
Definition mathf.h:372
vec4 vec4_scale(vec4 a, float b)
Definition mathf.h:252
float clamp(float a, float b, float c)
Definition mathf.h:379
bool m4_inverse(float out[16], const float m[16])
Definition mathf.h:164
uint32_t expand_bits(uint32_t v)
Definition mathf.h:438
ivec3 ivec3_add(ivec3 a, ivec3 b)
Definition mathf.h:278
mat4 mat4_inverse(mat4 in)
Definition mathf.h:340
float vec2_length(vec2 v)
Definition mathf.h:336
float vec3_dist2(vec3 a, vec3 b)
Definition mathf.h:310
float vec3_dist2x2(vec3 a, vec3 b)
Definition mathf.h:314
float vec3_length(vec3 v)
Definition mathf.h:328
float x
Definition types.h:11
float a1
Definition types.h:21
float w
Definition types.h:13
float c2
Definition types.h:20
float a0
Definition types.h:20
float z
Definition types.h:13
float d3
Definition types.h:21
float c2
Definition types.h:21
float a1
Definition types.h:20
float b0
Definition types.h:21
float c1
Definition types.h:21
float x
Definition types.h:13
float c0
Definition types.h:20
float a2
Definition types.h:21
float x
Definition types.h:12
float b0
Definition types.h:20
float b1
Definition types.h:20
int y
Definition types.h:15
float b3
Definition types.h:21
float y
Definition types.h:11
float y
Definition types.h:13
float y
Definition types.h:12
float d1
Definition types.h:21
float a3
Definition types.h:21
int z
Definition types.h:15
float c0
Definition types.h:21
float z
Definition types.h:12
int x
Definition types.h:15
float b2
Definition types.h:21
float d0
Definition types.h:21
float b2
Definition types.h:20
float a2
Definition types.h:20
float a0
Definition types.h:21
float c3
Definition types.h:21
float c1
Definition types.h:20
float b1
Definition types.h:21
float d2
Definition types.h:21
Definition types.h:15
Definition types.h:20
Definition types.h:21
Definition types.h:11
Definition types.h:12
Definition types.h:13