7403320df29eb5d3ba2d8a0c5f778172d43c4a2c
1 /* Copyright (C) 2021-2022 Harry Godden (hgn) - All Rights Reserved */
6 #include "vg_platform.h"
10 #define VG_PIf 3.14159265358979323846264338327950288f
11 #define VG_TAUf 6.28318530717958647692528676655900576f
13 static inline float vg_minf( float a
, float b
)
18 static inline float vg_maxf( float a
, float b
)
23 static inline float vg_clampf( float a
, float min
, float max
)
25 return vg_minf( max
, vg_maxf( a
, min
) );
28 static inline float vg_signf( float a
)
30 return a
< 0.0f
? -1.0f
: 1.0f
;
33 static inline float vg_fractf( float a
)
35 return a
- floorf( a
);
38 static float stable_force( float current
, float diff
)
40 float fnew
= current
+ diff
;
42 if( fnew
* current
< 0.0f
)
48 static inline int vg_min( int a
, int b
)
53 static inline int vg_max( int a
, int b
)
58 static inline float vg_rad( float deg
)
60 return deg
* VG_PIf
/ 180.0f
;
66 static inline void v2_copy( v2f a
, v2f b
)
68 b
[0] = a
[0]; b
[1] = a
[1];
71 static inline void v2_zero( v2f a
)
73 a
[0] = 0.f
; a
[1] = 0.f
;
76 static inline void v2i_copy( v2i a
, v2i b
)
78 b
[0] = a
[0]; b
[1] = a
[1];
81 static inline int v2i_eq( v2i a
, v2i b
)
83 return ((a
[0] == b
[0]) && (a
[1] == b
[1]));
86 static inline void v2i_add( v2i a
, v2i b
, v2i d
)
88 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
91 static inline void v2i_sub( v2i a
, v2i b
, v2i d
)
93 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
96 static inline void v2_minv( v2f a
, v2f b
, v2f dest
)
98 dest
[0] = vg_minf(a
[0], b
[0]);
99 dest
[1] = vg_minf(a
[1], b
[1]);
102 static inline void v2_maxv( v2f a
, v2f b
, v2f dest
)
104 dest
[0] = vg_maxf(a
[0], b
[0]);
105 dest
[1] = vg_maxf(a
[1], b
[1]);
108 static inline void v2_sub( v2f a
, v2f b
, v2f d
)
110 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
113 static inline float v2_dot( v2f a
, v2f b
)
115 return a
[0] * b
[0] + a
[1] * b
[1];
118 static inline float v2_cross( v2f a
, v2f b
)
120 return a
[0]*b
[1] - a
[1]*b
[0];
123 static inline void v2_add( v2f a
, v2f b
, v2f d
)
125 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
128 static inline void v2_muls( v2f a
, float s
, v2f d
)
130 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
133 static inline void v2_divs( v2f a
, float s
, v2f d
)
135 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
;
138 static inline void v2_mul( v2f a
, v2f b
, v2f d
)
144 static inline void v2_div( v2f a
, v2f b
, v2f d
)
146 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
149 static inline void v2_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
151 d
[0] = a
[0]+b
[0]*s
[0];
152 d
[1] = a
[1]+b
[1]*s
[1];
155 static inline void v2_muladds( v2f a
, v2f b
, float s
, v2f d
)
161 static inline float v2_length2( v2f a
)
163 return a
[0]*a
[0] + a
[1]*a
[1];
166 static inline float v2_length( v2f a
)
168 return sqrtf( v2_length2( a
) );
171 static inline float v2_dist2( v2f a
, v2f b
)
174 v2_sub( a
, b
, delta
);
175 return v2_length2( delta
);
178 static inline float v2_dist( v2f a
, v2f b
)
180 return sqrtf( v2_dist2( a
, b
) );
183 static inline void v2_lerp( v2f a
, v2f b
, float t
, v2f d
)
185 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
186 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
189 static inline void v2_normalize( v2f a
)
191 v2_muls( a
, 1.0f
/ v2_length( a
), a
);
194 static void v2_normalize_clamp( v2f a
)
196 float l2
= v2_length2( a
);
198 v2_muls( a
, 1.0f
/sqrtf(l2
), a
);
201 static inline void v2_floor( v2f a
, v2f b
)
203 b
[0] = floorf( a
[0] );
204 b
[1] = floorf( a
[1] );
210 static inline void v3_zero( v3f a
)
212 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
215 static inline void v3_copy( v3f a
, v3f b
)
217 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
220 static inline void v3_add( v3f a
, v3f b
, v3f d
)
222 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
225 static inline void v3_sub( v3f a
, v3f b
, v3f d
)
227 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
230 static inline void v3_mul( v3f a
, v3f b
, v3f d
)
232 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
235 static inline void v3_div( v3f a
, v3f b
, v3f d
)
237 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1]; d
[2] = a
[2]/b
[2];
240 static inline void v3_muls( v3f a
, float s
, v3f d
)
242 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
245 static inline void v3_divs( v3f a
, float s
, v3f d
)
247 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
; d
[2] = a
[2]/s
;
250 static inline void v3_muladds( v3f a
, v3f b
, float s
, v3f d
)
252 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
255 static inline void v3_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
257 d
[0] = a
[0]+b
[0]*s
[0];
258 d
[1] = a
[1]+b
[1]*s
[1];
259 d
[2] = a
[2]+b
[2]*s
[2];
262 static inline float v3_dot( v3f a
, v3f b
)
264 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
267 static inline void v3_cross( v3f a
, v3f b
, v3f dest
)
270 d
[0] = a
[1]*b
[2] - a
[2]*b
[1];
271 d
[1] = a
[2]*b
[0] - a
[0]*b
[2];
272 d
[2] = a
[0]*b
[1] - a
[1]*b
[0];
276 static inline float v3_length2( v3f a
)
278 return v3_dot( a
, a
);
281 static inline float v3_length( v3f a
)
283 return sqrtf( v3_length2( a
) );
286 static inline float v3_dist2( v3f a
, v3f b
)
289 v3_sub( a
, b
, delta
);
290 return v3_length2( delta
);
293 static inline float v3_dist( v3f a
, v3f b
)
295 return sqrtf( v3_dist2( a
, b
) );
298 static inline void v3_normalize( v3f a
)
300 v3_muls( a
, 1.f
/ v3_length( a
), a
);
303 static inline float vg_lerpf( float a
, float b
, float t
)
308 static inline double vg_lerp( double a
, double b
, double t
)
313 /* correctly lerp around circular period -pi -> pi */
314 static float vg_alerpf( float a
, float b
, float t
)
316 float d
= fmodf( b
-a
, VG_TAUf
),
317 s
= fmodf( 2.0f
*d
, VG_TAUf
) - d
;
321 static inline void v3_lerp( v3f a
, v3f b
, float t
, v3f d
)
323 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
324 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
325 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
328 static inline void v3_minv( v3f a
, v3f b
, v3f dest
)
330 dest
[0] = vg_minf(a
[0], b
[0]);
331 dest
[1] = vg_minf(a
[1], b
[1]);
332 dest
[2] = vg_minf(a
[2], b
[2]);
335 static inline void v3_maxv( v3f a
, v3f b
, v3f dest
)
337 dest
[0] = vg_maxf(a
[0], b
[0]);
338 dest
[1] = vg_maxf(a
[1], b
[1]);
339 dest
[2] = vg_maxf(a
[2], b
[2]);
342 static inline float v3_minf( v3f a
)
344 return vg_minf( vg_minf( a
[0], a
[1] ), a
[2] );
347 static inline float v3_maxf( v3f a
)
349 return vg_maxf( vg_maxf( a
[0], a
[1] ), a
[2] );
352 static inline void v3_fill( v3f a
, float v
)
359 static inline void v3_floor( v3f a
, v3f b
)
361 b
[0] = floorf( a
[0] );
362 b
[1] = floorf( a
[1] );
363 b
[2] = floorf( a
[2] );
366 static inline void v3_ceil( v3f a
, v3f b
)
368 b
[0] = ceilf( a
[0] );
369 b
[1] = ceilf( a
[1] );
370 b
[2] = ceilf( a
[2] );
373 static inline void v3_negate( v3f a
, v3f b
)
380 static inline void v3_rotate( v3f v
, float angle
, v3f axis
, v3f d
)
391 v3_cross( k
, v
, v2
);
392 v3_muls( v2
, s
, v2
);
393 v3_add( v1
, v2
, v1
);
394 v3_muls( k
, v3_dot(k
, v
) * (1.0f
- c
), v2
);
401 static inline void v4_copy( v4f a
, v4f b
)
403 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
406 static inline void v4_zero( v4f a
)
408 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
411 static inline void v4_muls( v4f a
, float s
, v4f d
)
419 static inline void v4_muladds( v4f a
, v4f b
, float s
, v4f d
)
427 static inline void v4_lerp( v4f a
, v4f b
, float t
, v4f d
)
429 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
430 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
431 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
432 d
[3] = a
[3] + t
*(b
[3]-a
[3]);
435 static inline float v4_dot( v4f a
, v4f b
)
437 return a
[0]*b
[0] + a
[1]*b
[1] + a
[2]*b
[2] + a
[3]*a
[3];
440 static inline float v4_length( v4f a
)
442 return sqrtf( v4_dot(a
,a
) );
449 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
452 #define M2X2_ZERO {{0.0f, 0.0f, }, \
455 static inline void m2x2_copy( m2x2f a
, m2x2f b
)
457 v2_copy( a
[0], b
[0] );
458 v2_copy( a
[1], b
[1] );
461 static inline void m2x2_identity( m2x2f a
)
463 m2x2f id
= M2X2_INDENTIY
;
467 static inline void m2x2_create_rotation( m2x2f a
, float theta
)
484 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
485 { 0.0f, 1.0f, 0.0f, },\
486 { 0.0f, 0.0f, 1.0f, }}
488 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
489 { 0.0f, 0.0f, 0.0f, },\
490 { 0.0f, 0.0f, 0.0f, }}
493 static inline void m3x3_copy( m3x3f a
, m3x3f b
)
495 v3_copy( a
[0], b
[0] );
496 v3_copy( a
[1], b
[1] );
497 v3_copy( a
[2], b
[2] );
500 static inline void m3x3_identity( m3x3f a
)
502 m3x3f id
= M3X3_IDENTITY
;
506 static inline void m3x3_zero( m3x3f a
)
512 static inline void m3x3_inv( m3x3f src
, m3x3f dest
)
514 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
515 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
516 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
523 dest
[0][0] = (e
*i
-h
*f
)*det
;
524 dest
[0][1] = -(b
*i
-c
*h
)*det
;
525 dest
[0][2] = (b
*f
-c
*e
)*det
;
526 dest
[1][0] = -(d
*i
-f
*g
)*det
;
527 dest
[1][1] = (a
*i
-c
*g
)*det
;
528 dest
[1][2] = -(a
*f
-d
*c
)*det
;
529 dest
[2][0] = (d
*h
-g
*e
)*det
;
530 dest
[2][1] = -(a
*h
-g
*b
)*det
;
531 dest
[2][2] = (a
*e
-d
*b
)*det
;
534 static inline void m3x3_transpose( m3x3f src
, m3x3f dest
)
536 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
537 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
538 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
551 static inline void m3x3_mul( m3x3f a
, m3x3f b
, m3x3f d
)
553 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
554 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
555 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
557 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
558 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
559 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2];
561 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
562 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
563 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
564 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
565 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
566 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
567 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
568 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
569 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
572 static inline void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
576 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
577 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
578 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
583 static inline void m3x3_projection( m3x3f dst
,
584 float const left
, float const right
, float const bottom
, float const top
)
590 rl
= 1.0f
/ (right
- left
);
591 tb
= 1.0f
/ (top
- bottom
);
593 dst
[0][0] = 2.0f
* rl
;
594 dst
[1][1] = 2.0f
* tb
;
598 static inline void m3x3_translate( m3x3f m
, v3f v
)
600 m
[2][0] = m
[0][0] * v
[0] + m
[1][0] * v
[1] + m
[2][0];
601 m
[2][1] = m
[0][1] * v
[0] + m
[1][1] * v
[1] + m
[2][1];
602 m
[2][2] = m
[0][2] * v
[0] + m
[1][2] * v
[1] + m
[2][2];
605 static inline void m3x3_scale( m3x3f m
, v3f v
)
607 m
[0][0] = m
[0][0] * v
[0];
608 m
[0][1] = m
[0][1] * v
[0];
609 m
[0][2] = m
[0][2] * v
[0];
611 m
[1][0] = m
[1][0] * v
[1];
612 m
[1][1] = m
[1][1] * v
[1];
613 m
[1][2] = m
[1][2] * v
[1];
616 static inline void m3x3_rotate( m3x3f m
, float angle
)
618 float m00
= m
[0][0], m10
= m
[1][0],
619 m01
= m
[0][1], m11
= m
[1][1],
620 m02
= m
[0][2], m12
= m
[1][2];
626 m
[0][0] = m00
* c
+ m10
* s
;
627 m
[0][1] = m01
* c
+ m11
* s
;
628 m
[0][2] = m02
* c
+ m12
* s
;
630 m
[1][0] = m00
* -s
+ m10
* c
;
631 m
[1][1] = m01
* -s
+ m11
* c
;
632 m
[1][2] = m02
* -s
+ m12
* c
;
639 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
640 { 0.0f, 1.0f, 0.0f, },\
641 { 0.0f, 0.0f, 1.0f, },\
642 { 0.0f, 0.0f, 0.0f }}
644 static inline void m4x3_to_3x3( m4x3f a
, m3x3f b
)
646 v3_copy( a
[0], b
[0] );
647 v3_copy( a
[1], b
[1] );
648 v3_copy( a
[2], b
[2] );
651 static inline void m4x3_invert_affine( m4x3f a
, m4x3f b
)
653 m3x3_transpose( a
, b
);
654 m3x3_mulv( b
, a
[3], b
[3] );
655 v3_negate( b
[3], b
[3] );
658 static void m4x3_invert_full( m4x3f src
, m4x3f dst
)
662 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
663 e
= src
[1][0], f
= src
[1][1], g
= src
[1][2],
664 i
= src
[2][0], j
= src
[2][1], k
= src
[2][2],
665 m
= src
[3][0], n
= src
[3][1], o
= src
[3][2];
671 dst
[0][0] = f
*k
- g
*j
;
672 dst
[1][0] =-(e
*k
- g
*i
);
673 dst
[2][0] = e
*j
- f
*i
;
674 dst
[3][0] =-(e
*t2
- f
*t4
+ g
*t5
);
676 dst
[0][1] =-(b
*k
- c
*j
);
677 dst
[1][1] = a
*k
- c
*i
;
678 dst
[2][1] =-(a
*j
- b
*i
);
679 dst
[3][1] = a
*t2
- b
*t4
+ c
*t5
;
685 dst
[0][2] = b
*g
- c
*f
;
686 dst
[1][2] =-(a
*g
- c
*e
);
687 dst
[2][2] = a
*f
- b
*e
;
688 dst
[3][2] =-(a
*t2
- b
*t4
+ c
* t5
);
690 det
= 1.0f
/ (a
* dst
[0][0] + b
* dst
[1][0] + c
* dst
[2][0]);
691 v3_muls( dst
[0], det
, dst
[0] );
692 v3_muls( dst
[1], det
, dst
[1] );
693 v3_muls( dst
[2], det
, dst
[2] );
694 v3_muls( dst
[3], det
, dst
[3] );
697 static inline void m4x3_copy( m4x3f a
, m4x3f b
)
699 v3_copy( a
[0], b
[0] );
700 v3_copy( a
[1], b
[1] );
701 v3_copy( a
[2], b
[2] );
702 v3_copy( a
[3], b
[3] );
705 static inline void m4x3_identity( m4x3f a
)
707 m4x3f id
= M4X3_IDENTITY
;
711 static inline void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
714 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
715 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
716 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
717 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
718 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
719 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
720 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
721 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
723 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
724 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
725 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
726 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
727 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
728 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
729 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
730 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
731 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
732 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
733 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
734 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
737 static inline void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
741 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
742 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
743 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
749 * Transform plane ( xyz, distance )
751 static inline void m4x3_mulp( m4x3f m
, v4f p
, v4f d
)
755 v3_muls( p
, p
[3], o
);
756 m4x3_mulv( m
, o
, o
);
757 m3x3_mulv( m
, p
, d
);
759 d
[3] = v3_dot( o
, d
);
766 static inline void m4x3_translate( m4x3f m
, v3f v
)
768 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
769 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
770 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
773 static inline void m4x3_scale( m4x3f m
, float s
)
775 v3_muls( m
[0], s
, m
[0] );
776 v3_muls( m
[1], s
, m
[1] );
777 v3_muls( m
[2], s
, m
[2] );
780 static inline void m4x3_scalev( m4x3f m
, v3f v
)
782 v3_muls(m
[0], v
[0], m
[0]);
783 v3_muls(m
[1], v
[1], m
[1]);
784 v3_muls(m
[2], v
[2], m
[2]);
787 static inline void m4x3_rotate_x( m4x3f m
, float angle
)
789 m4x3f t
= M4X3_IDENTITY
;
803 static inline void m4x3_rotate_y( m4x3f m
, float angle
)
805 m4x3f t
= M4X3_IDENTITY
;
819 static inline void m4x3_rotate_z( m4x3f m
, float angle
)
821 m4x3f t
= M4X3_IDENTITY
;
835 static inline void m4x3_expand( m4x3f m
, m4x4f d
)
837 v3_copy( m
[0], d
[0] );
838 v3_copy( m
[1], d
[1] );
839 v3_copy( m
[2], d
[2] );
840 v3_copy( m
[3], d
[3] );
847 static inline void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
850 m4x3_mulv( m
, point
, v
);
852 v3_minv( box
[0], v
, box
[0] );
853 v3_maxv( box
[1], v
, box
[1] );
856 static inline void box_addpt( boxf a
, v3f pt
)
858 v3_minv( a
[0], pt
, a
[0] );
859 v3_maxv( a
[1], pt
, a
[1] );
862 static inline void box_concat( boxf a
, boxf b
)
864 v3_minv( a
[0], b
[0], a
[0] );
865 v3_maxv( a
[1], b
[1], a
[1] );
868 static inline void box_copy( boxf a
, boxf b
)
870 v3_copy( a
[0], b
[0] );
871 v3_copy( a
[1], b
[1] );
874 static inline int box_overlap( boxf a
, boxf b
)
877 ( a
[0][0] <= b
[1][0] && a
[1][0] >= b
[0][0] ) &&
878 ( a
[0][1] <= b
[1][1] && a
[1][1] >= b
[0][1] ) &&
879 ( a
[0][2] <= b
[1][2] && a
[1][2] >= b
[0][2] )
883 static inline void box_init_inf( boxf box
)
885 v3_fill( box
[0], INFINITY
);
886 v3_fill( box
[1], -INFINITY
);
889 static inline void m4x3_transform_aabb( m4x3f m
, boxf box
)
893 v3_copy( box
[0], a
);
894 v3_copy( box
[1], b
);
895 v3_fill( box
[0], INFINITY
);
896 v3_fill( box
[1], -INFINITY
);
898 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], a
[1], a
[2] } );
899 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
900 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
901 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
903 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], a
[1], b
[2] } );
904 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
905 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
906 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
909 int ray_aabb( boxf box
, v3f co
, v3f dir
, float dist
)
914 v3_sub( box
[0], co
, v0
);
915 v3_sub( box
[1], co
, v1
);
916 v3_div( v0
, dir
, v0
);
917 v3_div( v1
, dir
, v1
);
919 tmin
= vg_minf( v0
[0], v1
[0] );
920 tmax
= vg_maxf( v0
[0], v1
[0] );
921 tmin
= vg_maxf( tmin
, vg_minf( v0
[1], v1
[1] ));
922 tmax
= vg_minf( tmax
, vg_maxf( v0
[1], v1
[1] ));
923 tmin
= vg_maxf( tmin
, vg_minf( v0
[2], v1
[2] ));
924 tmax
= vg_minf( tmax
, vg_maxf( v0
[2], v1
[2] ));
926 return tmax
>= tmin
&& tmin
< dist
&& tmax
> 0;
929 static inline void m4x3_lookat( m4x3f m
, v3f pos
, v3f target
, v3f up
)
932 v3_sub( target
, pos
, dir
);
935 v3_copy( dir
, m
[2] );
937 v3_cross( up
, m
[2], m
[0] );
938 v3_normalize( m
[0] );
940 v3_cross( m
[2], m
[0], m
[1] );
941 v3_copy( pos
, m
[3] );
948 #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\
949 { 0.0f, 1.0f, 0.0f, 0.0f },\
950 { 0.0f, 0.0f, 1.0f, 0.0f },\
951 { 0.0f, 0.0f, 0.0f, 1.0f }}
952 #define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\
953 { 0.0f, 0.0f, 0.0f, 0.0f },\
954 { 0.0f, 0.0f, 0.0f, 0.0f },\
955 { 0.0f, 0.0f, 0.0f, 0.0f }}
957 static void m4x4_projection( m4x4f m
, float angle
,
958 float ratio
, float fnear
, float ffar
)
960 float scale
= tanf( angle
* 0.5f
* VG_PIf
/ 180.0f
) * fnear
,
966 m
[0][0] = 2.0f
* fnear
/ (r
- l
);
971 m
[1][1] = 2.0f
* fnear
/ (t
- b
);
974 m
[2][0] = (r
+ l
) / (r
- l
);
975 m
[2][1] = (t
+ b
) / (t
- b
);
976 m
[2][2] = -(ffar
+ fnear
) / (ffar
- fnear
);
980 m
[3][2] = -2.0f
* ffar
* fnear
/ (ffar
- fnear
);
984 static void m4x4_translate( m4x4f m
, v3f v
)
986 v4_muladds( m
[3], m
[0], v
[0], m
[3] );
987 v4_muladds( m
[3], m
[1], v
[1], m
[3] );
988 v4_muladds( m
[3], m
[2], v
[2], m
[3] );
991 static inline void m4x4_copy( m4x4f a
, m4x4f b
)
993 v4_copy( a
[0], b
[0] );
994 v4_copy( a
[1], b
[1] );
995 v4_copy( a
[2], b
[2] );
996 v4_copy( a
[3], b
[3] );
999 static inline void m4x4_identity( m4x4f a
)
1001 m4x4f id
= M4X4_IDENTITY
;
1005 static inline void m4x4_zero( m4x4f a
)
1007 m4x4f zero
= M4X4_ZERO
;
1008 m4x4_copy( zero
, a
);
1011 static inline void m4x4_mul( m4x4f a
, m4x4f b
, m4x4f d
)
1013 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
1014 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
1015 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
1016 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
1018 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2], b03
= b
[0][3],
1019 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2], b13
= b
[1][3],
1020 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2], b23
= b
[2][3],
1021 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2], b33
= b
[3][3];
1023 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
+ a30
*b03
;
1024 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
+ a31
*b03
;
1025 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
+ a32
*b03
;
1026 d
[0][3] = a03
*b00
+ a13
*b01
+ a23
*b02
+ a33
*b03
;
1027 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
+ a30
*b13
;
1028 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
+ a31
*b13
;
1029 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
+ a32
*b13
;
1030 d
[1][3] = a03
*b10
+ a13
*b11
+ a23
*b12
+ a33
*b13
;
1031 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
+ a30
*b23
;
1032 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
+ a31
*b23
;
1033 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
+ a32
*b23
;
1034 d
[2][3] = a03
*b20
+ a13
*b21
+ a23
*b22
+ a33
*b23
;
1035 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
*b33
;
1036 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
*b33
;
1037 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
*b33
;
1038 d
[3][3] = a03
*b30
+ a13
*b31
+ a23
*b32
+ a33
*b33
;
1041 static inline void m4x4_mulv( m4x4f m
, v4f v
, v4f d
)
1045 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0]*v
[3];
1046 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1]*v
[3];
1047 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2]*v
[3];
1048 res
[3] = m
[0][3]*v
[0] + m
[1][3]*v
[1] + m
[2][3]*v
[2] + m
[3][3]*v
[3];
1053 static inline void m4x4_inv( m4x4f a
, m4x4f d
)
1055 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
1056 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
1057 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
1058 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
1062 t
[0] = a22
*a33
- a32
*a23
;
1063 t
[1] = a21
*a33
- a31
*a23
;
1064 t
[2] = a21
*a32
- a31
*a22
;
1065 t
[3] = a20
*a33
- a30
*a23
;
1066 t
[4] = a20
*a32
- a30
*a22
;
1067 t
[5] = a20
*a31
- a30
*a21
;
1069 d
[0][0] = a11
*t
[0] - a12
*t
[1] + a13
*t
[2];
1070 d
[1][0] =-(a10
*t
[0] - a12
*t
[3] + a13
*t
[4]);
1071 d
[2][0] = a10
*t
[1] - a11
*t
[3] + a13
*t
[5];
1072 d
[3][0] =-(a10
*t
[2] - a11
*t
[4] + a12
*t
[5]);
1074 d
[0][1] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
1075 d
[1][1] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
1076 d
[2][1] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
1077 d
[3][1] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
1079 t
[0] = a12
*a33
- a32
*a13
;
1080 t
[1] = a11
*a33
- a31
*a13
;
1081 t
[2] = a11
*a32
- a31
*a12
;
1082 t
[3] = a10
*a33
- a30
*a13
;
1083 t
[4] = a10
*a32
- a30
*a12
;
1084 t
[5] = a10
*a31
- a30
*a11
;
1086 d
[0][2] = a01
*t
[0] - a02
*t
[1] + a03
*t
[2];
1087 d
[1][2] =-(a00
*t
[0] - a02
*t
[3] + a03
*t
[4]);
1088 d
[2][2] = a00
*t
[1] - a01
*t
[3] + a03
*t
[5];
1089 d
[3][2] =-(a00
*t
[2] - a01
*t
[4] + a02
*t
[5]);
1091 t
[0] = a12
*a23
- a22
*a13
;
1092 t
[1] = a11
*a23
- a21
*a13
;
1093 t
[2] = a11
*a22
- a21
*a12
;
1094 t
[3] = a10
*a23
- a20
*a13
;
1095 t
[4] = a10
*a22
- a20
*a12
;
1096 t
[5] = a10
*a21
- a20
*a11
;
1098 d
[0][3] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
1099 d
[1][3] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
1100 d
[2][3] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
1101 d
[3][3] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
1103 det
= 1.0f
/ (a00
*d
[0][0] + a01
*d
[1][0] + a02
*d
[2][0] + a03
*d
[3][0]);
1104 v4_muls( d
[0], det
, d
[0] );
1105 v4_muls( d
[1], det
, d
[1] );
1106 v4_muls( d
[2], det
, d
[2] );
1107 v4_muls( d
[3], det
, d
[3] );
1111 * Planes (double precision)
1113 static inline void tri_to_plane( double a
[3], double b
[3],
1114 double c
[3], double p
[4] )
1120 edge0
[0] = b
[0] - a
[0];
1121 edge0
[1] = b
[1] - a
[1];
1122 edge0
[2] = b
[2] - a
[2];
1124 edge1
[0] = c
[0] - a
[0];
1125 edge1
[1] = c
[1] - a
[1];
1126 edge1
[2] = c
[2] - a
[2];
1128 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
1129 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
1130 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
1132 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
1133 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
1140 static inline int plane_intersect( double a
[4], double b
[4],
1141 double c
[4], double p
[4] )
1143 double const epsilon
= 1e-8f
;
1148 x
[0] = a
[1] * b
[2] - a
[2] * b
[1];
1149 x
[1] = a
[2] * b
[0] - a
[0] * b
[2];
1150 x
[2] = a
[0] * b
[1] - a
[1] * b
[0];
1152 d
= x
[0] * c
[0] + x
[1] * c
[1] + x
[2] * c
[2];
1154 if( d
< epsilon
&& d
> -epsilon
) return 0;
1156 p
[0] = (b
[1] * c
[2] - b
[2] * c
[1]) * -a
[3];
1157 p
[1] = (b
[2] * c
[0] - b
[0] * c
[2]) * -a
[3];
1158 p
[2] = (b
[0] * c
[1] - b
[1] * c
[0]) * -a
[3];
1160 p
[0] += (c
[1] * a
[2] - c
[2] * a
[1]) * -b
[3];
1161 p
[1] += (c
[2] * a
[0] - c
[0] * a
[2]) * -b
[3];
1162 p
[2] += (c
[0] * a
[1] - c
[1] * a
[0]) * -b
[3];
1164 p
[0] += (a
[1] * b
[2] - a
[2] * b
[1]) * -c
[3];
1165 p
[1] += (a
[2] * b
[0] - a
[0] * b
[2]) * -c
[3];
1166 p
[2] += (a
[0] * b
[1] - a
[1] * b
[0]) * -c
[3];
1175 static inline double plane_polarity( double p
[4], double a
[3] )
1178 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
1179 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])
1185 static inline void q_identity( v4f q
)
1187 q
[0] = 0.0f
; q
[1] = 0.0f
; q
[2] = 0.0f
; q
[3] = 1.0f
;
1190 static inline void q_axis_angle( v4f q
, v3f axis
, float angle
)
1192 float a
= angle
*0.5f
,
1202 static inline void q_mul( v4f q
, v4f q1
, v4f d
)
1205 t
[0] = q
[3]*q1
[0] + q
[0]*q1
[3] + q
[1]*q1
[2] - q
[2]*q1
[1];
1206 t
[1] = q
[3]*q1
[1] - q
[0]*q1
[2] + q
[1]*q1
[3] + q
[2]*q1
[0];
1207 t
[2] = q
[3]*q1
[2] + q
[0]*q1
[1] - q
[1]*q1
[0] + q
[2]*q1
[3];
1208 t
[3] = q
[3]*q1
[3] - q
[0]*q1
[0] - q
[1]*q1
[1] - q
[2]*q1
[2];
1212 static inline void q_normalize( v4f q
)
1214 float s
= 1.0f
/ sqrtf(v4_dot(q
,q
));
1221 static inline void q_inv( v4f q
, v4f d
)
1223 float s
= 1.0f
/ v4_dot(q
,q
);
1230 static inline void q_nlerp( v4f a
, v4f b
, float t
, v4f d
)
1232 if( v4_dot(a
,b
) < 0.0f
)
1234 v4_muls( b
, -1.0f
, d
);
1235 v4_lerp( a
, d
, t
, d
);
1238 v4_lerp( a
, b
, t
, d
);
1243 static inline void q_m3x3( v4f q
, m3x3f d
)
1247 s
= l
> 0.0f
? 2.0f
/l
: 0.0f
,
1249 xx
= s
*q
[0]*q
[0], xy
= s
*q
[0]*q
[1], wx
= s
*q
[3]*q
[0],
1250 yy
= s
*q
[1]*q
[1], yz
= s
*q
[1]*q
[2], wy
= s
*q
[3]*q
[1],
1251 zz
= s
*q
[2]*q
[2], xz
= s
*q
[0]*q
[2], wz
= s
*q
[3]*q
[2];
1253 d
[0][0] = 1.0f
- yy
- zz
;
1254 d
[1][1] = 1.0f
- xx
- zz
;
1255 d
[2][2] = 1.0f
- xx
- yy
;
1264 static void m3x3_q( m3x3f m
, v4f q
)
1266 float diag
, r
, rinv
;
1268 diag
= m
[0][0] + m
[1][1] + m
[2][2];
1271 r
= sqrtf( 1.0f
+ diag
);
1273 q
[0] = rinv
* (m
[1][2] - m
[2][1]);
1274 q
[1] = rinv
* (m
[2][0] - m
[0][2]);
1275 q
[2] = rinv
* (m
[0][1] - m
[1][0]);
1278 else if( m
[0][0] >= m
[1][1] && m
[0][0] >= m
[2][2] )
1280 r
= sqrtf( 1.0f
- m
[1][1] - m
[2][2] + m
[0][0] );
1283 q
[1] = rinv
* (m
[0][1] + m
[1][0]);
1284 q
[2] = rinv
* (m
[0][2] + m
[2][0]);
1285 q
[3] = rinv
* (m
[1][2] - m
[2][1]);
1287 else if( m
[1][1] >= m
[2][2] )
1289 r
= sqrtf( 1.0f
- m
[0][0] - m
[2][2] + m
[1][1] );
1291 q
[0] = rinv
* (m
[0][1] + m
[1][0]);
1293 q
[2] = rinv
* (m
[1][2] + m
[2][1]);
1294 q
[3] = rinv
* (m
[2][0] - m
[0][2]);
1298 r
= sqrtf( 1.0f
- m
[0][0] - m
[1][1] + m
[2][2] );
1300 q
[0] = rinv
* (m
[0][2] + m
[2][0]);
1301 q
[1] = rinv
* (m
[1][2] + m
[2][1]);
1303 q
[3] = rinv
* (m
[0][1] - m
[1][0]);
1307 static int ray_tri( v3f tri
[3], v3f co
, v3f dir
, float *dist
)
1309 float const kEpsilon
= 0.00001f
;
1311 v3f v0
, v1
, h
, s
, q
, n
;
1318 v3_sub( pb
, pa
, v0
);
1319 v3_sub( pc
, pa
, v1
);
1320 v3_cross( dir
, v1
, h
);
1321 v3_cross( v0
, v1
, n
);
1323 if( v3_dot( n
, dir
) > 0.0f
) /* Backface culling */
1327 a
= v3_dot( v0
, h
);
1328 if( a
> -kEpsilon
&& a
< kEpsilon
)
1332 v3_sub( co
, pa
, s
);
1334 u
= f
* v3_dot(s
, h
);
1335 if( u
< 0.0f
|| u
> 1.0f
)
1338 v3_cross( s
, v0
, q
);
1339 v
= f
* v3_dot( dir
, q
);
1340 if( v
< 0.0f
|| u
+v
> 1.0f
)
1343 t
= f
* v3_dot(v1
, q
);
1352 static inline float vg_randf(void)
1354 return (float)rand()/(float)(RAND_MAX
);
1357 static inline void vg_rand_dir(v3f dir
)
1359 dir
[0] = vg_randf();
1360 dir
[1] = vg_randf();
1361 dir
[2] = vg_randf();
1363 v3_muls( dir
, 2.0f
, dir
);
1364 v3_sub( dir
, (v3f
){1.0f
,1.0f
,1.0f
}, dir
);
1366 v3_normalize( dir
);
1369 static inline void vg_rand_sphere( v3f co
)
1372 v3_muls( co
, cbrtf( vg_randf() ), co
);
1375 static inline int vg_randint(int max
)
1380 static void eval_bezier_time( v3f p0
, v3f p1
, v3f h0
, v3f h1
, float t
, v3f p
)
1385 v3_muls( p1
, ttt
, p
);
1386 v3_muladds( p
, h1
, 3.0f
*tt
-3.0f
*ttt
, p
);
1387 v3_muladds( p
, h0
, 3.0f
*ttt
-6.0f
*tt
+3.0f
*t
, p
);
1388 v3_muladds( p
, p0
, 3.0f
*tt
-ttt
-3.0f
*t
+1.0f
, p
);