1 // Copyright (C) 2021 Harry Godden (hgn) - All Rights Reserved
4 // ==================================================================================================================
6 #define VG_PIf 3.14159265358979323846264338327950288f
7 #define VG_TAUf 6.28318530717958647692528676655900576f
9 // Simple min/max replacements
10 static inline float vg_minf( float a
, float b
)
15 static inline float vg_maxf( float a
, float b
)
20 static inline float vg_clampf( float a
, float min
, float max
)
22 return vg_minf( max
, vg_maxf( a
, min
) );
25 #define VG_MIN( A, B ) ((A)<(B)?(A):(B))
26 #define VG_MAX( A, B ) ((A)>(B)?(A):(B))
28 // Hopefully deprecate this!!
29 static inline int vg_min( int a
, int b
)
34 static inline int vg_max( int a
, int b
)
39 // Convert degrees to radians
40 static inline float vg_rad( float deg
)
42 return deg
* VG_PIf
/ 180.0f
;
46 // ==================================================================================================================
48 static inline void v2_copy( v2f a
, v2f b
)
50 b
[0] = a
[0]; b
[1] = a
[1];
53 static inline void v2i_copy( v2i a
, v2i b
)
55 b
[0] = a
[0]; b
[1] = a
[1];
58 static inline int v2i_eq( v2i a
, v2i b
)
60 return ((a
[0] == b
[0]) && (a
[1] == b
[1]));
63 static inline void v2i_add( v2i a
, v2i b
, v2i d
)
65 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
68 static inline void v2i_sub( v2i a
, v2i b
, v2i d
)
70 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
73 static inline void v2_minv( v2f a
, v2f b
, v2f dest
)
75 dest
[0] = vg_minf(a
[0], b
[0]);
76 dest
[1] = vg_minf(a
[1], b
[1]);
79 static inline void v2_maxv( v2f a
, v2f b
, v2f dest
)
81 dest
[0] = vg_maxf(a
[0], b
[0]);
82 dest
[1] = vg_maxf(a
[1], b
[1]);
85 static inline void v2_sub( v2f a
, v2f b
, v2f d
)
87 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
90 static inline float v2_cross( v2f a
, v2f b
)
92 return a
[0] * b
[1] - a
[1] * b
[0];
95 static inline void v2_add( v2f a
, v2f b
, v2f d
)
97 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
100 static inline void v2_muls( v2f a
, float s
, v2f d
)
102 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
105 static inline void v2_divs( v2f a
, float s
, v2f d
)
107 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
;
111 static inline void v2_mul( v2f a
, v2f b
, v2f d
)
113 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1];
116 static inline void v2_div( v2f a
, v2f b
, v2f d
)
118 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
121 static inline void v2_muladds( v2f a
, v2f b
, float s
, v2f d
)
123 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
;
126 static inline float v2_length2( v2f a
)
128 return a
[0]*a
[0] + a
[1]*a
[1];
131 static inline float v2_length( v2f a
)
133 return sqrtf( v2_length2( a
) );
136 static inline float v2_dist2( v2f a
, v2f b
)
139 v2_sub( a
, b
, delta
);
140 return v2_length2( delta
);
143 static inline float v2_dist( v2f a
, v2f b
)
145 return sqrtf( v2_dist2( a
, b
) );
148 static inline void v2_lerp( v2f a
, v2f b
, float t
, v2f d
)
150 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
151 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
155 // ==================================================================================================================
157 static inline void v3_zero( v3f a
)
159 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
162 static inline void v3_copy( v3f a
, v3f b
)
164 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
167 static inline void v3_add( v3f a
, v3f b
, v3f d
)
169 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
172 static inline void v3_sub( v3f a
, v3f b
, v3f d
)
174 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
177 static inline void v3_mul( v3f a
, v3f b
, v3f d
)
179 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
182 static inline void v3_div( v3f a
, v3f b
, v3f d
)
184 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1]; d
[2] = a
[2]/b
[2];
187 static inline void v3_muls( v3f a
, float s
, v3f d
)
189 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
192 static inline void v3_divs( v3f a
, float s
, v3f d
)
194 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
; d
[2] = a
[2]/s
;
197 static inline void v3_muladds( v3f a
, v3f b
, float s
, v3f d
)
199 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
202 static inline float v3_dot( v3f a
, v3f b
)
204 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
207 static inline void v3_cross( v3f a
, v3f b
, v3f d
)
209 d
[0] = a
[1] * b
[2] - a
[2] * b
[1];
210 d
[1] = a
[2] * b
[0] - a
[0] * b
[2];
211 d
[2] = a
[0] * b
[1] - a
[1] * b
[0];
214 static inline float v3_length2( v3f a
)
216 return v3_dot( a
, a
);
219 static inline float v3_length( v3f a
)
221 return sqrtf( v3_length2( a
) );
224 static inline float v3_dist2( v3f a
, v3f b
)
227 v3_sub( a
, b
, delta
);
228 return v3_length2( delta
);
231 static inline float v3_dist( v3f a
, v3f b
)
233 return sqrtf( v3_dist2( a
, b
) );
236 static inline void v3_normalize( v3f a
)
238 v3_muls( a
, 1.f
/ v3_length( a
), a
);
241 static inline float vg_lerpf( float a
, float b
, float t
)
246 static inline void v3_lerp( v3f a
, v3f b
, float t
, v3f d
)
248 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
249 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
250 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
253 static inline void v3_minv( v3f a
, v3f b
, v3f dest
)
255 dest
[0] = vg_minf(a
[0], b
[0]);
256 dest
[1] = vg_minf(a
[1], b
[1]);
257 dest
[2] = vg_minf(a
[2], b
[2]);
260 static inline void v3_maxv( v3f a
, v3f b
, v3f dest
)
262 dest
[0] = vg_maxf(a
[0], b
[0]);
263 dest
[1] = vg_maxf(a
[1], b
[1]);
264 dest
[2] = vg_maxf(a
[2], b
[2]);
267 static inline float v3_minf( v3f a
)
269 return vg_minf( vg_minf( a
[0], a
[1] ), a
[2] );
272 static inline float v3_maxf( v3f a
)
274 return vg_maxf( vg_maxf( a
[0], a
[1] ), a
[2] );
277 static inline void v3_fill( v3f a
, float v
)
285 // ==================================================================================================================
287 static inline void v4_copy( v4f a
, v4f b
)
289 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
292 static inline void v4_zero( v4f a
)
294 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
298 // ===========================================================================================================
300 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
303 #define M2X2_ZERO {{0.0f, 0.0f, }, \
306 static inline void m2x2_copy( m2x2f a
, m2x2f b
)
308 v2_copy( a
[0], b
[0] );
309 v2_copy( a
[1], b
[1] );
312 static inline void m2x2_identity( m2x2f a
)
314 m2x2f id
= M2X2_INDENTIY
;
318 static inline void m2x2_create_rotation( m2x2f a
, float theta
)
332 //======================================================================================================
334 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
335 { 0.0f, 1.0f, 0.0f, },\
336 { 0.0f, 0.0f, 1.0f, }}
338 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
339 { 0.0f, 0.0f, 0.0f, },\
340 { 0.0f, 0.0f, 0.0f, }}
343 static inline void m3x3_copy( m3x3f a
, m3x3f b
)
345 v3_copy( a
[0], b
[0] );
346 v3_copy( a
[1], b
[1] );
347 v3_copy( a
[2], b
[2] );
350 static inline void m3x3_identity( m3x3f a
)
352 m3x3f id
= M3X3_IDENTITY
;
356 static inline void m3x3_zero( m3x3f a
)
362 static inline void m3x3_inv( m3x3f src
, m3x3f dest
)
364 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
365 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
366 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
373 dest
[0][0] = (e
*i
-h
*f
)*det
;
374 dest
[0][1] = -(b
*i
-c
*h
)*det
;
375 dest
[0][2] = (b
*f
-c
*e
)*det
;
376 dest
[1][0] = -(d
*i
-f
*g
)*det
;
377 dest
[1][1] = (a
*i
-c
*g
)*det
;
378 dest
[1][2] = -(a
*f
-d
*c
)*det
;
379 dest
[2][0] = (d
*h
-g
*e
)*det
;
380 dest
[2][1] = -(a
*h
-g
*b
)*det
;
381 dest
[2][2] = (a
*e
-d
*b
)*det
;
384 static inline void m3x3_transpose( m3x3f src
, m3x3f dest
)
386 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
387 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
388 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
401 static inline void m3x3_mul( m3x3f a
, m3x3f b
, m3x3f d
)
403 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
404 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
405 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
407 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
408 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
409 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2];
411 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
412 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
413 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
414 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
415 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
416 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
417 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
418 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
419 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
422 static inline void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
426 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
427 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
428 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
433 static inline void m3x3_projection( m3x3f dst
, float const left
, float const right
, float const bottom
, float const top
)
439 rl
= 1.0f
/ (right
- left
);
440 tb
= 1.0f
/ (top
- bottom
);
442 dst
[0][0] = 2.0f
* rl
;
443 dst
[1][1] = 2.0f
* tb
;
447 static inline void m3x3_translate( m3x3f m
, v3f v
)
449 m
[2][0] = m
[0][0] * v
[0] + m
[1][0] * v
[1] + m
[2][0];
450 m
[2][1] = m
[0][1] * v
[0] + m
[1][1] * v
[1] + m
[2][1];
451 m
[2][2] = m
[0][2] * v
[0] + m
[1][2] * v
[1] + m
[2][2];
454 static inline void m3x3_scale( m3x3f m
, v3f v
)
456 m
[0][0] = m
[0][0] * v
[0];
457 m
[0][1] = m
[0][1] * v
[0];
458 m
[0][2] = m
[0][2] * v
[0];
460 m
[1][0] = m
[1][0] * v
[1];
461 m
[1][1] = m
[1][1] * v
[1];
462 m
[1][2] = m
[1][2] * v
[1];
465 static inline void m3x3_rotate( m3x3f m
, float angle
)
467 float m00
= m
[0][0], m10
= m
[1][0],
468 m01
= m
[0][1], m11
= m
[1][1],
469 m02
= m
[0][2], m12
= m
[1][2];
475 m
[0][0] = m00
* c
+ m10
* s
;
476 m
[0][1] = m01
* c
+ m11
* s
;
477 m
[0][2] = m02
* c
+ m12
* s
;
479 m
[1][0] = m00
* -s
+ m10
* c
;
480 m
[1][1] = m01
* -s
+ m11
* c
;
481 m
[1][2] = m02
* -s
+ m12
* c
;
485 // ==================================================================================================================
487 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
488 { 0.0f, 1.0f, 0.0f, },\
489 { 0.0f, 0.0f, 1.0f, },\
490 { 0.0f, 0.0f, 0.0f }}
492 static inline void m4x3_to_3x3( m4x3f a
, m3x3f b
)
494 v3_copy( a
[0], b
[0] );
495 v3_copy( a
[1], b
[1] );
496 v3_copy( a
[2], b
[2] );
499 static inline void m4x3_copy( m4x3f a
, m4x3f b
)
501 v3_copy( a
[0], b
[0] );
502 v3_copy( a
[1], b
[1] );
503 v3_copy( a
[2], b
[2] );
504 v3_copy( a
[3], b
[3] );
507 static inline void m4x3_identity( m4x3f a
)
509 m4x3f id
= M4X3_IDENTITY
;
513 static inline void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
516 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
517 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
518 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
519 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
520 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
521 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
522 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
523 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
525 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
526 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
527 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
528 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
529 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
530 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
531 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
532 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
533 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
534 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
535 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
536 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
539 static inline void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
543 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
544 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
545 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
551 // ====================================================================================================================
553 static inline void m4x3_translate( m4x3f m
, v3f v
)
555 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
556 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
557 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
560 static inline void m4x3_scale( m4x3f m
, float s
)
562 v3_muls( m
[0], s
, m
[0] );
563 v3_muls( m
[1], s
, m
[1] );
564 v3_muls( m
[2], s
, m
[2] );
567 static inline void m4x3_rotate_x( m4x3f m
, float angle
)
569 m4x3f t
= M4X3_IDENTITY
;
583 static inline void m4x3_rotate_y( m4x3f m
, float angle
)
585 m4x3f t
= M4X3_IDENTITY
;
599 static inline void m4x3_rotate_z( m4x3f m
, float angle
)
601 m4x3f t
= M4X3_IDENTITY
;
615 // Warning: These functions are unoptimized..
616 static inline void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
619 m4x3_mulv( m
, point
, v
);
621 v3_minv( box
[0], v
, box
[0] );
622 v3_maxv( box
[1], v
, box
[1] );
625 static inline void box_concat( boxf a
, boxf b
)
627 v3_minv( a
[0], b
[0], a
[0] );
628 v3_maxv( a
[1], b
[1], a
[1] );
631 static inline void box_copy( boxf a
, boxf b
)
633 v3_copy( a
[0], b
[0] );
634 v3_copy( a
[1], b
[1] );
637 static inline void m4x3_transform_aabb( m4x3f m
, boxf box
)
641 v3_copy( box
[0], a
);
642 v3_copy( box
[1], b
);
643 v3_fill( box
[0], INFINITY
);
644 v3_fill( box
[1], -INFINITY
);
646 m4x3_expand_aabb_point( m
, box
, a
);
647 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
648 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
649 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
650 m4x3_expand_aabb_point( m
, box
, b
);
651 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
652 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
653 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
656 // Planes (double precision)
657 // ==================================================================================================================
659 static inline void tri_to_plane( double a
[3], double b
[3], double c
[3], double p
[4] )
665 edge0
[0] = b
[0] - a
[0];
666 edge0
[1] = b
[1] - a
[1];
667 edge0
[2] = b
[2] - a
[2];
669 edge1
[0] = c
[0] - a
[0];
670 edge1
[1] = c
[1] - a
[1];
671 edge1
[2] = c
[2] - a
[2];
673 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
674 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
675 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
677 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
678 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
685 static inline int plane_intersect( double a
[4], double b
[4], double c
[4], double p
[4] )
687 double const epsilon
= 1e-8f
;
692 x
[0] = a
[1] * b
[2] - a
[2] * b
[1];
693 x
[1] = a
[2] * b
[0] - a
[0] * b
[2];
694 x
[2] = a
[0] * b
[1] - a
[1] * b
[0];
696 d
= x
[0] * c
[0] + x
[1] * c
[1] + x
[2] * c
[2];
698 if( d
< epsilon
&& d
> -epsilon
) return 0;
700 p
[0] = (b
[1] * c
[2] - b
[2] * c
[1]) * -a
[3];
701 p
[1] = (b
[2] * c
[0] - b
[0] * c
[2]) * -a
[3];
702 p
[2] = (b
[0] * c
[1] - b
[1] * c
[0]) * -a
[3];
704 p
[0] += (c
[1] * a
[2] - c
[2] * a
[1]) * -b
[3];
705 p
[1] += (c
[2] * a
[0] - c
[0] * a
[2]) * -b
[3];
706 p
[2] += (c
[0] * a
[1] - c
[1] * a
[0]) * -b
[3];
708 p
[0] += (a
[1] * b
[2] - a
[2] * b
[1]) * -c
[3];
709 p
[1] += (a
[2] * b
[0] - a
[0] * b
[2]) * -c
[3];
710 p
[2] += (a
[0] * b
[1] - a
[1] * b
[0]) * -c
[3];
719 static inline double plane_polarity( double p
[4], double a
[3] )
722 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
723 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])