1 /* Copyright (C) 2021 Harry Godden (hgn) - All Rights Reserved */
3 #define VG_PIf 3.14159265358979323846264338327950288f
4 #define VG_TAUf 6.28318530717958647692528676655900576f
6 static inline float vg_minf( float a
, float b
)
11 static inline float vg_maxf( float a
, float b
)
16 static inline float vg_clampf( float a
, float min
, float max
)
18 return vg_minf( max
, vg_maxf( a
, min
) );
21 static inline float vg_signf( float a
)
23 return a
< 0.0f
? -1.0f
: 1.0f
;
26 static inline float vg_randf(void)
28 return (float)rand()/(float)(RAND_MAX
);
31 static inline float vg_randint(int max
)
36 #define VG_MIN( A, B ) ((A)<(B)?(A):(B))
37 #define VG_MAX( A, B ) ((A)>(B)?(A):(B))
39 static inline int vg_min( int a
, int b
)
44 static inline int vg_max( int a
, int b
)
49 static inline float vg_rad( float deg
)
51 return deg
* VG_PIf
/ 180.0f
;
57 static inline void v2_copy( v2f a
, v2f b
)
59 b
[0] = a
[0]; b
[1] = a
[1];
62 static inline void v2i_copy( v2i a
, v2i b
)
64 b
[0] = a
[0]; b
[1] = a
[1];
67 static inline int v2i_eq( v2i a
, v2i b
)
69 return ((a
[0] == b
[0]) && (a
[1] == b
[1]));
72 static inline void v2i_add( v2i a
, v2i b
, v2i d
)
74 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
77 static inline void v2i_sub( v2i a
, v2i b
, v2i d
)
79 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
82 static inline void v2_minv( v2f a
, v2f b
, v2f dest
)
84 dest
[0] = vg_minf(a
[0], b
[0]);
85 dest
[1] = vg_minf(a
[1], b
[1]);
88 static inline void v2_maxv( v2f a
, v2f b
, v2f dest
)
90 dest
[0] = vg_maxf(a
[0], b
[0]);
91 dest
[1] = vg_maxf(a
[1], b
[1]);
94 static inline void v2_sub( v2f a
, v2f b
, v2f d
)
96 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
99 static inline float v2_dot( v2f a
, v2f b
)
101 return a
[0] * b
[0] + a
[1] * b
[1];
104 static inline float v2_cross( v2f a
, v2f b
)
106 return a
[0]*b
[1] - a
[1]*b
[0];
109 static inline void v2_add( v2f a
, v2f b
, v2f d
)
111 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
114 static inline void v2_muls( v2f a
, float s
, v2f d
)
116 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
119 static inline void v2_divs( v2f a
, float s
, v2f d
)
121 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
;
124 static inline void v2_mul( v2f a
, v2f b
, v2f d
)
130 static inline void v2_div( v2f a
, v2f b
, v2f d
)
132 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
135 static inline void v2_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
137 d
[0] = a
[0]+b
[0]*s
[0];
138 d
[1] = a
[1]+b
[1]*s
[1];
141 static inline void v2_muladds( v2f a
, v2f b
, float s
, v2f d
)
147 static inline float v2_length2( v2f a
)
149 return a
[0]*a
[0] + a
[1]*a
[1];
152 static inline float v2_length( v2f a
)
154 return sqrtf( v2_length2( a
) );
157 static inline float v2_dist2( v2f a
, v2f b
)
160 v2_sub( a
, b
, delta
);
161 return v2_length2( delta
);
164 static inline float v2_dist( v2f a
, v2f b
)
166 return sqrtf( v2_dist2( a
, b
) );
169 static inline void v2_lerp( v2f a
, v2f b
, float t
, v2f d
)
171 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
172 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
175 static inline void v2_normalize( v2f a
)
177 v2_muls( a
, 1.f
/ v2_length( a
), a
);
183 static inline void v3_zero( v3f a
)
185 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
188 static inline void v3_copy( v3f a
, v3f b
)
190 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
193 static inline void v3_add( v3f a
, v3f b
, v3f d
)
195 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
198 static inline void v3_sub( v3f a
, v3f b
, v3f d
)
200 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
203 static inline void v3_mul( v3f a
, v3f b
, v3f d
)
205 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
208 static inline void v3_div( v3f a
, v3f b
, v3f d
)
210 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1]; d
[2] = a
[2]/b
[2];
213 static inline void v3_muls( v3f a
, float s
, v3f d
)
215 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
218 static inline void v3_divs( v3f a
, float s
, v3f d
)
220 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
; d
[2] = a
[2]/s
;
223 static inline void v3_muladds( v3f a
, v3f b
, float s
, v3f d
)
225 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
228 static inline void v3_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
230 d
[0] = a
[0]+b
[0]*s
[0];
231 d
[1] = a
[1]+b
[1]*s
[1];
232 d
[2] = a
[2]+b
[2]*s
[2];
235 static inline float v3_dot( v3f a
, v3f b
)
237 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
240 static inline void v3_cross( v3f a
, v3f b
, v3f dest
)
243 d
[0] = a
[1]*b
[2] - a
[2]*b
[1];
244 d
[1] = a
[2]*b
[0] - a
[0]*b
[2];
245 d
[2] = a
[0]*b
[1] - a
[1]*b
[0];
249 static inline float v3_length2( v3f a
)
251 return v3_dot( a
, a
);
254 static inline float v3_length( v3f a
)
256 return sqrtf( v3_length2( a
) );
259 static inline float v3_dist2( v3f a
, v3f b
)
262 v3_sub( a
, b
, delta
);
263 return v3_length2( delta
);
266 static inline float v3_dist( v3f a
, v3f b
)
268 return sqrtf( v3_dist2( a
, b
) );
271 static inline void v3_normalize( v3f a
)
273 v3_muls( a
, 1.f
/ v3_length( a
), a
);
276 static inline float vg_lerpf( float a
, float b
, float t
)
281 static inline void v3_lerp( v3f a
, v3f b
, float t
, v3f d
)
283 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
284 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
285 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
288 static inline void v3_minv( v3f a
, v3f b
, v3f dest
)
290 dest
[0] = vg_minf(a
[0], b
[0]);
291 dest
[1] = vg_minf(a
[1], b
[1]);
292 dest
[2] = vg_minf(a
[2], b
[2]);
295 static inline void v3_maxv( v3f a
, v3f b
, v3f dest
)
297 dest
[0] = vg_maxf(a
[0], b
[0]);
298 dest
[1] = vg_maxf(a
[1], b
[1]);
299 dest
[2] = vg_maxf(a
[2], b
[2]);
302 static inline float v3_minf( v3f a
)
304 return vg_minf( vg_minf( a
[0], a
[1] ), a
[2] );
307 static inline float v3_maxf( v3f a
)
309 return vg_maxf( vg_maxf( a
[0], a
[1] ), a
[2] );
312 static inline void v3_fill( v3f a
, float v
)
319 static inline void v3_floor( v3f a
, v3f b
)
321 b
[0] = floorf( a
[0] );
322 b
[1] = floorf( a
[1] );
323 b
[2] = floorf( a
[2] );
326 static inline void v3_ceil( v3f a
, v3f b
)
328 b
[0] = ceilf( a
[0] );
329 b
[1] = ceilf( a
[1] );
330 b
[2] = ceilf( a
[2] );
333 static inline void v3_negate( v3f a
, v3f b
)
340 static inline void v3_rotate( v3f v
, float angle
, v3f axis
, v3f d
)
351 v3_cross( k
, v
, v2
);
352 v3_muls( v2
, s
, v2
);
353 v3_add( v1
, v2
, v1
);
354 v3_muls( k
, v3_dot(k
, v
) * (1.0f
- c
), v2
);
361 static inline void v4_copy( v4f a
, v4f b
)
363 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
366 static inline void v4_zero( v4f a
)
368 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
371 static inline void v4_muladds( v3f a
, v3f b
, float s
, v3f d
)
379 static inline void v4_lerp( v4f a
, v4f b
, float t
, v4f d
)
381 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
382 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
383 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
384 d
[3] = a
[3] + t
*(b
[3]-a
[3]);
387 static inline float v4_dot( v4f a
, v4f b
)
389 return a
[0]*b
[0] + a
[1]*b
[1] + a
[2]*b
[2] + a
[3]*a
[3];
392 static inline float v4_length( v4f a
)
394 return sqrtf( v4_dot(a
,a
) );
401 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
404 #define M2X2_ZERO {{0.0f, 0.0f, }, \
407 static inline void m2x2_copy( m2x2f a
, m2x2f b
)
409 v2_copy( a
[0], b
[0] );
410 v2_copy( a
[1], b
[1] );
413 static inline void m2x2_identity( m2x2f a
)
415 m2x2f id
= M2X2_INDENTIY
;
419 static inline void m2x2_create_rotation( m2x2f a
, float theta
)
436 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
437 { 0.0f, 1.0f, 0.0f, },\
438 { 0.0f, 0.0f, 1.0f, }}
440 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
441 { 0.0f, 0.0f, 0.0f, },\
442 { 0.0f, 0.0f, 0.0f, }}
445 static inline void m3x3_copy( m3x3f a
, m3x3f b
)
447 v3_copy( a
[0], b
[0] );
448 v3_copy( a
[1], b
[1] );
449 v3_copy( a
[2], b
[2] );
452 static inline void m3x3_identity( m3x3f a
)
454 m3x3f id
= M3X3_IDENTITY
;
458 static inline void m3x3_zero( m3x3f a
)
464 static inline void m3x3_inv( m3x3f src
, m3x3f dest
)
466 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
467 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
468 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
475 dest
[0][0] = (e
*i
-h
*f
)*det
;
476 dest
[0][1] = -(b
*i
-c
*h
)*det
;
477 dest
[0][2] = (b
*f
-c
*e
)*det
;
478 dest
[1][0] = -(d
*i
-f
*g
)*det
;
479 dest
[1][1] = (a
*i
-c
*g
)*det
;
480 dest
[1][2] = -(a
*f
-d
*c
)*det
;
481 dest
[2][0] = (d
*h
-g
*e
)*det
;
482 dest
[2][1] = -(a
*h
-g
*b
)*det
;
483 dest
[2][2] = (a
*e
-d
*b
)*det
;
486 static inline void m3x3_transpose( m3x3f src
, m3x3f dest
)
488 float a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
489 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
490 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
503 static inline void m3x3_mul( m3x3f a
, m3x3f b
, m3x3f d
)
505 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
506 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
507 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
509 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
510 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
511 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2];
513 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
514 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
515 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
516 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
517 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
518 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
519 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
520 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
521 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
524 static inline void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
528 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
529 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
530 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
535 static inline void m3x3_projection( m3x3f dst
,
536 float const left
, float const right
, float const bottom
, float const top
)
542 rl
= 1.0f
/ (right
- left
);
543 tb
= 1.0f
/ (top
- bottom
);
545 dst
[0][0] = 2.0f
* rl
;
546 dst
[1][1] = 2.0f
* tb
;
550 static inline void m3x3_translate( m3x3f m
, v3f v
)
552 m
[2][0] = m
[0][0] * v
[0] + m
[1][0] * v
[1] + m
[2][0];
553 m
[2][1] = m
[0][1] * v
[0] + m
[1][1] * v
[1] + m
[2][1];
554 m
[2][2] = m
[0][2] * v
[0] + m
[1][2] * v
[1] + m
[2][2];
557 static inline void m3x3_scale( m3x3f m
, v3f v
)
559 m
[0][0] = m
[0][0] * v
[0];
560 m
[0][1] = m
[0][1] * v
[0];
561 m
[0][2] = m
[0][2] * v
[0];
563 m
[1][0] = m
[1][0] * v
[1];
564 m
[1][1] = m
[1][1] * v
[1];
565 m
[1][2] = m
[1][2] * v
[1];
568 static inline void m3x3_rotate( m3x3f m
, float angle
)
570 float m00
= m
[0][0], m10
= m
[1][0],
571 m01
= m
[0][1], m11
= m
[1][1],
572 m02
= m
[0][2], m12
= m
[1][2];
578 m
[0][0] = m00
* c
+ m10
* s
;
579 m
[0][1] = m01
* c
+ m11
* s
;
580 m
[0][2] = m02
* c
+ m12
* s
;
582 m
[1][0] = m00
* -s
+ m10
* c
;
583 m
[1][1] = m01
* -s
+ m11
* c
;
584 m
[1][2] = m02
* -s
+ m12
* c
;
591 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
592 { 0.0f, 1.0f, 0.0f, },\
593 { 0.0f, 0.0f, 1.0f, },\
594 { 0.0f, 0.0f, 0.0f }}
596 static inline void m4x3_to_3x3( m4x3f a
, m3x3f b
)
598 v3_copy( a
[0], b
[0] );
599 v3_copy( a
[1], b
[1] );
600 v3_copy( a
[2], b
[2] );
603 static inline void m4x3_invert_affine( m4x3f a
, m4x3f b
)
605 m3x3_transpose( a
, b
);
606 m3x3_mulv( b
, a
[3], b
[3] );
607 v3_negate( b
[3], b
[3] );
610 static inline void m4x3_copy( m4x3f a
, m4x3f b
)
612 v3_copy( a
[0], b
[0] );
613 v3_copy( a
[1], b
[1] );
614 v3_copy( a
[2], b
[2] );
615 v3_copy( a
[3], b
[3] );
618 static inline void m4x3_identity( m4x3f a
)
620 m4x3f id
= M4X3_IDENTITY
;
624 static inline void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
627 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
628 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
629 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
630 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
631 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
632 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
633 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
634 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
636 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
637 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
638 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
639 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
640 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
641 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
642 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
643 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
644 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
645 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
646 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
647 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
650 static inline void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
654 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
655 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
656 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
665 static inline void m4x3_translate( m4x3f m
, v3f v
)
667 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
668 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
669 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
672 static inline void m4x3_scale( m4x3f m
, float s
)
674 v3_muls( m
[0], s
, m
[0] );
675 v3_muls( m
[1], s
, m
[1] );
676 v3_muls( m
[2], s
, m
[2] );
679 static inline void m4x3_rotate_x( m4x3f m
, float angle
)
681 m4x3f t
= M4X3_IDENTITY
;
695 static inline void m4x3_rotate_y( m4x3f m
, float angle
)
697 m4x3f t
= M4X3_IDENTITY
;
711 static inline void m4x3_rotate_z( m4x3f m
, float angle
)
713 m4x3f t
= M4X3_IDENTITY
;
727 static inline void m4x3_expand( m4x3f m
, m4x4f d
)
729 v3_copy( m
[0], d
[0] );
730 v3_copy( m
[1], d
[1] );
731 v3_copy( m
[2], d
[2] );
732 v3_copy( m
[3], d
[3] );
739 static inline void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
742 m4x3_mulv( m
, point
, v
);
744 v3_minv( box
[0], v
, box
[0] );
745 v3_maxv( box
[1], v
, box
[1] );
748 static inline void box_addpt( boxf a
, v3f pt
)
750 v3_minv( a
[0], pt
, a
[0] );
751 v3_maxv( a
[1], pt
, a
[1] );
754 static inline void box_concat( boxf a
, boxf b
)
756 v3_minv( a
[0], b
[0], a
[0] );
757 v3_maxv( a
[1], b
[1], a
[1] );
760 static inline void box_copy( boxf a
, boxf b
)
762 v3_copy( a
[0], b
[0] );
763 v3_copy( a
[1], b
[1] );
766 static inline void box_init_inf( boxf box
)
768 v3_fill( box
[0], INFINITY
);
769 v3_fill( box
[1], -INFINITY
);
772 static inline void m4x3_transform_aabb( m4x3f m
, boxf box
)
776 v3_copy( box
[0], a
);
777 v3_copy( box
[1], b
);
778 v3_fill( box
[0], INFINITY
);
779 v3_fill( box
[1], -INFINITY
);
781 m4x3_expand_aabb_point( m
, box
, a
);
782 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
783 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
784 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
785 m4x3_expand_aabb_point( m
, box
, b
);
786 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
787 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
788 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
791 static inline void m4x3_lookat( m4x3f m
, v3f pos
, v3f target
, v3f up
)
794 v3_sub( target
, pos
, dir
);
797 v3_copy( dir
, m
[2] );
799 v3_cross( up
, m
[2], m
[0] );
800 v3_normalize( m
[0] );
802 v3_cross( m
[2], m
[0], m
[1] );
803 v3_copy( pos
, m
[3] );
810 #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\
811 { 0.0f, 1.0f, 0.0f, 0.0f },\
812 { 0.0f, 0.0f, 1.0f, 0.0f },\
813 { 0.0f, 0.0f, 0.0f, 1.0f }}
815 static void m4x4_projection( m4x4f m
, float angle
,
816 float ratio
, float near
, float far
)
818 float scale
= tanf( angle
* 0.5f
* VG_PIf
/ 180.0f
) * near
,
824 m
[0][0] = 2.0f
* near
/ (r
- l
);
829 m
[1][1] = 2.0f
* near
/ (t
- b
);
832 m
[2][0] = (r
+ l
) / (r
- l
);
833 m
[2][1] = (t
+ b
) / (t
- b
);
834 m
[2][2] = -(far
+ near
) / (far
- near
);
838 m
[3][2] = -2.0f
* far
* near
/ (far
- near
);
842 static void m4x4_translate( m4x4f m
, v3f v
)
844 v4_muladds( m
[3], m
[0], v
[0], m
[3] );
845 v4_muladds( m
[3], m
[1], v
[1], m
[3] );
846 v4_muladds( m
[3], m
[2], v
[2], m
[3] );
849 static inline void m4x4_copy( m4x4f a
, m4x4f b
)
851 v4_copy( a
[0], b
[0] );
852 v4_copy( a
[1], b
[1] );
853 v4_copy( a
[2], b
[2] );
854 v4_copy( a
[3], b
[3] );
857 static inline void m4x4_identity( m4x4f a
)
859 m4x4f id
= M4X4_IDENTITY
;
863 static inline void m4x4_mul( m4x4f a
, m4x4f b
, m4x4f d
)
865 float a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
866 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
867 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
868 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
870 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2], b03
= b
[0][3],
871 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2], b13
= b
[1][3],
872 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2], b23
= b
[2][3],
873 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2], b33
= b
[3][3];
875 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
+ a30
*b03
;
876 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
+ a31
*b03
;
877 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
+ a32
*b03
;
878 d
[0][3] = a03
*b00
+ a13
*b01
+ a23
*b02
+ a33
*b03
;
879 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
+ a30
*b13
;
880 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
+ a31
*b13
;
881 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
+ a32
*b13
;
882 d
[1][3] = a03
*b10
+ a13
*b11
+ a23
*b12
+ a33
*b13
;
883 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
+ a30
*b23
;
884 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
+ a31
*b23
;
885 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
+ a32
*b23
;
886 d
[2][3] = a03
*b20
+ a13
*b21
+ a23
*b22
+ a33
*b23
;
887 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
*b33
;
888 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
*b33
;
889 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
*b33
;
890 d
[3][3] = a03
*b30
+ a13
*b31
+ a23
*b32
+ a33
*b33
;
894 * Planes (double precision)
896 static inline void tri_to_plane( double a
[3], double b
[3],
897 double c
[3], double p
[4] )
903 edge0
[0] = b
[0] - a
[0];
904 edge0
[1] = b
[1] - a
[1];
905 edge0
[2] = b
[2] - a
[2];
907 edge1
[0] = c
[0] - a
[0];
908 edge1
[1] = c
[1] - a
[1];
909 edge1
[2] = c
[2] - a
[2];
911 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
912 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
913 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
915 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
916 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
923 static inline int plane_intersect( double a
[4], double b
[4],
924 double c
[4], double p
[4] )
926 double const epsilon
= 1e-8f
;
931 x
[0] = a
[1] * b
[2] - a
[2] * b
[1];
932 x
[1] = a
[2] * b
[0] - a
[0] * b
[2];
933 x
[2] = a
[0] * b
[1] - a
[1] * b
[0];
935 d
= x
[0] * c
[0] + x
[1] * c
[1] + x
[2] * c
[2];
937 if( d
< epsilon
&& d
> -epsilon
) return 0;
939 p
[0] = (b
[1] * c
[2] - b
[2] * c
[1]) * -a
[3];
940 p
[1] = (b
[2] * c
[0] - b
[0] * c
[2]) * -a
[3];
941 p
[2] = (b
[0] * c
[1] - b
[1] * c
[0]) * -a
[3];
943 p
[0] += (c
[1] * a
[2] - c
[2] * a
[1]) * -b
[3];
944 p
[1] += (c
[2] * a
[0] - c
[0] * a
[2]) * -b
[3];
945 p
[2] += (c
[0] * a
[1] - c
[1] * a
[0]) * -b
[3];
947 p
[0] += (a
[1] * b
[2] - a
[2] * b
[1]) * -c
[3];
948 p
[1] += (a
[2] * b
[0] - a
[0] * b
[2]) * -c
[3];
949 p
[2] += (a
[0] * b
[1] - a
[1] * b
[0]) * -c
[3];
958 static inline double plane_polarity( double p
[4], double a
[3] )
961 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
962 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])
968 static inline void q_identity( v4f q
)
970 q
[0] = 0.0f
; q
[1] = 0.0f
; q
[2] = 0.0f
; q
[3] = 1.0f
;
973 static inline void q_axis_angle( v4f q
, v3f axis
, float angle
)
975 float a
= angle
*0.5f
,
985 static inline void q_mul( v4f q
, v4f q1
, v4f d
)
988 t
[0] = q
[3]*q1
[0] + q
[0]*q1
[3] + q
[1]*q1
[2] - q
[2]*q1
[1];
989 t
[1] = q
[3]*q1
[1] - q
[0]*q1
[2] + q
[1]*q1
[3] + q
[2]*q1
[0];
990 t
[2] = q
[3]*q1
[2] + q
[0]*q1
[1] - q
[1]*q1
[0] + q
[2]*q1
[3];
991 t
[3] = q
[3]*q1
[3] - q
[0]*q1
[0] - q
[1]*q1
[1] - q
[2]*q1
[2];
995 static inline void q_normalize( v4f q
)
997 float s
= 1.0f
/ sqrtf(v4_dot(q
,q
));
1004 static inline void q_inv( v4f q
, v4f d
)
1006 float s
= 1.0f
/ v4_dot(q
,q
);
1013 static inline void q_m3x3( v4f q
, m3x3f d
)
1017 s
= l
> 0.0f
? 2.0f
/l
: 0.0f
,
1019 xx
= s
*q
[0]*q
[0], xy
= s
*q
[0]*q
[1], wx
= s
*q
[3]*q
[0],
1020 yy
= s
*q
[1]*q
[1], yz
= s
*q
[1]*q
[2], wy
= s
*q
[3]*q
[1],
1021 zz
= s
*q
[2]*q
[2], xz
= s
*q
[0]*q
[2], wz
= s
*q
[3]*q
[2];
1023 d
[0][0] = 1.0f
- yy
- zz
;
1024 d
[1][1] = 1.0f
- xx
- zz
;
1025 d
[2][2] = 1.0f
- xx
- yy
;
1034 static void m3x3_q( m3x3f m
, v4f q
)
1036 float diag
, r
, rinv
;
1038 diag
= m
[0][0] + m
[1][1] + m
[2][2];
1041 r
= sqrtf( 1.0f
+ diag
);
1043 q
[0] = rinv
* (m
[1][2] - m
[2][1]);
1044 q
[1] = rinv
* (m
[2][0] - m
[0][2]);
1045 q
[2] = rinv
* (m
[0][1] - m
[1][0]);
1048 else if( m
[0][0] >= m
[1][1] && m
[0][0] >= m
[2][2] )
1050 r
= sqrtf( 1.0f
- m
[1][1] - m
[2][2] + m
[0][0] );
1053 q
[1] = rinv
* (m
[0][1] + m
[1][0]);
1054 q
[2] = rinv
* (m
[0][2] + m
[2][0]);
1055 q
[3] = rinv
* (m
[1][2] - m
[2][1]);
1057 else if( m
[1][1] >= m
[2][2] )
1059 r
= sqrtf( 1.0f
- m
[0][0] - m
[2][2] + m
[1][1] );
1061 q
[0] = rinv
* (m
[0][1] + m
[1][0]);
1063 q
[2] = rinv
* (m
[1][2] + m
[2][1]);
1064 q
[3] = rinv
* (m
[2][0] - m
[0][2]);
1068 r
= sqrtf( 1.0f
- m
[0][0] - m
[1][1] + m
[2][2] );
1070 q
[0] = rinv
* (m
[0][2] + m
[2][0]);
1071 q
[1] = rinv
* (m
[1][2] + m
[2][1]);
1073 q
[3] = rinv
* (m
[0][1] - m
[1][0]);