1 /* Copyright (C) 2021-2023 Harry Godden (hgn) - All Rights Reserved
19 * 5.d Raycast & Spherecasts
29 #include "vg_platform.h"
33 #define VG_PIf 3.14159265358979323846264338327950288f
34 #define VG_TAUf 6.28318530717958647692528676655900576f
36 * -----------------------------------------------------------------------------
37 * Section 0. Misc Operations
38 * -----------------------------------------------------------------------------
41 /* get the f32 as the raw bits in a u32 without converting */
42 static u32
vg_ftu32( f32 a
)
44 u32
*ptr
= (u32
*)(&a
);
48 /* check if f32 is infinite */
49 static int vg_isinff( f32 a
)
51 return ((vg_ftu32(a
)) & 0x7FFFFFFFU
) == 0x7F800000U
;
54 /* check if f32 is not a number */
55 static int vg_isnanf( f32 a
)
57 return !vg_isinff(a
) && ((vg_ftu32(a
)) & 0x7F800000U
) == 0x7F800000U
;
60 /* check if f32 is a number and is not infinite */
61 static int vg_validf( f32 a
)
63 return ((vg_ftu32(a
)) & 0x7F800000U
) != 0x7F800000U
;
66 static int v3_valid( v3f a
){
67 for( u32 i
=0; i
<3; i
++ )
68 if( !vg_validf(a
[i
]) ) return 0;
73 * -----------------------------------------------------------------------------
74 * Section 1. Scalar Operations
75 * -----------------------------------------------------------------------------
78 static inline f32
vg_minf( f32 a
, f32 b
){ return a
< b
? a
: b
; }
79 static inline f32
vg_maxf( f32 a
, f32 b
){ return a
> b
? a
: b
; }
81 static inline int vg_min( int a
, int b
){ return a
< b
? a
: b
; }
82 static inline int vg_max( int a
, int b
){ return a
> b
? a
: b
; }
84 static inline f32
vg_clampf( f32 a
, f32 min
, f32 max
)
86 return vg_minf( max
, vg_maxf( a
, min
) );
89 static inline f32
vg_signf( f32 a
)
91 return a
< 0.0f
? -1.0f
: 1.0f
;
94 static inline f32
vg_fractf( f32 a
)
96 return a
- floorf( a
);
99 static f32
vg_cfrictf( f32 velocity
, f32 F
)
101 return -vg_signf(velocity
) * vg_minf( F
, fabsf(velocity
) );
104 static inline f32
vg_rad( f32 deg
)
106 return deg
* VG_PIf
/ 180.0f
;
110 * -----------------------------------------------------------------------------
111 * Section 2.a 2D Vectors
112 * -----------------------------------------------------------------------------
115 static inline void v2_copy( v2f a
, v2f d
)
117 d
[0] = a
[0]; d
[1] = a
[1];
120 static inline void v2_zero( v2f a
)
122 a
[0] = 0.f
; a
[1] = 0.f
;
125 static inline void v2_add( v2f a
, v2f b
, v2f d
)
127 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
130 static inline void v2_sub( v2f a
, v2f b
, v2f d
)
132 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
135 static inline void v2_minv( v2f a
, v2f b
, v2f dest
)
137 dest
[0] = vg_minf(a
[0], b
[0]);
138 dest
[1] = vg_minf(a
[1], b
[1]);
141 static inline void v2_maxv( v2f a
, v2f b
, v2f dest
)
143 dest
[0] = vg_maxf(a
[0], b
[0]);
144 dest
[1] = vg_maxf(a
[1], b
[1]);
147 static inline f32
v2_dot( v2f a
, v2f b
)
149 return a
[0] * b
[0] + a
[1] * b
[1];
152 static inline f32
v2_cross( v2f a
, v2f b
)
154 return a
[0]*b
[1] - a
[1]*b
[0];
157 static inline void v2_abs( v2f a
, v2f d
)
159 d
[0] = fabsf( a
[0] );
160 d
[1] = fabsf( a
[1] );
163 static inline void v2_muls( v2f a
, f32 s
, v2f d
)
165 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
168 static inline void v2_divs( v2f a
, f32 s
, v2f d
)
170 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
;
173 static inline void v2_mul( v2f a
, v2f b
, v2f d
)
179 static inline void v2_div( v2f a
, v2f b
, v2f d
)
181 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
184 static inline void v2_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
186 d
[0] = a
[0]+b
[0]*s
[0];
187 d
[1] = a
[1]+b
[1]*s
[1];
190 static inline void v2_muladds( v2f a
, v2f b
, f32 s
, v2f d
)
196 static inline f32
v2_length2( v2f a
)
198 return a
[0]*a
[0] + a
[1]*a
[1];
201 static inline f32
v2_length( v2f a
)
203 return sqrtf( v2_length2( a
) );
206 static inline f32
v2_dist2( v2f a
, v2f b
)
209 v2_sub( a
, b
, delta
);
210 return v2_length2( delta
);
213 static inline f32
v2_dist( v2f a
, v2f b
)
215 return sqrtf( v2_dist2( a
, b
) );
218 static inline void v2_lerp( v2f a
, v2f b
, f32 t
, v2f d
)
220 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
221 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
224 static inline void v2_normalize( v2f a
)
226 v2_muls( a
, 1.0f
/ v2_length( a
), a
);
229 static void v2_normalize_clamp( v2f a
)
231 f32 l2
= v2_length2( a
);
233 v2_muls( a
, 1.0f
/sqrtf(l2
), a
);
236 static inline void v2_floor( v2f a
, v2f b
)
238 b
[0] = floorf( a
[0] );
239 b
[1] = floorf( a
[1] );
242 static inline void v2_fill( v2f a
, f32 v
)
248 static inline void v2_copysign( v2f a
, v2f b
)
250 a
[0] = copysignf( a
[0], b
[0] );
251 a
[1] = copysignf( a
[1], b
[1] );
255 * ---------------- */
257 static inline void v2i_copy( v2i a
, v2i b
)
259 b
[0] = a
[0]; b
[1] = a
[1];
262 static inline int v2i_eq( v2i a
, v2i b
)
264 return ((a
[0] == b
[0]) && (a
[1] == b
[1]));
267 static inline void v2i_add( v2i a
, v2i b
, v2i d
)
269 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
272 static inline void v2i_sub( v2i a
, v2i b
, v2i d
)
274 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
278 * -----------------------------------------------------------------------------
279 * Section 2.b 3D Vectors
280 * -----------------------------------------------------------------------------
283 static inline void v3_copy( v3f a
, v3f b
)
285 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
288 static inline void v3_zero( v3f a
)
290 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
293 static inline void v3_add( v3f a
, v3f b
, v3f d
)
295 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
298 static inline void v3i_add( v3i a
, v3i b
, v3i d
)
300 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
303 static inline void v3_sub( v3f a
, v3f b
, v3f d
)
305 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
308 static inline void v3i_sub( v3i a
, v3i b
, v3i d
)
310 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
313 static inline void v3_mul( v3f a
, v3f b
, v3f d
)
315 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
318 static inline void v3_div( v3f a
, v3f b
, v3f d
)
320 d
[0] = b
[0]!=0.0f
? a
[0]/b
[0]: INFINITY
;
321 d
[1] = b
[1]!=0.0f
? a
[1]/b
[1]: INFINITY
;
322 d
[2] = b
[2]!=0.0f
? a
[2]/b
[2]: INFINITY
;
325 static inline void v3_muls( v3f a
, f32 s
, v3f d
)
327 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
330 static inline void v3_fill( v3f a
, f32 v
)
337 static inline void v3_divs( v3f a
, f32 s
, v3f d
)
340 v3_fill( d
, INFINITY
);
349 static inline void v3_muladds( v3f a
, v3f b
, f32 s
, v3f d
)
351 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
354 static inline void v3_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
356 d
[0] = a
[0]+b
[0]*s
[0];
357 d
[1] = a
[1]+b
[1]*s
[1];
358 d
[2] = a
[2]+b
[2]*s
[2];
361 static inline f32
v3_dot( v3f a
, v3f b
)
363 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
366 static inline void v3_cross( v3f a
, v3f b
, v3f dest
)
369 d
[0] = a
[1]*b
[2] - a
[2]*b
[1];
370 d
[1] = a
[2]*b
[0] - a
[0]*b
[2];
371 d
[2] = a
[0]*b
[1] - a
[1]*b
[0];
375 static inline f32
v3_length2( v3f a
)
377 return v3_dot( a
, a
);
380 static inline f32
v3_length( v3f a
)
382 return sqrtf( v3_length2( a
) );
385 static inline f32
v3_dist2( v3f a
, v3f b
)
388 v3_sub( a
, b
, delta
);
389 return v3_length2( delta
);
392 static inline f32
v3_dist( v3f a
, v3f b
)
394 return sqrtf( v3_dist2( a
, b
) );
397 static inline void v3_normalize( v3f a
)
399 v3_muls( a
, 1.f
/ v3_length( a
), a
);
402 static inline f32
vg_lerpf( f32 a
, f32 b
, f32 t
){
406 static inline void vg_slewf( f32
*a
, f32 b
, f32 speed
){
407 f32 d
= vg_signf( b
-*a
),
409 *a
= vg_minf( b
*d
, c
*d
) * d
;
412 static inline f64
vg_lerp( f64 a
, f64 b
, f64 t
)
417 /* correctly lerp around circular period -pi -> pi */
418 static f32
vg_alerpf( f32 a
, f32 b
, f32 t
)
420 f32 d
= fmodf( b
-a
, VG_TAUf
),
421 s
= fmodf( 2.0f
*d
, VG_TAUf
) - d
;
425 static inline void v3_lerp( v3f a
, v3f b
, f32 t
, v3f d
)
427 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
428 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
429 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
432 static inline void v3_minv( v3f a
, v3f b
, v3f dest
)
434 dest
[0] = vg_minf(a
[0], b
[0]);
435 dest
[1] = vg_minf(a
[1], b
[1]);
436 dest
[2] = vg_minf(a
[2], b
[2]);
439 static inline void v3_maxv( v3f a
, v3f b
, v3f dest
)
441 dest
[0] = vg_maxf(a
[0], b
[0]);
442 dest
[1] = vg_maxf(a
[1], b
[1]);
443 dest
[2] = vg_maxf(a
[2], b
[2]);
446 static inline f32
v3_minf( v3f a
)
448 return vg_minf( vg_minf( a
[0], a
[1] ), a
[2] );
451 static inline f32
v3_maxf( v3f a
)
453 return vg_maxf( vg_maxf( a
[0], a
[1] ), a
[2] );
456 static inline void v3_floor( v3f a
, v3f b
)
458 b
[0] = floorf( a
[0] );
459 b
[1] = floorf( a
[1] );
460 b
[2] = floorf( a
[2] );
463 static inline void v3_ceil( v3f a
, v3f b
)
465 b
[0] = ceilf( a
[0] );
466 b
[1] = ceilf( a
[1] );
467 b
[2] = ceilf( a
[2] );
470 static inline void v3_negate( v3f a
, v3f b
)
477 static inline void v3_rotate( v3f v
, f32 angle
, v3f axis
, v3f d
)
488 v3_cross( k
, v
, v2
);
489 v3_muls( v2
, s
, v2
);
490 v3_add( v1
, v2
, v1
);
491 v3_muls( k
, v3_dot(k
, v
) * (1.0f
- c
), v2
);
495 static void v3_tangent_basis( v3f n
, v3f tx
, v3f ty
){
496 /* Compute tangent basis (box2d) */
497 if( fabsf( n
[0] ) >= 0.57735027f
){
509 v3_cross( n
, tx
, ty
);
514 * -----------------------------------------------------------------------------
515 * Section 2.c 4D Vectors
516 * -----------------------------------------------------------------------------
519 static inline void v4_copy( v4f a
, v4f b
)
521 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
524 static inline void v4_add( v4f a
, v4f b
, v4f d
)
532 static inline void v4_zero( v4f a
)
534 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
537 static inline void v4_muls( v4f a
, f32 s
, v4f d
)
545 static inline void v4_muladds( v4f a
, v4f b
, f32 s
, v4f d
)
553 static inline void v4_lerp( v4f a
, v4f b
, f32 t
, v4f d
)
555 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
556 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
557 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
558 d
[3] = a
[3] + t
*(b
[3]-a
[3]);
561 static inline f32
v4_dot( v4f a
, v4f b
)
563 return a
[0]*b
[0] + a
[1]*b
[1] + a
[2]*b
[2] + a
[3]*b
[3];
566 static inline f32
v4_length( v4f a
)
568 return sqrtf( v4_dot(a
,a
) );
572 * -----------------------------------------------------------------------------
573 * Section 3 Quaternions
574 * -----------------------------------------------------------------------------
577 static inline void q_identity( v4f q
)
579 q
[0] = 0.0f
; q
[1] = 0.0f
; q
[2] = 0.0f
; q
[3] = 1.0f
;
582 static inline void q_axis_angle( v4f q
, v3f axis
, f32 angle
)
594 static inline void q_mul( v4f q
, v4f q1
, v4f d
)
597 t
[0] = q
[3]*q1
[0] + q
[0]*q1
[3] + q
[1]*q1
[2] - q
[2]*q1
[1];
598 t
[1] = q
[3]*q1
[1] - q
[0]*q1
[2] + q
[1]*q1
[3] + q
[2]*q1
[0];
599 t
[2] = q
[3]*q1
[2] + q
[0]*q1
[1] - q
[1]*q1
[0] + q
[2]*q1
[3];
600 t
[3] = q
[3]*q1
[3] - q
[0]*q1
[0] - q
[1]*q1
[1] - q
[2]*q1
[2];
604 static inline void q_normalize( v4f q
)
606 f32 l2
= v4_dot(q
,q
);
607 if( l2
< 0.00001f
) q_identity( q
);
609 f32 s
= 1.0f
/sqrtf(l2
);
617 static inline void q_inv( v4f q
, v4f d
)
619 f32 s
= 1.0f
/ v4_dot(q
,q
);
626 static inline void q_nlerp( v4f a
, v4f b
, f32 t
, v4f d
)
628 if( v4_dot(a
,b
) < 0.0f
){
629 v4_muls( b
, -1.0f
, d
);
630 v4_lerp( a
, d
, t
, d
);
633 v4_lerp( a
, b
, t
, d
);
638 static inline void q_m3x3( v4f q
, m3x3f d
)
642 s
= l
> 0.0f
? 2.0f
/l
: 0.0f
,
644 xx
= s
*q
[0]*q
[0], xy
= s
*q
[0]*q
[1], wx
= s
*q
[3]*q
[0],
645 yy
= s
*q
[1]*q
[1], yz
= s
*q
[1]*q
[2], wy
= s
*q
[3]*q
[1],
646 zz
= s
*q
[2]*q
[2], xz
= s
*q
[0]*q
[2], wz
= s
*q
[3]*q
[2];
648 d
[0][0] = 1.0f
- yy
- zz
;
649 d
[1][1] = 1.0f
- xx
- zz
;
650 d
[2][2] = 1.0f
- xx
- yy
;
659 static void q_mulv( v4f q
, v3f v
, v3f d
)
663 v3_muls( q
, 2.0f
*v3_dot(q
,v
), v1
);
664 v3_muls( v
, q
[3]*q
[3] - v3_dot(q
,q
), v2
);
665 v3_add( v1
, v2
, v1
);
666 v3_cross( q
, v
, v2
);
667 v3_muls( v2
, 2.0f
*q
[3], v2
);
672 * -----------------------------------------------------------------------------
673 * Section 4.a 2x2 matrices
674 * -----------------------------------------------------------------------------
677 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
680 #define M2X2_ZERO {{0.0f, 0.0f, }, \
683 static inline void m2x2_copy( m2x2f a
, m2x2f b
)
685 v2_copy( a
[0], b
[0] );
686 v2_copy( a
[1], b
[1] );
689 static inline void m2x2_identity( m2x2f a
)
691 m2x2f id
= M2X2_INDENTIY
;
695 static inline void m2x2_create_rotation( m2x2f a
, f32 theta
)
709 * -----------------------------------------------------------------------------
710 * Section 4.b 3x3 matrices
711 * -----------------------------------------------------------------------------
714 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
715 { 0.0f, 1.0f, 0.0f, },\
716 { 0.0f, 0.0f, 1.0f, }}
718 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
719 { 0.0f, 0.0f, 0.0f, },\
720 { 0.0f, 0.0f, 0.0f, }}
723 static void euler_m3x3( v3f angles
, m3x3f d
)
725 f32 cosY
= cosf( angles
[0] ),
726 sinY
= sinf( angles
[0] ),
727 cosP
= cosf( angles
[1] ),
728 sinP
= sinf( angles
[1] ),
729 cosR
= cosf( angles
[2] ),
730 sinR
= sinf( angles
[2] );
732 d
[2][0] = -sinY
* cosP
;
734 d
[2][2] = cosY
* cosP
;
736 d
[0][0] = cosY
* cosR
;
738 d
[0][2] = sinY
* cosR
;
740 v3_cross( d
[0], d
[2], d
[1] );
743 static void m3x3_q( m3x3f m
, v4f q
)
747 diag
= m
[0][0] + m
[1][1] + m
[2][2];
750 r
= sqrtf( 1.0f
+ diag
);
752 q
[0] = rinv
* (m
[1][2] - m
[2][1]);
753 q
[1] = rinv
* (m
[2][0] - m
[0][2]);
754 q
[2] = rinv
* (m
[0][1] - m
[1][0]);
757 else if( m
[0][0] >= m
[1][1] && m
[0][0] >= m
[2][2] )
759 r
= sqrtf( 1.0f
- m
[1][1] - m
[2][2] + m
[0][0] );
762 q
[1] = rinv
* (m
[0][1] + m
[1][0]);
763 q
[2] = rinv
* (m
[0][2] + m
[2][0]);
764 q
[3] = rinv
* (m
[1][2] - m
[2][1]);
766 else if( m
[1][1] >= m
[2][2] )
768 r
= sqrtf( 1.0f
- m
[0][0] - m
[2][2] + m
[1][1] );
770 q
[0] = rinv
* (m
[0][1] + m
[1][0]);
772 q
[2] = rinv
* (m
[1][2] + m
[2][1]);
773 q
[3] = rinv
* (m
[2][0] - m
[0][2]);
777 r
= sqrtf( 1.0f
- m
[0][0] - m
[1][1] + m
[2][2] );
779 q
[0] = rinv
* (m
[0][2] + m
[2][0]);
780 q
[1] = rinv
* (m
[1][2] + m
[2][1]);
782 q
[3] = rinv
* (m
[0][1] - m
[1][0]);
786 /* a X b == [b]T a == ...*/
787 static void m3x3_skew_symetric( m3x3f a
, v3f v
)
800 static void m3x3_add( m3x3f a
, m3x3f b
, m3x3f d
)
802 v3_add( a
[0], b
[0], d
[0] );
803 v3_add( a
[1], b
[1], d
[1] );
804 v3_add( a
[2], b
[2], d
[2] );
807 static inline void m3x3_copy( m3x3f a
, m3x3f b
)
809 v3_copy( a
[0], b
[0] );
810 v3_copy( a
[1], b
[1] );
811 v3_copy( a
[2], b
[2] );
814 static inline void m3x3_identity( m3x3f a
)
816 m3x3f id
= M3X3_IDENTITY
;
820 static void m3x3_diagonal( m3x3f a
, f32 v
)
828 static void m3x3_setdiagonalv3( m3x3f a
, v3f v
)
835 static inline void m3x3_zero( m3x3f a
)
841 static inline void m3x3_inv( m3x3f src
, m3x3f dest
)
843 f32 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
844 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
845 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
852 dest
[0][0] = (e
*i
-h
*f
)*det
;
853 dest
[0][1] = -(b
*i
-c
*h
)*det
;
854 dest
[0][2] = (b
*f
-c
*e
)*det
;
855 dest
[1][0] = -(d
*i
-f
*g
)*det
;
856 dest
[1][1] = (a
*i
-c
*g
)*det
;
857 dest
[1][2] = -(a
*f
-d
*c
)*det
;
858 dest
[2][0] = (d
*h
-g
*e
)*det
;
859 dest
[2][1] = -(a
*h
-g
*b
)*det
;
860 dest
[2][2] = (a
*e
-d
*b
)*det
;
863 static f32
m3x3_det( m3x3f m
)
865 return m
[0][0] * (m
[1][1] * m
[2][2] - m
[2][1] * m
[1][2])
866 - m
[0][1] * (m
[1][0] * m
[2][2] - m
[1][2] * m
[2][0])
867 + m
[0][2] * (m
[1][0] * m
[2][1] - m
[1][1] * m
[2][0]);
870 static inline void m3x3_transpose( m3x3f src
, m3x3f dest
)
872 f32 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
873 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
874 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
887 static inline void m3x3_mul( m3x3f a
, m3x3f b
, m3x3f d
)
889 f32 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
890 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
891 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
893 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
894 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
895 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2];
897 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
898 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
899 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
900 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
901 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
902 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
903 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
904 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
905 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
908 static inline void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
912 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
913 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
914 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
919 static inline void m3x3_projection( m3x3f dst
,
920 f32
const left
, f32
const right
, f32
const bottom
, f32
const top
)
926 rl
= 1.0f
/ (right
- left
);
927 tb
= 1.0f
/ (top
- bottom
);
929 dst
[0][0] = 2.0f
* rl
;
930 dst
[1][1] = 2.0f
* tb
;
934 static inline void m3x3_translate( m3x3f m
, v3f v
)
936 m
[2][0] = m
[0][0] * v
[0] + m
[1][0] * v
[1] + m
[2][0];
937 m
[2][1] = m
[0][1] * v
[0] + m
[1][1] * v
[1] + m
[2][1];
938 m
[2][2] = m
[0][2] * v
[0] + m
[1][2] * v
[1] + m
[2][2];
941 static inline void m3x3_scale( m3x3f m
, v3f v
)
943 v3_muls( m
[0], v
[0], m
[0] );
944 v3_muls( m
[1], v
[1], m
[1] );
945 v3_muls( m
[2], v
[2], m
[2] );
948 static inline void m3x3_scalef( m3x3f m
, f32 f
)
955 static inline void m3x3_rotate( m3x3f m
, f32 angle
)
957 f32 m00
= m
[0][0], m10
= m
[1][0],
958 m01
= m
[0][1], m11
= m
[1][1],
959 m02
= m
[0][2], m12
= m
[1][2];
965 m
[0][0] = m00
* c
+ m10
* s
;
966 m
[0][1] = m01
* c
+ m11
* s
;
967 m
[0][2] = m02
* c
+ m12
* s
;
969 m
[1][0] = m00
* -s
+ m10
* c
;
970 m
[1][1] = m01
* -s
+ m11
* c
;
971 m
[1][2] = m02
* -s
+ m12
* c
;
975 * -----------------------------------------------------------------------------
976 * Section 4.c 4x3 matrices
977 * -----------------------------------------------------------------------------
980 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
981 { 0.0f, 1.0f, 0.0f, },\
982 { 0.0f, 0.0f, 1.0f, },\
983 { 0.0f, 0.0f, 0.0f }}
985 static inline void m4x3_to_3x3( m4x3f a
, m3x3f b
)
987 v3_copy( a
[0], b
[0] );
988 v3_copy( a
[1], b
[1] );
989 v3_copy( a
[2], b
[2] );
992 static inline void m4x3_invert_affine( m4x3f a
, m4x3f b
)
994 m3x3_transpose( a
, b
);
995 m3x3_mulv( b
, a
[3], b
[3] );
996 v3_negate( b
[3], b
[3] );
999 static void m4x3_invert_full( m4x3f src
, m4x3f dst
)
1003 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
1004 e
= src
[1][0], f
= src
[1][1], g
= src
[1][2],
1005 i
= src
[2][0], j
= src
[2][1], k
= src
[2][2],
1006 m
= src
[3][0], n
= src
[3][1], o
= src
[3][2];
1012 dst
[0][0] = f
*k
- g
*j
;
1013 dst
[1][0] =-(e
*k
- g
*i
);
1014 dst
[2][0] = e
*j
- f
*i
;
1015 dst
[3][0] =-(e
*t2
- f
*t4
+ g
*t5
);
1017 dst
[0][1] =-(b
*k
- c
*j
);
1018 dst
[1][1] = a
*k
- c
*i
;
1019 dst
[2][1] =-(a
*j
- b
*i
);
1020 dst
[3][1] = a
*t2
- b
*t4
+ c
*t5
;
1026 dst
[0][2] = b
*g
- c
*f
;
1027 dst
[1][2] =-(a
*g
- c
*e
);
1028 dst
[2][2] = a
*f
- b
*e
;
1029 dst
[3][2] =-(a
*t2
- b
*t4
+ c
* t5
);
1031 det
= 1.0f
/ (a
* dst
[0][0] + b
* dst
[1][0] + c
* dst
[2][0]);
1032 v3_muls( dst
[0], det
, dst
[0] );
1033 v3_muls( dst
[1], det
, dst
[1] );
1034 v3_muls( dst
[2], det
, dst
[2] );
1035 v3_muls( dst
[3], det
, dst
[3] );
1038 static inline void m4x3_copy( m4x3f a
, m4x3f b
)
1040 v3_copy( a
[0], b
[0] );
1041 v3_copy( a
[1], b
[1] );
1042 v3_copy( a
[2], b
[2] );
1043 v3_copy( a
[3], b
[3] );
1046 static inline void m4x3_identity( m4x3f a
)
1048 m4x3f id
= M4X3_IDENTITY
;
1052 static void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
1055 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
1056 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
1057 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
1058 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
1059 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
1060 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
1061 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
1062 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
1064 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
1065 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
1066 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
1067 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
1068 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
1069 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
1070 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
1071 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
1072 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
1073 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
1074 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
1075 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
1078 #if 0 /* shat appf mingw wstringop-overflow */
1081 static void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
1085 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
1086 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
1087 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
1093 * Transform plane ( xyz, distance )
1095 static void m4x3_mulp( m4x3f m
, v4f p
, v4f d
)
1099 v3_muls( p
, p
[3], o
);
1100 m4x3_mulv( m
, o
, o
);
1101 m3x3_mulv( m
, p
, d
);
1103 d
[3] = v3_dot( o
, d
);
1110 static void m4x3_translate( m4x3f m
, v3f v
)
1112 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
1113 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
1114 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
1117 static void m4x3_rotate_x( m4x3f m
, f32 angle
)
1119 m4x3f t
= M4X3_IDENTITY
;
1130 m4x3_mul( m
, t
, m
);
1133 static void m4x3_rotate_y( m4x3f m
, f32 angle
)
1135 m4x3f t
= M4X3_IDENTITY
;
1146 m4x3_mul( m
, t
, m
);
1149 static void m4x3_rotate_z( m4x3f m
, f32 angle
)
1151 m4x3f t
= M4X3_IDENTITY
;
1162 m4x3_mul( m
, t
, m
);
1165 static void m4x3_expand( m4x3f m
, m4x4f d
)
1167 v3_copy( m
[0], d
[0] );
1168 v3_copy( m
[1], d
[1] );
1169 v3_copy( m
[2], d
[2] );
1170 v3_copy( m
[3], d
[3] );
1177 static void m4x3_decompose( m4x3f m
, v3f co
, v4f q
, v3f s
)
1179 v3_copy( m
[3], co
);
1180 s
[0] = v3_length(m
[0]);
1181 s
[1] = v3_length(m
[1]);
1182 s
[2] = v3_length(m
[2]);
1185 v3_divs( m
[0], s
[0], rot
[0] );
1186 v3_divs( m
[1], s
[1], rot
[1] );
1187 v3_divs( m
[2], s
[2], rot
[2] );
1192 static void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
)
1195 m4x3_mulv( m
, point
, v
);
1197 v3_minv( box
[0], v
, box
[0] );
1198 v3_maxv( box
[1], v
, box
[1] );
1201 static void m4x3_transform_aabb( m4x3f m
, boxf box
)
1205 v3_copy( box
[0], a
);
1206 v3_copy( box
[1], b
);
1207 v3_fill( box
[0], INFINITY
);
1208 v3_fill( box
[1], -INFINITY
);
1210 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], a
[1], a
[2] } );
1211 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], a
[2] } );
1212 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], a
[2] } );
1213 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], a
[2] } );
1215 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], a
[1], b
[2] } );
1216 m4x3_expand_aabb_point( m
, box
, (v3f
){ a
[0], b
[1], b
[2] } );
1217 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], b
[1], b
[2] } );
1218 m4x3_expand_aabb_point( m
, box
, (v3f
){ b
[0], a
[1], b
[2] } );
1221 static inline void m4x3_lookat( m4x3f m
, v3f pos
, v3f target
, v3f up
)
1224 v3_sub( target
, pos
, dir
);
1225 v3_normalize( dir
);
1227 v3_copy( dir
, m
[2] );
1229 v3_cross( up
, m
[2], m
[0] );
1230 v3_normalize( m
[0] );
1232 v3_cross( m
[2], m
[0], m
[1] );
1233 v3_copy( pos
, m
[3] );
1237 * -----------------------------------------------------------------------------
1238 * Section 4.d 4x4 matrices
1239 * -----------------------------------------------------------------------------
1242 #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\
1243 { 0.0f, 1.0f, 0.0f, 0.0f },\
1244 { 0.0f, 0.0f, 1.0f, 0.0f },\
1245 { 0.0f, 0.0f, 0.0f, 1.0f }}
1246 #define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\
1247 { 0.0f, 0.0f, 0.0f, 0.0f },\
1248 { 0.0f, 0.0f, 0.0f, 0.0f },\
1249 { 0.0f, 0.0f, 0.0f, 0.0f }}
1251 static void m4x4_projection( m4x4f m
, f32 angle
,
1252 f32 ratio
, f32 fnear
, f32 ffar
)
1254 f32 scale
= tanf( angle
* 0.5f
* VG_PIf
/ 180.0f
) * fnear
,
1260 m
[0][0] = 2.0f
* fnear
/ (r
- l
);
1266 m
[1][1] = 2.0f
* fnear
/ (t
- b
);
1270 m
[2][0] = (r
+ l
) / (r
- l
);
1271 m
[2][1] = (t
+ b
) / (t
- b
);
1272 m
[2][2] = -(ffar
+ fnear
) / (ffar
- fnear
);
1277 m
[3][2] = -2.0f
* ffar
* fnear
/ (ffar
- fnear
);
1281 static void m4x4_translate( m4x4f m
, v3f v
)
1283 v4_muladds( m
[3], m
[0], v
[0], m
[3] );
1284 v4_muladds( m
[3], m
[1], v
[1], m
[3] );
1285 v4_muladds( m
[3], m
[2], v
[2], m
[3] );
1288 static inline void m4x4_copy( m4x4f a
, m4x4f b
)
1290 v4_copy( a
[0], b
[0] );
1291 v4_copy( a
[1], b
[1] );
1292 v4_copy( a
[2], b
[2] );
1293 v4_copy( a
[3], b
[3] );
1296 static inline void m4x4_identity( m4x4f a
)
1298 m4x4f id
= M4X4_IDENTITY
;
1302 static inline void m4x4_zero( m4x4f a
)
1304 m4x4f zero
= M4X4_ZERO
;
1305 m4x4_copy( zero
, a
);
1308 static inline void m4x4_mul( m4x4f a
, m4x4f b
, m4x4f d
)
1310 f32 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
1311 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
1312 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
1313 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
1315 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2], b03
= b
[0][3],
1316 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2], b13
= b
[1][3],
1317 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2], b23
= b
[2][3],
1318 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2], b33
= b
[3][3];
1320 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
+ a30
*b03
;
1321 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
+ a31
*b03
;
1322 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
+ a32
*b03
;
1323 d
[0][3] = a03
*b00
+ a13
*b01
+ a23
*b02
+ a33
*b03
;
1324 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
+ a30
*b13
;
1325 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
+ a31
*b13
;
1326 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
+ a32
*b13
;
1327 d
[1][3] = a03
*b10
+ a13
*b11
+ a23
*b12
+ a33
*b13
;
1328 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
+ a30
*b23
;
1329 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
+ a31
*b23
;
1330 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
+ a32
*b23
;
1331 d
[2][3] = a03
*b20
+ a13
*b21
+ a23
*b22
+ a33
*b23
;
1332 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
*b33
;
1333 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
*b33
;
1334 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
*b33
;
1335 d
[3][3] = a03
*b30
+ a13
*b31
+ a23
*b32
+ a33
*b33
;
1338 static inline void m4x4_mulv( m4x4f m
, v4f v
, v4f d
)
1342 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0]*v
[3];
1343 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1]*v
[3];
1344 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2]*v
[3];
1345 res
[3] = m
[0][3]*v
[0] + m
[1][3]*v
[1] + m
[2][3]*v
[2] + m
[3][3]*v
[3];
1350 static inline void m4x4_inv( m4x4f a
, m4x4f d
)
1352 f32 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
1353 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
1354 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
1355 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
1359 t
[0] = a22
*a33
- a32
*a23
;
1360 t
[1] = a21
*a33
- a31
*a23
;
1361 t
[2] = a21
*a32
- a31
*a22
;
1362 t
[3] = a20
*a33
- a30
*a23
;
1363 t
[4] = a20
*a32
- a30
*a22
;
1364 t
[5] = a20
*a31
- a30
*a21
;
1366 d
[0][0] = a11
*t
[0] - a12
*t
[1] + a13
*t
[2];
1367 d
[1][0] =-(a10
*t
[0] - a12
*t
[3] + a13
*t
[4]);
1368 d
[2][0] = a10
*t
[1] - a11
*t
[3] + a13
*t
[5];
1369 d
[3][0] =-(a10
*t
[2] - a11
*t
[4] + a12
*t
[5]);
1371 d
[0][1] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
1372 d
[1][1] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
1373 d
[2][1] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
1374 d
[3][1] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
1376 t
[0] = a12
*a33
- a32
*a13
;
1377 t
[1] = a11
*a33
- a31
*a13
;
1378 t
[2] = a11
*a32
- a31
*a12
;
1379 t
[3] = a10
*a33
- a30
*a13
;
1380 t
[4] = a10
*a32
- a30
*a12
;
1381 t
[5] = a10
*a31
- a30
*a11
;
1383 d
[0][2] = a01
*t
[0] - a02
*t
[1] + a03
*t
[2];
1384 d
[1][2] =-(a00
*t
[0] - a02
*t
[3] + a03
*t
[4]);
1385 d
[2][2] = a00
*t
[1] - a01
*t
[3] + a03
*t
[5];
1386 d
[3][2] =-(a00
*t
[2] - a01
*t
[4] + a02
*t
[5]);
1388 t
[0] = a12
*a23
- a22
*a13
;
1389 t
[1] = a11
*a23
- a21
*a13
;
1390 t
[2] = a11
*a22
- a21
*a12
;
1391 t
[3] = a10
*a23
- a20
*a13
;
1392 t
[4] = a10
*a22
- a20
*a12
;
1393 t
[5] = a10
*a21
- a20
*a11
;
1395 d
[0][3] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
1396 d
[1][3] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
1397 d
[2][3] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
1398 d
[3][3] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
1400 det
= 1.0f
/ (a00
*d
[0][0] + a01
*d
[1][0] + a02
*d
[2][0] + a03
*d
[3][0]);
1401 v4_muls( d
[0], det
, d
[0] );
1402 v4_muls( d
[1], det
, d
[1] );
1403 v4_muls( d
[2], det
, d
[2] );
1404 v4_muls( d
[3], det
, d
[3] );
1408 * -----------------------------------------------------------------------------
1410 * -----------------------------------------------------------------------------
1413 static inline void box_addpt( boxf a
, v3f pt
)
1415 v3_minv( a
[0], pt
, a
[0] );
1416 v3_maxv( a
[1], pt
, a
[1] );
1419 static inline void box_concat( boxf a
, boxf b
)
1421 v3_minv( a
[0], b
[0], a
[0] );
1422 v3_maxv( a
[1], b
[1], a
[1] );
1425 static inline void box_copy( boxf a
, boxf b
)
1427 v3_copy( a
[0], b
[0] );
1428 v3_copy( a
[1], b
[1] );
1431 static inline int box_overlap( boxf a
, boxf b
)
1434 ( a
[0][0] <= b
[1][0] && a
[1][0] >= b
[0][0] ) &&
1435 ( a
[0][1] <= b
[1][1] && a
[1][1] >= b
[0][1] ) &&
1436 ( a
[0][2] <= b
[1][2] && a
[1][2] >= b
[0][2] )
1440 static int box_within( boxf greater
, boxf lesser
)
1443 v3_sub( lesser
[0], greater
[0], a
);
1444 v3_sub( lesser
[1], greater
[1], b
);
1446 if( (a
[0] >= 0.0f
) && (a
[1] >= 0.0f
) && (a
[2] >= 0.0f
) &&
1447 (b
[0] <= 0.0f
) && (b
[1] <= 0.0f
) && (b
[2] <= 0.0f
) )
1455 static inline void box_init_inf( boxf box
)
1457 v3_fill( box
[0], INFINITY
);
1458 v3_fill( box
[1], -INFINITY
);
1462 * -----------------------------------------------------------------------------
1463 * Section 5.b Planes
1464 * -----------------------------------------------------------------------------
1467 static inline void tri_to_plane( f64 a
[3], f64 b
[3],
1468 f64 c
[3], f64 p
[4] )
1474 edge0
[0] = b
[0] - a
[0];
1475 edge0
[1] = b
[1] - a
[1];
1476 edge0
[2] = b
[2] - a
[2];
1478 edge1
[0] = c
[0] - a
[0];
1479 edge1
[1] = c
[1] - a
[1];
1480 edge1
[2] = c
[2] - a
[2];
1482 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
1483 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
1484 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
1486 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
1487 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
1494 static int plane_intersect3( v4f a
, v4f b
, v4f c
, v3f p
)
1496 f32
const epsilon
= 1e-6f
;
1499 v3_cross( a
, b
, x
);
1500 f32 d
= v3_dot( x
, c
);
1502 if( (d
< epsilon
) && (d
> -epsilon
) ) return 0;
1505 v3_cross( b
, c
, v0
);
1506 v3_cross( c
, a
, v1
);
1507 v3_cross( a
, b
, v2
);
1509 v3_muls( v0
, a
[3], p
);
1510 v3_muladds( p
, v1
, b
[3], p
);
1511 v3_muladds( p
, v2
, c
[3], p
);
1517 int plane_intersect2( v4f a
, v4f b
, v3f p
, v3f n
)
1519 f32
const epsilon
= 1e-6f
;
1522 v3_cross( a
, b
, c
);
1523 f32 d
= v3_length2( c
);
1525 if( (d
< epsilon
) && (d
> -epsilon
) )
1529 v3_cross( c
, b
, v0
);
1530 v3_cross( a
, c
, v1
);
1532 v3_muls( v0
, a
[3], vx
);
1533 v3_muladds( vx
, v1
, b
[3], vx
);
1534 v3_divs( vx
, d
, p
);
1540 static int plane_segment( v4f plane
, v3f a
, v3f b
, v3f co
)
1542 f32 d0
= v3_dot( a
, plane
) - plane
[3],
1543 d1
= v3_dot( b
, plane
) - plane
[3];
1547 f32 tot
= 1.0f
/( fabsf(d0
)+fabsf(d1
) );
1549 v3_muls( a
, fabsf(d1
) * tot
, co
);
1550 v3_muladds( co
, b
, fabsf(d0
) * tot
, co
);
1557 static inline f64
plane_polarity( f64 p
[4], f64 a
[3] )
1560 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
1561 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])
1566 * -----------------------------------------------------------------------------
1567 * Section 5.c Closest point functions
1568 * -----------------------------------------------------------------------------
1572 * These closest point tests were learned from Real-Time Collision Detection by
1575 VG_STATIC f32
closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
1576 f32
*s
, f32
*t
, v3f c1
, v3f c2
)
1579 v3_sub( q1
, p1
, d1
);
1580 v3_sub( q2
, p2
, d2
);
1581 v3_sub( p1
, p2
, r
);
1583 f32 a
= v3_length2( d1
),
1584 e
= v3_length2( d2
),
1585 f
= v3_dot( d2
, r
);
1587 const f32 kEpsilon
= 0.0001f
;
1589 if( a
<= kEpsilon
&& e
<= kEpsilon
)
1597 v3_sub( c1
, c2
, v0
);
1599 return v3_length2( v0
);
1605 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
1609 f32 c
= v3_dot( d1
, r
);
1613 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
1617 f32 b
= v3_dot(d1
,d2
),
1622 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
1629 *t
= (b
*(*s
)+f
) / e
;
1634 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
1636 else if( *t
> 1.0f
)
1639 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
1644 v3_muladds( p1
, d1
, *s
, c1
);
1645 v3_muladds( p2
, d2
, *t
, c2
);
1648 v3_sub( c1
, c2
, v0
);
1649 return v3_length2( v0
);
1652 VG_STATIC
int point_inside_aabb( boxf box
, v3f point
)
1654 if((point
[0]<=box
[1][0]) && (point
[1]<=box
[1][1]) && (point
[2]<=box
[1][2]) &&
1655 (point
[0]>=box
[0][0]) && (point
[1]>=box
[0][1]) && (point
[2]>=box
[0][2]) )
1661 VG_STATIC
void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
1663 v3_maxv( p
, box
[0], dest
);
1664 v3_minv( dest
, box
[1], dest
);
1667 VG_STATIC
void closest_point_obb( v3f p
, boxf box
,
1668 m4x3f mtx
, m4x3f inv_mtx
, v3f dest
)
1671 m4x3_mulv( inv_mtx
, p
, local
);
1672 closest_point_aabb( local
, box
, local
);
1673 m4x3_mulv( mtx
, local
, dest
);
1676 VG_STATIC f32
closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
1680 v3_sub( point
, a
, v1
);
1682 f32 t
= v3_dot( v1
, v0
) / v3_length2(v0
);
1683 t
= vg_clampf(t
,0.0f
,1.0f
);
1684 v3_muladds( a
, v0
, t
, dest
);
1688 VG_STATIC
void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
1693 /* Region outside A */
1694 v3_sub( tri
[1], tri
[0], ab
);
1695 v3_sub( tri
[2], tri
[0], ac
);
1696 v3_sub( p
, tri
[0], ap
);
1700 if( d1
<= 0.0f
&& d2
<= 0.0f
)
1702 v3_copy( tri
[0], dest
);
1703 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1707 /* Region outside B */
1711 v3_sub( p
, tri
[1], bp
);
1712 d3
= v3_dot( ab
, bp
);
1713 d4
= v3_dot( ac
, bp
);
1715 if( d3
>= 0.0f
&& d4
<= d3
)
1717 v3_copy( tri
[1], dest
);
1718 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1722 /* Edge region of AB */
1723 f32 vc
= d1
*d4
- d3
*d2
;
1724 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
1726 f32 v
= d1
/ (d1
-d3
);
1727 v3_muladds( tri
[0], ab
, v
, dest
);
1728 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1732 /* Region outside C */
1735 v3_sub( p
, tri
[2], cp
);
1736 d5
= v3_dot(ab
, cp
);
1737 d6
= v3_dot(ac
, cp
);
1739 if( d6
>= 0.0f
&& d5
<= d6
)
1741 v3_copy( tri
[2], dest
);
1742 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1747 f32 vb
= d5
*d2
- d1
*d6
;
1748 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
1750 f32 w
= d2
/ (d2
-d6
);
1751 v3_muladds( tri
[0], ac
, w
, dest
);
1752 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1757 f32 va
= d3
*d6
- d5
*d4
;
1758 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
1760 f32 w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
1762 v3_sub( tri
[2], tri
[1], bc
);
1763 v3_muladds( tri
[1], bc
, w
, dest
);
1764 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1768 /* P inside region, Q via barycentric coordinates uvw */
1769 f32 d
= 1.0f
/(va
+vb
+vc
),
1773 v3_muladds( tri
[0], ab
, v
, dest
);
1774 v3_muladds( dest
, ac
, w
, dest
);
1779 k_contact_type_default
,
1780 k_contact_type_disabled
,
1784 VG_STATIC
enum contact_type
closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
1789 /* Region outside A */
1790 v3_sub( tri
[1], tri
[0], ab
);
1791 v3_sub( tri
[2], tri
[0], ac
);
1792 v3_sub( p
, tri
[0], ap
);
1796 if( d1
<= 0.0f
&& d2
<= 0.0f
)
1798 v3_copy( tri
[0], dest
);
1799 return k_contact_type_default
;
1802 /* Region outside B */
1806 v3_sub( p
, tri
[1], bp
);
1807 d3
= v3_dot( ab
, bp
);
1808 d4
= v3_dot( ac
, bp
);
1810 if( d3
>= 0.0f
&& d4
<= d3
)
1812 v3_copy( tri
[1], dest
);
1813 return k_contact_type_edge
;
1816 /* Edge region of AB */
1817 f32 vc
= d1
*d4
- d3
*d2
;
1818 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
1820 f32 v
= d1
/ (d1
-d3
);
1821 v3_muladds( tri
[0], ab
, v
, dest
);
1822 return k_contact_type_edge
;
1825 /* Region outside C */
1828 v3_sub( p
, tri
[2], cp
);
1829 d5
= v3_dot(ab
, cp
);
1830 d6
= v3_dot(ac
, cp
);
1832 if( d6
>= 0.0f
&& d5
<= d6
)
1834 v3_copy( tri
[2], dest
);
1835 return k_contact_type_edge
;
1839 f32 vb
= d5
*d2
- d1
*d6
;
1840 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
1842 f32 w
= d2
/ (d2
-d6
);
1843 v3_muladds( tri
[0], ac
, w
, dest
);
1844 return k_contact_type_edge
;
1848 f32 va
= d3
*d6
- d5
*d4
;
1849 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
1851 f32 w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
1853 v3_sub( tri
[2], tri
[1], bc
);
1854 v3_muladds( tri
[1], bc
, w
, dest
);
1855 return k_contact_type_edge
;
1858 /* P inside region, Q via barycentric coordinates uvw */
1859 f32 d
= 1.0f
/(va
+vb
+vc
),
1863 v3_muladds( tri
[0], ab
, v
, dest
);
1864 v3_muladds( dest
, ac
, w
, dest
);
1866 return k_contact_type_default
;
1869 static void closest_point_elipse( v2f p
, v2f e
, v2f o
)
1871 v2f pabs
, ei
, e2
, ve
, t
;
1874 v2_div( (v2f
){ 1.0f
, 1.0f
}, e
, ei
);
1876 v2_mul( ei
, (v2f
){ e2
[0]-e2
[1], e2
[1]-e2
[0] }, ve
);
1878 v2_fill( t
, 0.70710678118654752f
);
1880 for( int i
=0; i
<3; i
++ ){
1883 v2_mul( ve
, t
, v
); /* ve*t*t*t */
1887 v2_sub( pabs
, v
, u
);
1891 v2_sub( ud
, v
, ud
);
1893 v2_muls( u
, v2_length( ud
), u
);
1898 v2_maxv( (v2f
){0.0f
,0.0f
}, w
, t
);
1903 v2_copysign( o
, p
);
1907 * -----------------------------------------------------------------------------
1908 * Section 5.d Raycasts & Spherecasts
1909 * -----------------------------------------------------------------------------
1912 int ray_aabb1( boxf box
, v3f co
, v3f dir_inv
, f32 dist
)
1917 v3_sub( box
[0], co
, v0
);
1918 v3_sub( box
[1], co
, v1
);
1920 v3_mul( v0
, dir_inv
, v0
);
1921 v3_mul( v1
, dir_inv
, v1
);
1923 tmin
= vg_minf( v0
[0], v1
[0] );
1924 tmax
= vg_maxf( v0
[0], v1
[0] );
1925 tmin
= vg_maxf( tmin
, vg_minf( v0
[1], v1
[1] ));
1926 tmax
= vg_minf( tmax
, vg_maxf( v0
[1], v1
[1] ));
1927 tmin
= vg_maxf( tmin
, vg_minf( v0
[2], v1
[2] ));
1928 tmax
= vg_minf( tmax
, vg_maxf( v0
[2], v1
[2] ));
1930 return (tmax
>= tmin
) && (tmin
<= dist
) && (tmax
>= 0.0f
);
1933 /* Time of intersection with ray vs triangle */
1934 static int ray_tri( v3f tri
[3], v3f co
,
1935 v3f dir
, f32
*dist
)
1937 f32
const kEpsilon
= 0.00001f
;
1939 v3f v0
, v1
, h
, s
, q
, n
;
1946 v3_sub( pb
, pa
, v0
);
1947 v3_sub( pc
, pa
, v1
);
1948 v3_cross( dir
, v1
, h
);
1949 v3_cross( v0
, v1
, n
);
1951 if( v3_dot( n
, dir
) > 0.0f
) /* Backface culling */
1955 a
= v3_dot( v0
, h
);
1957 if( a
> -kEpsilon
&& a
< kEpsilon
)
1961 v3_sub( co
, pa
, s
);
1963 u
= f
* v3_dot(s
, h
);
1964 if( u
< 0.0f
|| u
> 1.0f
)
1967 v3_cross( s
, v0
, q
);
1968 v
= f
* v3_dot( dir
, q
);
1969 if( v
< 0.0f
|| u
+v
> 1.0f
)
1972 t
= f
* v3_dot(v1
, q
);
1981 /* time of intersection with ray vs sphere */
1982 static int ray_sphere( v3f c
, f32 r
,
1983 v3f co
, v3f dir
, f32
*t
)
1988 f32 b
= v3_dot( m
, dir
),
1989 c1
= v3_dot( m
, m
) - r
*r
;
1991 /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */
1992 if( c1
> 0.0f
&& b
> 0.0f
)
1995 f32 discr
= b
*b
- c1
;
1997 /* A negative discriminant corresponds to ray missing sphere */
2002 * Ray now found to intersect sphere, compute smallest t value of
2005 *t
= -b
- sqrtf( discr
);
2007 /* If t is negative, ray started inside sphere so clamp t to zero */
2015 * time of intersection of ray vs cylinder
2016 * The cylinder does not have caps but is finite
2018 * Heavily adapted from regular segment vs cylinder from:
2019 * Real-Time Collision Detection
2021 static int ray_uncapped_finite_cylinder( v3f q
, v3f p
, f32 r
,
2022 v3f co
, v3f dir
, f32
*t
)
2025 v3_muladds( co
, dir
, 1.0f
, sb
);
2029 v3_sub( sb
, co
, n
);
2031 f32 md
= v3_dot( m
, d
),
2032 nd
= v3_dot( n
, d
),
2033 dd
= v3_dot( d
, d
),
2034 nn
= v3_dot( n
, n
),
2035 mn
= v3_dot( m
, n
),
2037 k
= v3_dot( m
, m
) - r
*r
,
2040 if( fabsf(a
) < 0.00001f
)
2042 /* Segment runs parallel to cylinder axis */
2046 f32 b
= dd
*mn
- nd
*md
,
2050 return 0; /* No real roots; no intersection */
2052 *t
= (-b
- sqrtf(discr
)) / a
;
2054 return 0; /* Intersection behind ray */
2056 /* Check within cylinder segment */
2057 if( md
+ (*t
)*nd
< 0.0f
)
2060 if( md
+ (*t
)*nd
> dd
)
2063 /* Segment intersects cylinder between the endcaps; t is correct */
2068 * Time of intersection of sphere and triangle. Origin must be outside the
2069 * colliding area. This is a fairly long procedure.
2071 static int spherecast_triangle( v3f tri
[3],
2072 v3f co
, v3f dir
, f32 r
, f32
*t
, v3f n
)
2077 v3_sub( tri
[1], tri
[0], v0
);
2078 v3_sub( tri
[2], tri
[0], v1
);
2079 v3_cross( v0
, v1
, n
);
2081 v3_muladds( tri
[0], n
, r
, sum
[0] );
2082 v3_muladds( tri
[1], n
, r
, sum
[1] );
2083 v3_muladds( tri
[2], n
, r
, sum
[2] );
2086 f32 t_min
= INFINITY
,
2089 if( ray_tri( sum
, co
, dir
, &t1
) ){
2090 t_min
= vg_minf( t_min
, t1
);
2095 * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
2098 for( int i
=0; i
<3; i
++ ){
2099 if( ray_sphere( tri
[i
], r
, co
, dir
, &t1
) ){
2100 t_min
= vg_minf( t_min
, t1
);
2106 for( int i
=0; i
<3; i
++ ){
2110 if( ray_uncapped_finite_cylinder( tri
[i0
], tri
[i1
], r
, co
, dir
, &t1
) ){
2115 v3_add( dir
, co
, co1
);
2116 v3_lerp( co
, co1
, t_min
, ct
);
2118 closest_point_segment( tri
[i0
], tri
[i1
], ct
, cx
);
2119 v3_sub( ct
, cx
, n
);
2132 * -----------------------------------------------------------------------------
2133 * Section 5.e Curves
2134 * -----------------------------------------------------------------------------
2137 static void eval_bezier_time( v3f p0
, v3f p1
, v3f h0
, v3f h1
, f32 t
, v3f p
)
2142 v3_muls( p1
, ttt
, p
);
2143 v3_muladds( p
, h1
, 3.0f
*tt
-3.0f
*ttt
, p
);
2144 v3_muladds( p
, h0
, 3.0f
*ttt
-6.0f
*tt
+3.0f
*t
, p
);
2145 v3_muladds( p
, p0
, 3.0f
*tt
-ttt
-3.0f
*t
+1.0f
, p
);
2148 static void eval_bezier3( v3f p0
, v3f p1
, v3f p2
, f32 t
, v3f p
)
2152 v3_muls( p0
, u
*u
, p
);
2153 v3_muladds( p
, p1
, 2.0f
*u
*t
, p
);
2154 v3_muladds( p
, p2
, t
*t
, p
);
2158 * -----------------------------------------------------------------------------
2159 * Section 5.f Volumes
2160 * -----------------------------------------------------------------------------
2163 static float vg_sphere_volume( float radius
){
2164 float r3
= radius
*radius
*radius
;
2165 return (4.0f
/3.0f
) * VG_PIf
* r3
;
2169 * -----------------------------------------------------------------------------
2170 * Section 6.a PSRNG and some distributions
2171 * -----------------------------------------------------------------------------
2174 /* An implementation of the MT19937 Algorithm for the Mersenne Twister
2175 * by Evan Sultanik. Based upon the pseudocode in: M. Matsumoto and
2176 * T. Nishimura, "Mersenne Twister: A 623-dimensionally
2177 * equidistributed uniform pseudorandom number generator," ACM
2178 * Transactions on Modeling and Computer Simulation Vol. 8, No. 1,
2179 * January pp.3-30 1998.
2181 * http://www.sultanik.com/Mersenne_twister
2182 * https://github.com/ESultanik/mtwister/blob/master/mtwister.c
2185 #define MT_UPPER_MASK 0x80000000
2186 #define MT_LOWER_MASK 0x7fffffff
2187 #define MT_TEMPERING_MASK_B 0x9d2c5680
2188 #define MT_TEMPERING_MASK_C 0xefc60000
2190 #define MT_STATE_VECTOR_LENGTH 624
2192 /* changes to STATE_VECTOR_LENGTH also require changes to this */
2193 #define MT_STATE_VECTOR_M 397
2196 u32 mt
[MT_STATE_VECTOR_LENGTH
];
2201 static void vg_rand_seed( unsigned long seed
)
2203 /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator
2204 * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer
2205 * Programming," Vol. 2 (2nd Ed.) pp.102.
2207 vg_rand
.mt
[0] = seed
& 0xffffffff;
2208 for( vg_rand
.index
=1; vg_rand
.index
<MT_STATE_VECTOR_LENGTH
; vg_rand
.index
++){
2209 vg_rand
.mt
[vg_rand
.index
] =
2210 (6069 * vg_rand
.mt
[vg_rand
.index
-1]) & 0xffffffff;
2215 * Generates a pseudo-randomly generated long.
2217 static u32
vg_randu32(void)
2220 /* mag[x] = x * 0x9908b0df for x = 0,1 */
2221 static u32 mag
[2] = {0x0, 0x9908b0df};
2222 if( vg_rand
.index
>= MT_STATE_VECTOR_LENGTH
|| vg_rand
.index
< 0 ){
2223 /* generate STATE_VECTOR_LENGTH words at a time */
2225 if( vg_rand
.index
>= MT_STATE_VECTOR_LENGTH
+1 || vg_rand
.index
< 0 ){
2226 vg_rand_seed( 4357 );
2228 for( kk
=0; kk
<MT_STATE_VECTOR_LENGTH
-MT_STATE_VECTOR_M
; kk
++ ){
2229 y
= (vg_rand
.mt
[kk
] & MT_UPPER_MASK
) |
2230 (vg_rand
.mt
[kk
+1] & MT_LOWER_MASK
);
2231 vg_rand
.mt
[kk
] = vg_rand
.mt
[kk
+MT_STATE_VECTOR_M
] ^
2232 (y
>> 1) ^ mag
[y
& 0x1];
2234 for( ; kk
<MT_STATE_VECTOR_LENGTH
-1; kk
++ ){
2235 y
= (vg_rand
.mt
[kk
] & MT_UPPER_MASK
) |
2236 (vg_rand
.mt
[kk
+1] & MT_LOWER_MASK
);
2238 vg_rand
.mt
[ kk
+(MT_STATE_VECTOR_M
-MT_STATE_VECTOR_LENGTH
)] ^
2239 (y
>> 1) ^ mag
[y
& 0x1];
2241 y
= (vg_rand
.mt
[MT_STATE_VECTOR_LENGTH
-1] & MT_UPPER_MASK
) |
2242 (vg_rand
.mt
[0] & MT_LOWER_MASK
);
2243 vg_rand
.mt
[MT_STATE_VECTOR_LENGTH
-1] =
2244 vg_rand
.mt
[MT_STATE_VECTOR_M
-1] ^ (y
>> 1) ^ mag
[y
& 0x1];
2247 y
= vg_rand
.mt
[vg_rand
.index
++];
2249 y
^= (y
<< 7) & MT_TEMPERING_MASK_B
;
2250 y
^= (y
<< 15) & MT_TEMPERING_MASK_C
;
2256 * Generates a pseudo-randomly generated f64 in the range [0..1].
2258 static inline f64
vg_randf64(void)
2260 return (f64
)vg_randu32()/(f64
)0xffffffff;
2263 static inline f64
vg_randf64_range( f64 min
, f64 max
)
2265 return vg_lerp( min
, max
, (f64
)vg_randf64() );
2268 static inline void vg_rand_dir( v3f dir
)
2270 dir
[0] = vg_randf64();
2271 dir
[1] = vg_randf64();
2272 dir
[2] = vg_randf64();
2274 v3_muls( dir
, 2.0f
, dir
);
2275 v3_sub( dir
, (v3f
){1.0f
,1.0f
,1.0f
}, dir
);
2277 v3_normalize( dir
);
2280 static inline void vg_rand_sphere( v3f co
)
2283 v3_muls( co
, cbrtf( vg_randf64() ), co
);