map loader
[fishladder.git] / vg / vg_m.h
1 // Copyright (C) 2021 Harry Godden (hgn) - All Rights Reserved
2
3 // Util
4 // ==================================================================================================================
5
6 #define VG_PIf 3.14159265358979323846264338327950288f
7
8 // Simple min/max replacements
9 static inline float vg_minf( float a, float b )
10 {
11 return a < b? a: b;
12 }
13
14 static inline float vg_maxf( float a, float b )
15 {
16 return a > b? a: b;
17 }
18
19 static inline int vg_min( int a, int b )
20 {
21 return a < b? a: b;
22 }
23
24 static inline int vg_max( int a, int b )
25 {
26 return a > b? a: b;
27 }
28
29 // Convert degrees to radians
30 static inline float vg_rad( float deg )
31 {
32 return deg * VG_PIf / 180.0f;
33 }
34
35 // Vector 2
36 // ==================================================================================================================
37
38 static inline void v2_copy( v2f a, v2f b )
39 {
40 b[0] = a[0]; b[1] = a[1];
41 }
42
43 static inline void v2_minv( v2f a, v2f b, v2f dest )
44 {
45 dest[0] = vg_minf(a[0], b[0]);
46 dest[1] = vg_minf(a[1], b[1]);
47 }
48
49 static inline void v2_maxv( v2f a, v2f b, v2f dest )
50 {
51 dest[0] = vg_maxf(a[0], b[0]);
52 dest[1] = vg_maxf(a[1], b[1]);
53 }
54
55 static inline void v2_sub( v2f a, v2f b, v2f d )
56 {
57 d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
58 }
59
60 static inline float v2_cross( v2f a, v2f b )
61 {
62 return a[0] * b[1] - a[1] * b[0];
63 }
64
65 static inline void v2_add( v2f a, v2f b, v2f d )
66 {
67 d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
68 }
69
70 static inline void v2_muls( v2f a, float s, v2f d )
71 {
72 d[0] = a[0]*s; d[1] = a[1]*s;
73 }
74
75 static inline void v2_mul( v2f a, v2f b, v2f d )
76 {
77 d[0] = a[0]*b[0]; d[1] = a[1]*b[1];
78 }
79
80 static inline void v2_div( v2f a, v2f b, v2f d )
81 {
82 d[0] = a[0]/b[0]; d[1] = a[1]/b[1];
83 }
84
85 // Vector 3
86 // ==================================================================================================================
87
88 static inline void v3_zero( v3f a )
89 {
90 a[0] = 0.f; a[1] = 0.f; a[2] = 0.f;
91 }
92
93 static inline void v3_copy( v3f a, v3f b )
94 {
95 b[0] = a[0]; b[1] = a[1]; b[2] = a[2];
96 }
97
98 static inline void v3_add( v3f a, v3f b, v3f d )
99 {
100 d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2];
101 }
102
103 static inline void v3_sub( v3f a, v3f b, v3f d )
104 {
105 d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
106 }
107
108 static inline void v3_mul( v3f a, v3f b, v3f d )
109 {
110 d[0] = a[0]*b[0]; d[1] = a[1]*b[1]; d[2] = a[2]*b[2];
111 }
112
113 static inline void v3_div( v3f a, v3f b, v3f d )
114 {
115 d[0] = a[0]/b[0]; d[1] = a[1]/b[1]; d[2] = a[2]/b[2];
116 }
117
118 static inline void v3_muls( v3f a, float s, v3f d )
119 {
120 d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s;
121 }
122
123 static inline void v3_divs( v3f a, float s, v3f d )
124 {
125 d[0] = a[0]/s; d[1] = a[1]/s; d[2] = a[2]/s;
126 }
127
128 static inline void v3_muladds( v3f a, v3f b, float s, v3f d )
129 {
130 d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; d[2] = a[2]+b[2]*s;
131 }
132
133 static inline float v3_dot( v3f a, v3f b )
134 {
135 return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
136 }
137
138 static inline void v3_cross( v3f a, v3f b, v3f d )
139 {
140 d[0] = a[1] * b[2] - a[2] * b[1];
141 d[1] = a[2] * b[0] - a[0] * b[2];
142 d[2] = a[0] * b[1] - a[1] * b[0];
143 }
144
145 static inline float v3_length2( v3f a )
146 {
147 return v3_dot( a, a );
148 }
149
150 static inline float v3_length( v3f a )
151 {
152 return sqrtf( v3_length2( a ) );
153 }
154
155 static inline float v3_dist2( v3f a, v3f b )
156 {
157 v3f delta;
158 v3_sub( a, b, delta );
159 return v3_length2( delta );
160 }
161
162 static inline float v3_dist( v3f a, v3f b )
163 {
164 return sqrtf( v3_dist2( a, b ) );
165 }
166
167 static inline void v3_normalize( v3f a )
168 {
169 v3_muls( a, 1.f / v3_length( a ), a );
170 }
171
172 static inline float csr_lerpf( float a, float b, float t )
173 {
174 return a + t*(b-a);
175 }
176
177 static inline void v3_lerp( v3f a, v3f b, float t, v3f d )
178 {
179 d[0] = a[0] + t*(b[0]-a[0]);
180 d[1] = a[1] + t*(b[1]-a[1]);
181 d[2] = a[2] + t*(b[2]-a[2]);
182 }
183
184 static inline void v3_minv( v3f a, v3f b, v3f dest )
185 {
186 dest[0] = vg_minf(a[0], b[0]);
187 dest[1] = vg_minf(a[1], b[1]);
188 dest[2] = vg_minf(a[2], b[2]);
189 }
190
191 static inline void v3_maxv( v3f a, v3f b, v3f dest )
192 {
193 dest[0] = vg_maxf(a[0], b[0]);
194 dest[1] = vg_maxf(a[1], b[1]);
195 dest[2] = vg_maxf(a[2], b[2]);
196 }
197
198 static inline float v3_minf( v3f a )
199 {
200 return vg_minf( vg_minf( a[0], a[1] ), a[2] );
201 }
202
203 static inline float v3_maxf( v3f a )
204 {
205 return vg_maxf( vg_maxf( a[0], a[1] ), a[2] );
206 }
207
208 static inline void v3_fill( v3f a, float v )
209 {
210 a[0] = v;
211 a[1] = v;
212 a[2] = v;
213 }
214
215 // Vector 4
216 // ==================================================================================================================
217
218 static inline void v4_copy( v4f a, v4f b )
219 {
220 b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3];
221 }
222
223 static inline void v4_zero( v4f a )
224 {
225 a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f;
226 }
227
228 // Matrix 3x3
229 //======================================================================================================
230
231 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
232 { 0.0f, 1.0f, 0.0f, },\
233 { 0.0f, 0.0f, 1.0f, }}
234
235 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
236 { 0.0f, 0.0f, 0.0f, },\
237 { 0.0f, 0.0f, 0.0f, }}
238
239
240 static inline void m3x3_copy( m3x3f a, m3x3f b )
241 {
242 v3_copy( a[0], b[0] );
243 v3_copy( a[1], b[1] );
244 v3_copy( a[2], b[2] );
245 }
246
247 static inline void m3x3_identity( m3x3f a )
248 {
249 m3x3f id = M3X3_IDENTITY;
250 m3x3_copy( id, a );
251 }
252
253 static inline void m3x3_zero( m3x3f a )
254 {
255 m3x3f z = M3X3_ZERO;
256 m3x3_copy( z, a );
257 }
258
259 static inline void m3x3_inv( m3x3f src, m3x3f dest )
260 {
261 float a = src[0][0], b = src[0][1], c = src[0][2],
262 d = src[1][0], e = src[1][1], f = src[1][2],
263 g = src[2][0], h = src[2][1], i = src[2][2];
264
265 float det = 1.f /
266 (+a*(e*i-h*f)
267 -b*(d*i-f*g)
268 +c*(d*h-e*g));
269
270 dest[0][0] = (e*i-h*f)*det;
271 dest[0][1] = -(b*i-c*h)*det;
272 dest[0][2] = (b*f-c*e)*det;
273 dest[1][0] = -(d*i-f*g)*det;
274 dest[1][1] = (a*i-c*g)*det;
275 dest[1][2] = -(a*f-d*c)*det;
276 dest[2][0] = (d*h-g*e)*det;
277 dest[2][1] = -(a*h-g*b)*det;
278 dest[2][2] = (a*e-d*b)*det;
279 }
280
281 static inline void m3x3_transpose( m3x3f src, m3x3f dest )
282 {
283 float a = src[0][0], b = src[0][1], c = src[0][2],
284 d = src[1][0], e = src[1][1], f = src[1][2],
285 g = src[2][0], h = src[2][1], i = src[2][2];
286
287 dest[0][0] = a;
288 dest[0][1] = d;
289 dest[0][2] = g;
290 dest[1][0] = b;
291 dest[1][1] = e;
292 dest[1][2] = h;
293 dest[2][0] = c;
294 dest[2][1] = f;
295 dest[2][2] = i;
296 }
297
298 static inline void m3x3_mul( m3x3f a, m3x3f b, m3x3f d )
299 {
300 float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
301 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
302 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],
303
304 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
305 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
306 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2];
307
308 d[0][0] = a00*b00 + a10*b01 + a20*b02;
309 d[0][1] = a01*b00 + a11*b01 + a21*b02;
310 d[0][2] = a02*b00 + a12*b01 + a22*b02;
311 d[1][0] = a00*b10 + a10*b11 + a20*b12;
312 d[1][1] = a01*b10 + a11*b11 + a21*b12;
313 d[1][2] = a02*b10 + a12*b11 + a22*b12;
314 d[2][0] = a00*b20 + a10*b21 + a20*b22;
315 d[2][1] = a01*b20 + a11*b21 + a21*b22;
316 d[2][2] = a02*b20 + a12*b21 + a22*b22;
317 }
318
319 static inline void m3x3_mulv( m3x3f m, v3f v, v3f d )
320 {
321 v3f res;
322
323 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
324 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
325 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
326
327 v3_copy( res, d );
328 }
329
330 static inline void m3x3_projection( m3x3f dst, float const left, float const right, float const bottom, float const top )
331 {
332 float rl, tb;
333
334 m3x3_zero( dst );
335
336 rl = 1.0f / (right - left);
337 tb = 1.0f / (top - bottom);
338
339 dst[0][0] = 2.0f * rl;
340 dst[1][1] = 2.0f * tb;
341 dst[2][2] = 1.0f;
342 }
343
344 static inline void m3x3_translate( m3x3f m, v3f v )
345 {
346 m[2][0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0];
347 m[2][1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1];
348 m[2][2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2];
349 }
350
351 static inline void m3x3_scale( m3x3f m, v3f v )
352 {
353 m[0][0] = m[0][0] * v[0];
354 m[0][1] = m[0][1] * v[0];
355 m[0][2] = m[0][2] * v[0];
356
357 m[1][0] = m[1][0] * v[1];
358 m[1][1] = m[1][1] * v[1];
359 m[1][2] = m[1][2] * v[1];
360 }
361
362 static inline void m3x3_rotate( m3x3f m, float angle )
363 {
364 float m00 = m[0][0], m10 = m[1][0],
365 m01 = m[0][1], m11 = m[1][1],
366 m02 = m[0][2], m12 = m[1][2];
367 float c, s;
368
369 s = sinf( angle );
370 c = cosf( angle );
371
372 m[0][0] = m00 * c + m10 * s;
373 m[0][1] = m01 * c + m11 * s;
374 m[0][2] = m02 * c + m12 * s;
375
376 m[1][0] = m00 * -s + m10 * c;
377 m[1][1] = m01 * -s + m11 * c;
378 m[1][2] = m02 * -s + m12 * c;
379 }
380
381 // Matrix 4x3
382 // ==================================================================================================================
383
384 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
385 { 0.0f, 1.0f, 0.0f, },\
386 { 0.0f, 0.0f, 1.0f, },\
387 { 0.0f, 0.0f, 0.0f }}
388
389 static inline void m4x3_to_3x3( m4x3f a, m3x3f b )
390 {
391 v3_copy( a[0], b[0] );
392 v3_copy( a[1], b[1] );
393 v3_copy( a[2], b[2] );
394 }
395
396 static inline void m4x3_copy( m4x3f a, m4x3f b )
397 {
398 v3_copy( a[0], b[0] );
399 v3_copy( a[1], b[1] );
400 v3_copy( a[2], b[2] );
401 v3_copy( a[3], b[3] );
402 }
403
404 static inline void m4x3_identity( m4x3f a )
405 {
406 m4x3f id = M4X3_IDENTITY;
407 m4x3_copy( id, a );
408 }
409
410 static inline void m4x3_mul( m4x3f a, m4x3f b, m4x3f d )
411 {
412 float
413 a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
414 a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
415 a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],
416 a30 = a[3][0], a31 = a[3][1], a32 = a[3][2],
417 b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
418 b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
419 b20 = b[2][0], b21 = b[2][1], b22 = b[2][2],
420 b30 = b[3][0], b31 = b[3][1], b32 = b[3][2];
421
422 d[0][0] = a00*b00 + a10*b01 + a20*b02;
423 d[0][1] = a01*b00 + a11*b01 + a21*b02;
424 d[0][2] = a02*b00 + a12*b01 + a22*b02;
425 d[1][0] = a00*b10 + a10*b11 + a20*b12;
426 d[1][1] = a01*b10 + a11*b11 + a21*b12;
427 d[1][2] = a02*b10 + a12*b11 + a22*b12;
428 d[2][0] = a00*b20 + a10*b21 + a20*b22;
429 d[2][1] = a01*b20 + a11*b21 + a21*b22;
430 d[2][2] = a02*b20 + a12*b21 + a22*b22;
431 d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30;
432 d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31;
433 d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32;
434 }
435
436 static inline void m4x3_mulv( m4x3f m, v3f v, v3f d )
437 {
438 v3f res;
439
440 res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0];
441 res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1];
442 res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2];
443
444 v3_copy( res, d );
445 }
446
447 // Affine transforms
448 // ====================================================================================================================
449
450 static inline void m4x3_translate( m4x3f m, v3f v )
451 {
452 v3_muladds( m[3], m[0], v[0], m[3] );
453 v3_muladds( m[3], m[1], v[1], m[3] );
454 v3_muladds( m[3], m[2], v[2], m[3] );
455 }
456
457 static inline void m4x3_scale( m4x3f m, float s )
458 {
459 v3_muls( m[0], s, m[0] );
460 v3_muls( m[1], s, m[1] );
461 v3_muls( m[2], s, m[2] );
462 }
463
464 static inline void m4x3_rotate_x( m4x3f m, float angle )
465 {
466 m4x3f t = M4X3_IDENTITY;
467 float c, s;
468
469 c = cosf( angle );
470 s = sinf( angle );
471
472 t[1][1] = c;
473 t[1][2] = s;
474 t[2][1] = -s;
475 t[2][2] = c;
476
477 m4x3_mul( m, t, m );
478 }
479
480 static inline void m4x3_rotate_y( m4x3f m, float angle )
481 {
482 m4x3f t = M4X3_IDENTITY;
483 float c, s;
484
485 c = cosf( angle );
486 s = sinf( angle );
487
488 t[0][0] = c;
489 t[0][2] = -s;
490 t[2][0] = s;
491 t[2][2] = c;
492
493 m4x3_mul( m, t, m );
494 }
495
496 static inline void m4x3_rotate_z( m4x3f m, float angle )
497 {
498 m4x3f t = M4X3_IDENTITY;
499 float c, s;
500
501 c = cosf( angle );
502 s = sinf( angle );
503
504 t[0][0] = c;
505 t[0][1] = s;
506 t[1][0] = -s;
507 t[1][1] = c;
508
509 m4x3_mul( m, t, m );
510 }
511
512 // Warning: These functions are unoptimized..
513 static inline void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point )
514 {
515 v3f v;
516 m4x3_mulv( m, point, v );
517
518 v3_minv( box[0], v, box[0] );
519 v3_maxv( box[1], v, box[1] );
520 }
521
522 static inline void box_concat( boxf a, boxf b )
523 {
524 v3_minv( a[0], b[0], a[0] );
525 v3_maxv( a[1], b[1], a[1] );
526 }
527
528 static inline void box_copy( boxf a, boxf b )
529 {
530 v3_copy( a[0], b[0] );
531 v3_copy( a[1], b[1] );
532 }
533
534 static inline void m4x3_transform_aabb( m4x3f m, boxf box )
535 {
536 v3f a; v3f b;
537
538 v3_copy( box[0], a );
539 v3_copy( box[1], b );
540 v3_fill( box[0], INFINITY );
541 v3_fill( box[1], -INFINITY );
542
543 m4x3_expand_aabb_point( m, box, a );
544 m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], a[2] } );
545 m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], a[2] } );
546 m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], a[2] } );
547 m4x3_expand_aabb_point( m, box, b );
548 m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], b[2] } );
549 m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], b[2] } );
550 m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], b[2] } );
551 }
552
553 // Planes (double precision)
554 // ==================================================================================================================
555
556 static inline void tri_to_plane( double a[3], double b[3], double c[3], double p[4] )
557 {
558 double edge0[3];
559 double edge1[3];
560 double l;
561
562 edge0[0] = b[0] - a[0];
563 edge0[1] = b[1] - a[1];
564 edge0[2] = b[2] - a[2];
565
566 edge1[0] = c[0] - a[0];
567 edge1[1] = c[1] - a[1];
568 edge1[2] = c[2] - a[2];
569
570 p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1];
571 p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2];
572 p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0];
573
574 l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
575 p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l;
576
577 p[0] = p[0] / l;
578 p[1] = p[1] / l;
579 p[2] = p[2] / l;
580 }
581
582 static inline int plane_intersect( double a[4], double b[4], double c[4], double p[4] )
583 {
584 double const epsilon = 1e-8f;
585
586 double x[3];
587 double d;
588
589 x[0] = a[1] * b[2] - a[2] * b[1];
590 x[1] = a[2] * b[0] - a[0] * b[2];
591 x[2] = a[0] * b[1] - a[1] * b[0];
592
593 d = x[0] * c[0] + x[1] * c[1] + x[2] * c[2];
594
595 if( d < epsilon && d > -epsilon ) return 0;
596
597 p[0] = (b[1] * c[2] - b[2] * c[1]) * -a[3];
598 p[1] = (b[2] * c[0] - b[0] * c[2]) * -a[3];
599 p[2] = (b[0] * c[1] - b[1] * c[0]) * -a[3];
600
601 p[0] += (c[1] * a[2] - c[2] * a[1]) * -b[3];
602 p[1] += (c[2] * a[0] - c[0] * a[2]) * -b[3];
603 p[2] += (c[0] * a[1] - c[1] * a[0]) * -b[3];
604
605 p[0] += (a[1] * b[2] - a[2] * b[1]) * -c[3];
606 p[1] += (a[2] * b[0] - a[0] * b[2]) * -c[3];
607 p[2] += (a[0] * b[1] - a[1] * b[0]) * -c[3];
608
609 p[0] = -p[0] / d;
610 p[1] = -p[1] / d;
611 p[2] = -p[2] / d;
612
613 return 1;
614 }
615
616 static inline double plane_polarity( double p[4], double a[3] )
617 {
618 return
619 (a[0] * p[0] + a[1] * p[1] + a[2] * p[2])
620 -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2])
621 ;
622 }