1 /* Copyright (C) 2021-2023 Harry Godden (hgn) - All Rights Reserved
19 * 5.d Raycast & Spherecasts
30 #include "vg_platform.h"
34 #define VG_PIf 3.14159265358979323846264338327950288f
35 #define VG_TAUf 6.28318530717958647692528676655900576f
38 * -----------------------------------------------------------------------------
39 * Section 0. Misc Operations
40 * -----------------------------------------------------------------------------
43 /* get the f32 as the raw bits in a u32 without converting */
44 static u32
vg_ftu32( f32 a
)
46 u32
*ptr
= (u32
*)(&a
);
50 /* check if f32 is infinite */
51 static int vg_isinff( f32 a
)
53 return ((vg_ftu32(a
)) & 0x7FFFFFFFU
) == 0x7F800000U
;
56 /* check if f32 is not a number */
57 static int vg_isnanf( f32 a
)
59 return !vg_isinff(a
) && ((vg_ftu32(a
)) & 0x7F800000U
) == 0x7F800000U
;
62 /* check if f32 is a number and is not infinite */
63 static int vg_validf( f32 a
)
65 return ((vg_ftu32(a
)) & 0x7F800000U
) != 0x7F800000U
;
68 static int v3_valid( v3f a
){
69 for( u32 i
=0; i
<3; i
++ )
70 if( !vg_validf(a
[i
]) ) return 0;
75 * -----------------------------------------------------------------------------
76 * Section 1. Scalar Operations
77 * -----------------------------------------------------------------------------
80 static inline f32
vg_minf( f32 a
, f32 b
){ return a
< b
? a
: b
; }
81 static inline f32
vg_maxf( f32 a
, f32 b
){ return a
> b
? a
: b
; }
83 static inline int vg_min( int a
, int b
){ return a
< b
? a
: b
; }
84 static inline int vg_max( int a
, int b
){ return a
> b
? a
: b
; }
86 static inline f32
vg_clampf( f32 a
, f32 min
, f32 max
)
88 return vg_minf( max
, vg_maxf( a
, min
) );
91 static inline f32
vg_signf( f32 a
)
93 return a
< 0.0f
? -1.0f
: 1.0f
;
96 static inline f32
vg_fractf( f32 a
)
98 return a
- floorf( a
);
101 static inline f64
vg_fractf64( f64 a
){
102 return a
- floor( a
);
105 static f32
vg_cfrictf( f32 velocity
, f32 F
)
107 return -vg_signf(velocity
) * vg_minf( F
, fabsf(velocity
) );
110 static inline f32
vg_rad( f32 deg
)
112 return deg
* VG_PIf
/ 180.0f
;
115 /* angle to reach b from a */
116 static f32
vg_angle_diff( f32 a
, f32 b
){
117 f32 d
= fmod(b
,VG_TAUf
)-fmodf(a
,VG_TAUf
);
118 if( fabsf(d
) > VG_PIf
)
119 d
= -vg_signf(d
) * (VG_TAUf
- fabsf(d
));
125 * quantize float to bit count
127 static u32
vg_quantf( f32 a
, u32 bits
, f32 min
, f32 max
){
128 u32 mask
= (0x1 << bits
) - 1;
129 return vg_clampf((a
- min
) * ((f32
)mask
/(max
-min
)), 0.0f
, mask
);
133 * un-quantize discreet to float
135 static f32
vg_dequantf( u32 q
, u32 bits
, f32 min
, f32 max
){
136 u32 mask
= (0x1 << bits
) - 1;
137 return min
+ (f32
)q
* ((max
-min
) / (f32
)mask
);
140 /* https://iquilezles.org/articles/functions/
142 * Use k to control the stretching of the function. Its maximum, which is 1,
143 * happens at exactly x = 1/k.
145 static f32
vg_exp_impulse( f32 x
, f32 k
){
147 return h
*expf(1.0f
-h
);
151 * -----------------------------------------------------------------------------
152 * Section 2.a 2D Vectors
153 * -----------------------------------------------------------------------------
156 static inline void v2_copy( v2f a
, v2f d
)
158 d
[0] = a
[0]; d
[1] = a
[1];
161 static inline void v2_zero( v2f a
)
163 a
[0] = 0.f
; a
[1] = 0.f
;
166 static inline void v2_add( v2f a
, v2f b
, v2f d
)
168 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
171 static inline void v2_sub( v2f a
, v2f b
, v2f d
)
173 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
176 static inline void v2_minv( v2f a
, v2f b
, v2f dest
)
178 dest
[0] = vg_minf(a
[0], b
[0]);
179 dest
[1] = vg_minf(a
[1], b
[1]);
182 static inline void v2_maxv( v2f a
, v2f b
, v2f dest
)
184 dest
[0] = vg_maxf(a
[0], b
[0]);
185 dest
[1] = vg_maxf(a
[1], b
[1]);
188 static inline f32
v2_dot( v2f a
, v2f b
)
190 return a
[0] * b
[0] + a
[1] * b
[1];
193 static inline f32
v2_cross( v2f a
, v2f b
)
195 return a
[0]*b
[1] - a
[1]*b
[0];
198 static inline void v2_abs( v2f a
, v2f d
)
200 d
[0] = fabsf( a
[0] );
201 d
[1] = fabsf( a
[1] );
204 static inline void v2_muls( v2f a
, f32 s
, v2f d
)
206 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
;
209 static inline void v2_divs( v2f a
, f32 s
, v2f d
)
211 d
[0] = a
[0]/s
; d
[1] = a
[1]/s
;
214 static inline void v2_mul( v2f a
, v2f b
, v2f d
)
220 static inline void v2_div( v2f a
, v2f b
, v2f d
)
222 d
[0] = a
[0]/b
[0]; d
[1] = a
[1]/b
[1];
225 static inline void v2_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
227 d
[0] = a
[0]+b
[0]*s
[0];
228 d
[1] = a
[1]+b
[1]*s
[1];
231 static inline void v2_muladds( v2f a
, v2f b
, f32 s
, v2f d
)
237 static inline f32
v2_length2( v2f a
)
239 return a
[0]*a
[0] + a
[1]*a
[1];
242 static inline f32
v2_length( v2f a
)
244 return sqrtf( v2_length2( a
) );
247 static inline f32
v2_dist2( v2f a
, v2f b
)
250 v2_sub( a
, b
, delta
);
251 return v2_length2( delta
);
254 static inline f32
v2_dist( v2f a
, v2f b
)
256 return sqrtf( v2_dist2( a
, b
) );
259 static inline void v2_lerp( v2f a
, v2f b
, f32 t
, v2f d
)
261 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
262 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
265 static inline void v2_normalize( v2f a
)
267 v2_muls( a
, 1.0f
/ v2_length( a
), a
);
270 static void v2_normalize_clamp( v2f a
)
272 f32 l2
= v2_length2( a
);
274 v2_muls( a
, 1.0f
/sqrtf(l2
), a
);
277 static inline void v2_floor( v2f a
, v2f b
)
279 b
[0] = floorf( a
[0] );
280 b
[1] = floorf( a
[1] );
283 static inline void v2_fill( v2f a
, f32 v
)
289 static inline void v2_copysign( v2f a
, v2f b
)
291 a
[0] = copysignf( a
[0], b
[0] );
292 a
[1] = copysignf( a
[1], b
[1] );
296 * ---------------- */
298 static inline void v2i_copy( v2i a
, v2i b
)
300 b
[0] = a
[0]; b
[1] = a
[1];
303 static inline int v2i_eq( v2i a
, v2i b
)
305 return ((a
[0] == b
[0]) && (a
[1] == b
[1]));
308 static inline void v2i_add( v2i a
, v2i b
, v2i d
)
310 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1];
313 static inline void v2i_sub( v2i a
, v2i b
, v2i d
)
315 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1];
319 * -----------------------------------------------------------------------------
320 * Section 2.b 3D Vectors
321 * -----------------------------------------------------------------------------
324 static inline void v3_copy( v3f a
, v3f b
)
326 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2];
329 static inline void v3_zero( v3f a
)
331 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
;
334 static inline void v3_add( v3f a
, v3f b
, v3f d
)
336 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
339 static inline void v3i_add( v3i a
, v3i b
, v3i d
)
341 d
[0] = a
[0]+b
[0]; d
[1] = a
[1]+b
[1]; d
[2] = a
[2]+b
[2];
344 static inline void v3_sub( v3f a
, v3f b
, v3f d
)
346 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
349 static inline void v3i_sub( v3i a
, v3i b
, v3i d
)
351 d
[0] = a
[0]-b
[0]; d
[1] = a
[1]-b
[1]; d
[2] = a
[2]-b
[2];
354 static inline void v3_mul( v3f a
, v3f b
, v3f d
)
356 d
[0] = a
[0]*b
[0]; d
[1] = a
[1]*b
[1]; d
[2] = a
[2]*b
[2];
359 static inline void v3_div( v3f a
, v3f b
, v3f d
)
361 d
[0] = b
[0]!=0.0f
? a
[0]/b
[0]: INFINITY
;
362 d
[1] = b
[1]!=0.0f
? a
[1]/b
[1]: INFINITY
;
363 d
[2] = b
[2]!=0.0f
? a
[2]/b
[2]: INFINITY
;
366 static inline void v3_muls( v3f a
, f32 s
, v3f d
)
368 d
[0] = a
[0]*s
; d
[1] = a
[1]*s
; d
[2] = a
[2]*s
;
371 static inline void v3_fill( v3f a
, f32 v
)
378 static inline void v3_divs( v3f a
, f32 s
, v3f d
)
381 v3_fill( d
, INFINITY
);
390 static inline void v3_muladds( v3f a
, v3f b
, f32 s
, v3f d
)
392 d
[0] = a
[0]+b
[0]*s
; d
[1] = a
[1]+b
[1]*s
; d
[2] = a
[2]+b
[2]*s
;
395 static inline void v3_muladd( v2f a
, v2f b
, v2f s
, v2f d
)
397 d
[0] = a
[0]+b
[0]*s
[0];
398 d
[1] = a
[1]+b
[1]*s
[1];
399 d
[2] = a
[2]+b
[2]*s
[2];
402 static inline f32
v3_dot( v3f a
, v3f b
)
404 return a
[0] * b
[0] + a
[1] * b
[1] + a
[2] * b
[2];
407 static inline void v3_cross( v3f a
, v3f b
, v3f dest
)
410 d
[0] = a
[1]*b
[2] - a
[2]*b
[1];
411 d
[1] = a
[2]*b
[0] - a
[0]*b
[2];
412 d
[2] = a
[0]*b
[1] - a
[1]*b
[0];
416 static inline f32
v3_length2( v3f a
)
418 return v3_dot( a
, a
);
421 static inline f32
v3_length( v3f a
)
423 return sqrtf( v3_length2( a
) );
426 static inline f32
v3_dist2( v3f a
, v3f b
)
429 v3_sub( a
, b
, delta
);
430 return v3_length2( delta
);
433 static inline f32
v3_dist( v3f a
, v3f b
)
435 return sqrtf( v3_dist2( a
, b
) );
438 static inline void v3_normalize( v3f a
)
440 v3_muls( a
, 1.f
/ v3_length( a
), a
);
443 static inline f32
vg_lerpf( f32 a
, f32 b
, f32 t
){
447 static inline f64
vg_lerp( f64 a
, f64 b
, f64 t
)
452 static inline void vg_slewf( f32
*a
, f32 b
, f32 speed
){
453 f32 d
= vg_signf( b
-*a
),
455 *a
= vg_minf( b
*d
, c
*d
) * d
;
458 static inline f32
vg_smoothstepf( f32 x
){
459 return x
*x
*(3.0f
- 2.0f
*x
);
463 /* correctly lerp around circular period -pi -> pi */
464 static f32
vg_alerpf( f32 a
, f32 b
, f32 t
)
466 f32 d
= fmodf( b
-a
, VG_TAUf
),
467 s
= fmodf( 2.0f
*d
, VG_TAUf
) - d
;
471 static inline void v3_lerp( v3f a
, v3f b
, f32 t
, v3f d
)
473 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
474 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
475 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
478 static inline void v3_minv( v3f a
, v3f b
, v3f dest
)
480 dest
[0] = vg_minf(a
[0], b
[0]);
481 dest
[1] = vg_minf(a
[1], b
[1]);
482 dest
[2] = vg_minf(a
[2], b
[2]);
485 static inline void v3_maxv( v3f a
, v3f b
, v3f dest
)
487 dest
[0] = vg_maxf(a
[0], b
[0]);
488 dest
[1] = vg_maxf(a
[1], b
[1]);
489 dest
[2] = vg_maxf(a
[2], b
[2]);
492 static inline f32
v3_minf( v3f a
)
494 return vg_minf( vg_minf( a
[0], a
[1] ), a
[2] );
497 static inline f32
v3_maxf( v3f a
)
499 return vg_maxf( vg_maxf( a
[0], a
[1] ), a
[2] );
502 static inline void v3_floor( v3f a
, v3f b
)
504 b
[0] = floorf( a
[0] );
505 b
[1] = floorf( a
[1] );
506 b
[2] = floorf( a
[2] );
509 static inline void v3_ceil( v3f a
, v3f b
)
511 b
[0] = ceilf( a
[0] );
512 b
[1] = ceilf( a
[1] );
513 b
[2] = ceilf( a
[2] );
516 static inline void v3_negate( v3f a
, v3f b
)
523 static inline void v3_rotate( v3f v
, f32 angle
, v3f axis
, v3f d
)
534 v3_cross( k
, v
, v2
);
535 v3_muls( v2
, s
, v2
);
536 v3_add( v1
, v2
, v1
);
537 v3_muls( k
, v3_dot(k
, v
) * (1.0f
- c
), v2
);
541 static void v3_tangent_basis( v3f n
, v3f tx
, v3f ty
){
542 /* Compute tangent basis (box2d) */
543 if( fabsf( n
[0] ) >= 0.57735027f
){
555 v3_cross( n
, tx
, ty
);
559 * Compute yaw and pitch based of a normalized vector representing forward
561 * result -> (YAW,PITCH,0.0)
563 static void v3_angles( v3f v
, v3f out_angles
){
564 float yaw
= atan2f( v
[0], -v
[2] ),
568 v
[0]*v
[0] + v
[2]*v
[2]
573 out_angles
[1] = pitch
;
574 out_angles
[2] = 0.0f
;
578 * Compute the forward vector from (YAW,PITCH,ROLL)
581 static void v3_angles_vector( v3f angles
, v3f out_v
){
582 out_v
[0] = sinf( angles
[0] ) * cosf( angles
[1] );
583 out_v
[1] = -sinf( angles
[1] );
584 out_v
[2] = -cosf( angles
[0] ) * cosf( angles
[1] );
588 * -----------------------------------------------------------------------------
589 * Section 2.c 4D Vectors
590 * -----------------------------------------------------------------------------
593 static inline void v4_copy( v4f a
, v4f b
)
595 b
[0] = a
[0]; b
[1] = a
[1]; b
[2] = a
[2]; b
[3] = a
[3];
598 static inline void v4_add( v4f a
, v4f b
, v4f d
)
606 static inline void v4_zero( v4f a
)
608 a
[0] = 0.f
; a
[1] = 0.f
; a
[2] = 0.f
; a
[3] = 0.f
;
611 static inline void v4_muls( v4f a
, f32 s
, v4f d
)
619 static inline void v4_muladds( v4f a
, v4f b
, f32 s
, v4f d
)
627 static inline void v4_lerp( v4f a
, v4f b
, f32 t
, v4f d
)
629 d
[0] = a
[0] + t
*(b
[0]-a
[0]);
630 d
[1] = a
[1] + t
*(b
[1]-a
[1]);
631 d
[2] = a
[2] + t
*(b
[2]-a
[2]);
632 d
[3] = a
[3] + t
*(b
[3]-a
[3]);
635 static inline f32
v4_dot( v4f a
, v4f b
)
637 return a
[0]*b
[0] + a
[1]*b
[1] + a
[2]*b
[2] + a
[3]*b
[3];
640 static inline f32
v4_length( v4f a
)
642 return sqrtf( v4_dot(a
,a
) );
646 * -----------------------------------------------------------------------------
647 * Section 3 Quaternions
648 * -----------------------------------------------------------------------------
651 static inline void q_identity( v4f q
)
653 q
[0] = 0.0f
; q
[1] = 0.0f
; q
[2] = 0.0f
; q
[3] = 1.0f
;
656 static inline void q_axis_angle( v4f q
, v3f axis
, f32 angle
)
668 static inline void q_mul( v4f q
, v4f q1
, v4f d
)
671 t
[0] = q
[3]*q1
[0] + q
[0]*q1
[3] + q
[1]*q1
[2] - q
[2]*q1
[1];
672 t
[1] = q
[3]*q1
[1] - q
[0]*q1
[2] + q
[1]*q1
[3] + q
[2]*q1
[0];
673 t
[2] = q
[3]*q1
[2] + q
[0]*q1
[1] - q
[1]*q1
[0] + q
[2]*q1
[3];
674 t
[3] = q
[3]*q1
[3] - q
[0]*q1
[0] - q
[1]*q1
[1] - q
[2]*q1
[2];
678 static inline void q_normalize( v4f q
)
680 f32 l2
= v4_dot(q
,q
);
681 if( l2
< 0.00001f
) q_identity( q
);
683 f32 s
= 1.0f
/sqrtf(l2
);
691 static inline void q_inv( v4f q
, v4f d
)
693 f32 s
= 1.0f
/ v4_dot(q
,q
);
700 static inline void q_nlerp( v4f a
, v4f b
, f32 t
, v4f d
){
701 if( v4_dot(a
,b
) < 0.0f
){
703 v4_muls( b
, -1.0f
, c
);
704 v4_lerp( a
, c
, t
, d
);
707 v4_lerp( a
, b
, t
, d
);
712 static inline void q_m3x3( v4f q
, m3x3f d
)
716 s
= l
> 0.0f
? 2.0f
/l
: 0.0f
,
718 xx
= s
*q
[0]*q
[0], xy
= s
*q
[0]*q
[1], wx
= s
*q
[3]*q
[0],
719 yy
= s
*q
[1]*q
[1], yz
= s
*q
[1]*q
[2], wy
= s
*q
[3]*q
[1],
720 zz
= s
*q
[2]*q
[2], xz
= s
*q
[0]*q
[2], wz
= s
*q
[3]*q
[2];
722 d
[0][0] = 1.0f
- yy
- zz
;
723 d
[1][1] = 1.0f
- xx
- zz
;
724 d
[2][2] = 1.0f
- xx
- yy
;
733 static void q_mulv( v4f q
, v3f v
, v3f d
)
737 v3_muls( q
, 2.0f
*v3_dot(q
,v
), v1
);
738 v3_muls( v
, q
[3]*q
[3] - v3_dot(q
,q
), v2
);
739 v3_add( v1
, v2
, v1
);
740 v3_cross( q
, v
, v2
);
741 v3_muls( v2
, 2.0f
*q
[3], v2
);
745 static f32
q_dist( v4f q0
, v4f q1
){
746 return acosf( 2.0f
* v4_dot(q0
,q1
) -1.0f
);
750 * -----------------------------------------------------------------------------
751 * Section 4.a 2x2 matrices
752 * -----------------------------------------------------------------------------
755 #define M2X2_INDENTIY {{1.0f, 0.0f, }, \
758 #define M2X2_ZERO {{0.0f, 0.0f, }, \
761 static inline void m2x2_copy( m2x2f a
, m2x2f b
)
763 v2_copy( a
[0], b
[0] );
764 v2_copy( a
[1], b
[1] );
767 static inline void m2x2_identity( m2x2f a
)
769 m2x2f id
= M2X2_INDENTIY
;
773 static inline void m2x2_create_rotation( m2x2f a
, f32 theta
)
786 static inline void m2x2_mulv( m2x2f m
, v2f v
, v2f d
)
790 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1];
791 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1];
797 * -----------------------------------------------------------------------------
798 * Section 4.b 3x3 matrices
799 * -----------------------------------------------------------------------------
802 #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
803 { 0.0f, 1.0f, 0.0f, },\
804 { 0.0f, 0.0f, 1.0f, }}
806 #define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\
807 { 0.0f, 0.0f, 0.0f, },\
808 { 0.0f, 0.0f, 0.0f, }}
811 static void euler_m3x3( v3f angles
, m3x3f d
)
813 f32 cosY
= cosf( angles
[0] ),
814 sinY
= sinf( angles
[0] ),
815 cosP
= cosf( angles
[1] ),
816 sinP
= sinf( angles
[1] ),
817 cosR
= cosf( angles
[2] ),
818 sinR
= sinf( angles
[2] );
820 d
[2][0] = -sinY
* cosP
;
822 d
[2][2] = cosY
* cosP
;
824 d
[0][0] = cosY
* cosR
;
826 d
[0][2] = sinY
* cosR
;
828 v3_cross( d
[0], d
[2], d
[1] );
831 static void m3x3_q( m3x3f m
, v4f q
)
835 diag
= m
[0][0] + m
[1][1] + m
[2][2];
838 r
= sqrtf( 1.0f
+ diag
);
840 q
[0] = rinv
* (m
[1][2] - m
[2][1]);
841 q
[1] = rinv
* (m
[2][0] - m
[0][2]);
842 q
[2] = rinv
* (m
[0][1] - m
[1][0]);
845 else if( m
[0][0] >= m
[1][1] && m
[0][0] >= m
[2][2] )
847 r
= sqrtf( 1.0f
- m
[1][1] - m
[2][2] + m
[0][0] );
850 q
[1] = rinv
* (m
[0][1] + m
[1][0]);
851 q
[2] = rinv
* (m
[0][2] + m
[2][0]);
852 q
[3] = rinv
* (m
[1][2] - m
[2][1]);
854 else if( m
[1][1] >= m
[2][2] )
856 r
= sqrtf( 1.0f
- m
[0][0] - m
[2][2] + m
[1][1] );
858 q
[0] = rinv
* (m
[0][1] + m
[1][0]);
860 q
[2] = rinv
* (m
[1][2] + m
[2][1]);
861 q
[3] = rinv
* (m
[2][0] - m
[0][2]);
865 r
= sqrtf( 1.0f
- m
[0][0] - m
[1][1] + m
[2][2] );
867 q
[0] = rinv
* (m
[0][2] + m
[2][0]);
868 q
[1] = rinv
* (m
[1][2] + m
[2][1]);
870 q
[3] = rinv
* (m
[0][1] - m
[1][0]);
874 /* a X b == [b]T a == ...*/
875 static void m3x3_skew_symetric( m3x3f a
, v3f v
)
888 /* aka kronecker product */
889 static void m3x3_outer_product( m3x3f out_m
, v3f a
, v3f b
)
891 out_m
[0][0] = a
[0]*b
[0];
892 out_m
[0][1] = a
[0]*b
[1];
893 out_m
[0][2] = a
[0]*b
[2];
894 out_m
[1][0] = a
[1]*b
[0];
895 out_m
[1][1] = a
[1]*b
[1];
896 out_m
[1][2] = a
[1]*b
[2];
897 out_m
[2][0] = a
[2]*b
[0];
898 out_m
[2][1] = a
[2]*b
[1];
899 out_m
[2][2] = a
[2]*b
[2];
902 static void m3x3_add( m3x3f a
, m3x3f b
, m3x3f d
)
904 v3_add( a
[0], b
[0], d
[0] );
905 v3_add( a
[1], b
[1], d
[1] );
906 v3_add( a
[2], b
[2], d
[2] );
909 static void m3x3_sub( m3x3f a
, m3x3f b
, m3x3f d
)
911 v3_sub( a
[0], b
[0], d
[0] );
912 v3_sub( a
[1], b
[1], d
[1] );
913 v3_sub( a
[2], b
[2], d
[2] );
916 static inline void m3x3_copy( m3x3f a
, m3x3f b
)
918 v3_copy( a
[0], b
[0] );
919 v3_copy( a
[1], b
[1] );
920 v3_copy( a
[2], b
[2] );
923 static inline void m3x3_identity( m3x3f a
)
925 m3x3f id
= M3X3_IDENTITY
;
929 static void m3x3_diagonal( m3x3f out_a
, f32 v
)
931 m3x3_identity( out_a
);
937 static void m3x3_setdiagonalv3( m3x3f a
, v3f v
)
944 static inline void m3x3_zero( m3x3f a
)
950 static inline void m3x3_inv( m3x3f src
, m3x3f dest
)
952 f32 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
953 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
954 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
961 dest
[0][0] = (e
*i
-h
*f
)*det
;
962 dest
[0][1] = -(b
*i
-c
*h
)*det
;
963 dest
[0][2] = (b
*f
-c
*e
)*det
;
964 dest
[1][0] = -(d
*i
-f
*g
)*det
;
965 dest
[1][1] = (a
*i
-c
*g
)*det
;
966 dest
[1][2] = -(a
*f
-d
*c
)*det
;
967 dest
[2][0] = (d
*h
-g
*e
)*det
;
968 dest
[2][1] = -(a
*h
-g
*b
)*det
;
969 dest
[2][2] = (a
*e
-d
*b
)*det
;
972 static f32
m3x3_det( m3x3f m
)
974 return m
[0][0] * (m
[1][1] * m
[2][2] - m
[2][1] * m
[1][2])
975 - m
[0][1] * (m
[1][0] * m
[2][2] - m
[1][2] * m
[2][0])
976 + m
[0][2] * (m
[1][0] * m
[2][1] - m
[1][1] * m
[2][0]);
979 static inline void m3x3_transpose( m3x3f src
, m3x3f dest
)
981 f32 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
982 d
= src
[1][0], e
= src
[1][1], f
= src
[1][2],
983 g
= src
[2][0], h
= src
[2][1], i
= src
[2][2];
996 static inline void m3x3_mul( m3x3f a
, m3x3f b
, m3x3f d
)
998 f32 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
999 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
1000 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
1002 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
1003 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
1004 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2];
1006 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
1007 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
1008 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
1009 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
1010 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
1011 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
1012 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
1013 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
1014 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
1017 static inline void m3x3_mulv( m3x3f m
, v3f v
, v3f d
)
1021 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2];
1022 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2];
1023 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2];
1028 static inline void m3x3_projection( m3x3f dst
,
1029 f32
const left
, f32
const right
, f32
const bottom
, f32
const top
)
1035 rl
= 1.0f
/ (right
- left
);
1036 tb
= 1.0f
/ (top
- bottom
);
1038 dst
[0][0] = 2.0f
* rl
;
1039 dst
[1][1] = 2.0f
* tb
;
1043 static inline void m3x3_translate( m3x3f m
, v3f v
)
1045 m
[2][0] = m
[0][0] * v
[0] + m
[1][0] * v
[1] + m
[2][0];
1046 m
[2][1] = m
[0][1] * v
[0] + m
[1][1] * v
[1] + m
[2][1];
1047 m
[2][2] = m
[0][2] * v
[0] + m
[1][2] * v
[1] + m
[2][2];
1050 static inline void m3x3_scale( m3x3f m
, v3f v
)
1052 v3_muls( m
[0], v
[0], m
[0] );
1053 v3_muls( m
[1], v
[1], m
[1] );
1054 v3_muls( m
[2], v
[2], m
[2] );
1057 static inline void m3x3_scalef( m3x3f m
, f32 f
)
1064 static inline void m3x3_rotate( m3x3f m
, f32 angle
)
1066 f32 m00
= m
[0][0], m10
= m
[1][0],
1067 m01
= m
[0][1], m11
= m
[1][1],
1068 m02
= m
[0][2], m12
= m
[1][2];
1074 m
[0][0] = m00
* c
+ m10
* s
;
1075 m
[0][1] = m01
* c
+ m11
* s
;
1076 m
[0][2] = m02
* c
+ m12
* s
;
1078 m
[1][0] = m00
* -s
+ m10
* c
;
1079 m
[1][1] = m01
* -s
+ m11
* c
;
1080 m
[1][2] = m02
* -s
+ m12
* c
;
1084 * -----------------------------------------------------------------------------
1085 * Section 4.c 4x3 matrices
1086 * -----------------------------------------------------------------------------
1089 #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
1090 { 0.0f, 1.0f, 0.0f, },\
1091 { 0.0f, 0.0f, 1.0f, },\
1092 { 0.0f, 0.0f, 0.0f }}
1094 static inline void m4x3_to_3x3( m4x3f a
, m3x3f b
)
1096 v3_copy( a
[0], b
[0] );
1097 v3_copy( a
[1], b
[1] );
1098 v3_copy( a
[2], b
[2] );
1101 static inline void m4x3_invert_affine( m4x3f a
, m4x3f b
)
1103 m3x3_transpose( a
, b
);
1104 m3x3_mulv( b
, a
[3], b
[3] );
1105 v3_negate( b
[3], b
[3] );
1108 static void m4x3_invert_full( m4x3f src
, m4x3f dst
)
1112 a
= src
[0][0], b
= src
[0][1], c
= src
[0][2],
1113 e
= src
[1][0], f
= src
[1][1], g
= src
[1][2],
1114 i
= src
[2][0], j
= src
[2][1], k
= src
[2][2],
1115 m
= src
[3][0], n
= src
[3][1], o
= src
[3][2];
1121 dst
[0][0] = f
*k
- g
*j
;
1122 dst
[1][0] =-(e
*k
- g
*i
);
1123 dst
[2][0] = e
*j
- f
*i
;
1124 dst
[3][0] =-(e
*t2
- f
*t4
+ g
*t5
);
1126 dst
[0][1] =-(b
*k
- c
*j
);
1127 dst
[1][1] = a
*k
- c
*i
;
1128 dst
[2][1] =-(a
*j
- b
*i
);
1129 dst
[3][1] = a
*t2
- b
*t4
+ c
*t5
;
1135 dst
[0][2] = b
*g
- c
*f
;
1136 dst
[1][2] =-(a
*g
- c
*e
);
1137 dst
[2][2] = a
*f
- b
*e
;
1138 dst
[3][2] =-(a
*t2
- b
*t4
+ c
* t5
);
1140 det
= 1.0f
/ (a
* dst
[0][0] + b
* dst
[1][0] + c
* dst
[2][0]);
1141 v3_muls( dst
[0], det
, dst
[0] );
1142 v3_muls( dst
[1], det
, dst
[1] );
1143 v3_muls( dst
[2], det
, dst
[2] );
1144 v3_muls( dst
[3], det
, dst
[3] );
1147 static inline void m4x3_copy( m4x3f a
, m4x3f b
)
1149 v3_copy( a
[0], b
[0] );
1150 v3_copy( a
[1], b
[1] );
1151 v3_copy( a
[2], b
[2] );
1152 v3_copy( a
[3], b
[3] );
1155 static inline void m4x3_identity( m4x3f a
)
1157 m4x3f id
= M4X3_IDENTITY
;
1161 static void m4x3_mul( m4x3f a
, m4x3f b
, m4x3f d
)
1164 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2],
1165 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2],
1166 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2],
1167 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2],
1168 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2],
1169 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2],
1170 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2],
1171 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2];
1173 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
;
1174 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
;
1175 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
;
1176 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
;
1177 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
;
1178 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
;
1179 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
;
1180 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
;
1181 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
;
1182 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
;
1183 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
;
1184 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
;
1187 #if 0 /* shat appf mingw wstringop-overflow */
1190 static void m4x3_mulv( m4x3f m
, v3f v
, v3f d
)
1194 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0];
1195 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1];
1196 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2];
1202 * Transform plane ( xyz, distance )
1204 static void m4x3_mulp( m4x3f m
, v4f p
, v4f d
)
1208 v3_muls( p
, p
[3], o
);
1209 m4x3_mulv( m
, o
, o
);
1210 m3x3_mulv( m
, p
, d
);
1212 d
[3] = v3_dot( o
, d
);
1219 static void m4x3_translate( m4x3f m
, v3f v
)
1221 v3_muladds( m
[3], m
[0], v
[0], m
[3] );
1222 v3_muladds( m
[3], m
[1], v
[1], m
[3] );
1223 v3_muladds( m
[3], m
[2], v
[2], m
[3] );
1226 static void m4x3_rotate_x( m4x3f m
, f32 angle
)
1228 m4x3f t
= M4X3_IDENTITY
;
1239 m4x3_mul( m
, t
, m
);
1242 static void m4x3_rotate_y( m4x3f m
, f32 angle
)
1244 m4x3f t
= M4X3_IDENTITY
;
1255 m4x3_mul( m
, t
, m
);
1258 static void m4x3_rotate_z( m4x3f m
, f32 angle
)
1260 m4x3f t
= M4X3_IDENTITY
;
1271 m4x3_mul( m
, t
, m
);
1274 static void m4x3_expand( m4x3f m
, m4x4f d
)
1276 v3_copy( m
[0], d
[0] );
1277 v3_copy( m
[1], d
[1] );
1278 v3_copy( m
[2], d
[2] );
1279 v3_copy( m
[3], d
[3] );
1286 static void m4x3_decompose( m4x3f m
, v3f co
, v4f q
, v3f s
)
1288 v3_copy( m
[3], co
);
1289 s
[0] = v3_length(m
[0]);
1290 s
[1] = v3_length(m
[1]);
1291 s
[2] = v3_length(m
[2]);
1294 v3_divs( m
[0], s
[0], rot
[0] );
1295 v3_divs( m
[1], s
[1], rot
[1] );
1296 v3_divs( m
[2], s
[2], rot
[2] );
1301 static void m4x3_expand_aabb_point( m4x3f m
, boxf box
, v3f point
){
1303 m4x3_mulv( m
, point
, v
);
1305 v3_minv( box
[0], v
, box
[0] );
1306 v3_maxv( box
[1], v
, box
[1] );
1309 static void m4x3_expand_aabb_aabb( m4x3f m
, boxf boxa
, boxf boxb
){
1311 v3_copy( boxb
[0], a
);
1312 v3_copy( boxb
[1], b
);
1313 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ a
[0], a
[1], a
[2] } );
1314 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ a
[0], b
[1], a
[2] } );
1315 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ b
[0], b
[1], a
[2] } );
1316 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ b
[0], a
[1], a
[2] } );
1317 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ a
[0], a
[1], b
[2] } );
1318 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ a
[0], b
[1], b
[2] } );
1319 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ b
[0], b
[1], b
[2] } );
1320 m4x3_expand_aabb_point( m
, boxa
, (v3f
){ b
[0], a
[1], b
[2] } );
1322 static inline void m4x3_lookat( m4x3f m
, v3f pos
, v3f target
, v3f up
)
1325 v3_sub( target
, pos
, dir
);
1326 v3_normalize( dir
);
1328 v3_copy( dir
, m
[2] );
1330 v3_cross( up
, m
[2], m
[0] );
1331 v3_normalize( m
[0] );
1333 v3_cross( m
[2], m
[0], m
[1] );
1334 v3_copy( pos
, m
[3] );
1338 * -----------------------------------------------------------------------------
1339 * Section 4.d 4x4 matrices
1340 * -----------------------------------------------------------------------------
1343 #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\
1344 { 0.0f, 1.0f, 0.0f, 0.0f },\
1345 { 0.0f, 0.0f, 1.0f, 0.0f },\
1346 { 0.0f, 0.0f, 0.0f, 1.0f }}
1347 #define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\
1348 { 0.0f, 0.0f, 0.0f, 0.0f },\
1349 { 0.0f, 0.0f, 0.0f, 0.0f },\
1350 { 0.0f, 0.0f, 0.0f, 0.0f }}
1352 static void m4x4_projection( m4x4f m
, f32 angle
,
1353 f32 ratio
, f32 fnear
, f32 ffar
)
1355 f32 scale
= tanf( angle
* 0.5f
* VG_PIf
/ 180.0f
) * fnear
,
1361 m
[0][0] = 2.0f
* fnear
/ (r
- l
);
1367 m
[1][1] = 2.0f
* fnear
/ (t
- b
);
1371 m
[2][0] = (r
+ l
) / (r
- l
);
1372 m
[2][1] = (t
+ b
) / (t
- b
);
1373 m
[2][2] = -(ffar
+ fnear
) / (ffar
- fnear
);
1378 m
[3][2] = -2.0f
* ffar
* fnear
/ (ffar
- fnear
);
1382 static void m4x4_translate( m4x4f m
, v3f v
)
1384 v4_muladds( m
[3], m
[0], v
[0], m
[3] );
1385 v4_muladds( m
[3], m
[1], v
[1], m
[3] );
1386 v4_muladds( m
[3], m
[2], v
[2], m
[3] );
1389 static inline void m4x4_copy( m4x4f a
, m4x4f b
)
1391 v4_copy( a
[0], b
[0] );
1392 v4_copy( a
[1], b
[1] );
1393 v4_copy( a
[2], b
[2] );
1394 v4_copy( a
[3], b
[3] );
1397 static inline void m4x4_identity( m4x4f a
)
1399 m4x4f id
= M4X4_IDENTITY
;
1403 static inline void m4x4_zero( m4x4f a
)
1405 m4x4f zero
= M4X4_ZERO
;
1406 m4x4_copy( zero
, a
);
1409 static inline void m4x4_mul( m4x4f a
, m4x4f b
, m4x4f d
)
1411 f32 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
1412 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
1413 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
1414 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
1416 b00
= b
[0][0], b01
= b
[0][1], b02
= b
[0][2], b03
= b
[0][3],
1417 b10
= b
[1][0], b11
= b
[1][1], b12
= b
[1][2], b13
= b
[1][3],
1418 b20
= b
[2][0], b21
= b
[2][1], b22
= b
[2][2], b23
= b
[2][3],
1419 b30
= b
[3][0], b31
= b
[3][1], b32
= b
[3][2], b33
= b
[3][3];
1421 d
[0][0] = a00
*b00
+ a10
*b01
+ a20
*b02
+ a30
*b03
;
1422 d
[0][1] = a01
*b00
+ a11
*b01
+ a21
*b02
+ a31
*b03
;
1423 d
[0][2] = a02
*b00
+ a12
*b01
+ a22
*b02
+ a32
*b03
;
1424 d
[0][3] = a03
*b00
+ a13
*b01
+ a23
*b02
+ a33
*b03
;
1425 d
[1][0] = a00
*b10
+ a10
*b11
+ a20
*b12
+ a30
*b13
;
1426 d
[1][1] = a01
*b10
+ a11
*b11
+ a21
*b12
+ a31
*b13
;
1427 d
[1][2] = a02
*b10
+ a12
*b11
+ a22
*b12
+ a32
*b13
;
1428 d
[1][3] = a03
*b10
+ a13
*b11
+ a23
*b12
+ a33
*b13
;
1429 d
[2][0] = a00
*b20
+ a10
*b21
+ a20
*b22
+ a30
*b23
;
1430 d
[2][1] = a01
*b20
+ a11
*b21
+ a21
*b22
+ a31
*b23
;
1431 d
[2][2] = a02
*b20
+ a12
*b21
+ a22
*b22
+ a32
*b23
;
1432 d
[2][3] = a03
*b20
+ a13
*b21
+ a23
*b22
+ a33
*b23
;
1433 d
[3][0] = a00
*b30
+ a10
*b31
+ a20
*b32
+ a30
*b33
;
1434 d
[3][1] = a01
*b30
+ a11
*b31
+ a21
*b32
+ a31
*b33
;
1435 d
[3][2] = a02
*b30
+ a12
*b31
+ a22
*b32
+ a32
*b33
;
1436 d
[3][3] = a03
*b30
+ a13
*b31
+ a23
*b32
+ a33
*b33
;
1439 static inline void m4x4_mulv( m4x4f m
, v4f v
, v4f d
)
1443 res
[0] = m
[0][0]*v
[0] + m
[1][0]*v
[1] + m
[2][0]*v
[2] + m
[3][0]*v
[3];
1444 res
[1] = m
[0][1]*v
[0] + m
[1][1]*v
[1] + m
[2][1]*v
[2] + m
[3][1]*v
[3];
1445 res
[2] = m
[0][2]*v
[0] + m
[1][2]*v
[1] + m
[2][2]*v
[2] + m
[3][2]*v
[3];
1446 res
[3] = m
[0][3]*v
[0] + m
[1][3]*v
[1] + m
[2][3]*v
[2] + m
[3][3]*v
[3];
1451 static inline void m4x4_inv( m4x4f a
, m4x4f d
)
1453 f32 a00
= a
[0][0], a01
= a
[0][1], a02
= a
[0][2], a03
= a
[0][3],
1454 a10
= a
[1][0], a11
= a
[1][1], a12
= a
[1][2], a13
= a
[1][3],
1455 a20
= a
[2][0], a21
= a
[2][1], a22
= a
[2][2], a23
= a
[2][3],
1456 a30
= a
[3][0], a31
= a
[3][1], a32
= a
[3][2], a33
= a
[3][3],
1460 t
[0] = a22
*a33
- a32
*a23
;
1461 t
[1] = a21
*a33
- a31
*a23
;
1462 t
[2] = a21
*a32
- a31
*a22
;
1463 t
[3] = a20
*a33
- a30
*a23
;
1464 t
[4] = a20
*a32
- a30
*a22
;
1465 t
[5] = a20
*a31
- a30
*a21
;
1467 d
[0][0] = a11
*t
[0] - a12
*t
[1] + a13
*t
[2];
1468 d
[1][0] =-(a10
*t
[0] - a12
*t
[3] + a13
*t
[4]);
1469 d
[2][0] = a10
*t
[1] - a11
*t
[3] + a13
*t
[5];
1470 d
[3][0] =-(a10
*t
[2] - a11
*t
[4] + a12
*t
[5]);
1472 d
[0][1] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
1473 d
[1][1] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
1474 d
[2][1] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
1475 d
[3][1] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
1477 t
[0] = a12
*a33
- a32
*a13
;
1478 t
[1] = a11
*a33
- a31
*a13
;
1479 t
[2] = a11
*a32
- a31
*a12
;
1480 t
[3] = a10
*a33
- a30
*a13
;
1481 t
[4] = a10
*a32
- a30
*a12
;
1482 t
[5] = a10
*a31
- a30
*a11
;
1484 d
[0][2] = a01
*t
[0] - a02
*t
[1] + a03
*t
[2];
1485 d
[1][2] =-(a00
*t
[0] - a02
*t
[3] + a03
*t
[4]);
1486 d
[2][2] = a00
*t
[1] - a01
*t
[3] + a03
*t
[5];
1487 d
[3][2] =-(a00
*t
[2] - a01
*t
[4] + a02
*t
[5]);
1489 t
[0] = a12
*a23
- a22
*a13
;
1490 t
[1] = a11
*a23
- a21
*a13
;
1491 t
[2] = a11
*a22
- a21
*a12
;
1492 t
[3] = a10
*a23
- a20
*a13
;
1493 t
[4] = a10
*a22
- a20
*a12
;
1494 t
[5] = a10
*a21
- a20
*a11
;
1496 d
[0][3] =-(a01
*t
[0] - a02
*t
[1] + a03
*t
[2]);
1497 d
[1][3] = a00
*t
[0] - a02
*t
[3] + a03
*t
[4];
1498 d
[2][3] =-(a00
*t
[1] - a01
*t
[3] + a03
*t
[5]);
1499 d
[3][3] = a00
*t
[2] - a01
*t
[4] + a02
*t
[5];
1501 det
= 1.0f
/ (a00
*d
[0][0] + a01
*d
[1][0] + a02
*d
[2][0] + a03
*d
[3][0]);
1502 v4_muls( d
[0], det
, d
[0] );
1503 v4_muls( d
[1], det
, d
[1] );
1504 v4_muls( d
[2], det
, d
[2] );
1505 v4_muls( d
[3], det
, d
[3] );
1509 * -----------------------------------------------------------------------------
1511 * -----------------------------------------------------------------------------
1514 static inline void box_addpt( boxf a
, v3f pt
)
1516 v3_minv( a
[0], pt
, a
[0] );
1517 v3_maxv( a
[1], pt
, a
[1] );
1520 static inline void box_concat( boxf a
, boxf b
)
1522 v3_minv( a
[0], b
[0], a
[0] );
1523 v3_maxv( a
[1], b
[1], a
[1] );
1526 static inline void box_copy( boxf a
, boxf b
)
1528 v3_copy( a
[0], b
[0] );
1529 v3_copy( a
[1], b
[1] );
1532 static inline int box_overlap( boxf a
, boxf b
)
1535 ( a
[0][0] <= b
[1][0] && a
[1][0] >= b
[0][0] ) &&
1536 ( a
[0][1] <= b
[1][1] && a
[1][1] >= b
[0][1] ) &&
1537 ( a
[0][2] <= b
[1][2] && a
[1][2] >= b
[0][2] )
1541 static int box_within( boxf greater
, boxf lesser
)
1544 v3_sub( lesser
[0], greater
[0], a
);
1545 v3_sub( lesser
[1], greater
[1], b
);
1547 if( (a
[0] >= 0.0f
) && (a
[1] >= 0.0f
) && (a
[2] >= 0.0f
) &&
1548 (b
[0] <= 0.0f
) && (b
[1] <= 0.0f
) && (b
[2] <= 0.0f
) )
1556 static inline void box_init_inf( boxf box
){
1557 v3_fill( box
[0], INFINITY
);
1558 v3_fill( box
[1], -INFINITY
);
1562 * -----------------------------------------------------------------------------
1563 * Section 5.b Planes
1564 * -----------------------------------------------------------------------------
1567 static inline void tri_to_plane( f64 a
[3], f64 b
[3],
1568 f64 c
[3], f64 p
[4] )
1574 edge0
[0] = b
[0] - a
[0];
1575 edge0
[1] = b
[1] - a
[1];
1576 edge0
[2] = b
[2] - a
[2];
1578 edge1
[0] = c
[0] - a
[0];
1579 edge1
[1] = c
[1] - a
[1];
1580 edge1
[2] = c
[2] - a
[2];
1582 p
[0] = edge0
[1] * edge1
[2] - edge0
[2] * edge1
[1];
1583 p
[1] = edge0
[2] * edge1
[0] - edge0
[0] * edge1
[2];
1584 p
[2] = edge0
[0] * edge1
[1] - edge0
[1] * edge1
[0];
1586 l
= sqrt(p
[0] * p
[0] + p
[1] * p
[1] + p
[2] * p
[2]);
1587 p
[3] = (p
[0] * a
[0] + p
[1] * a
[1] + p
[2] * a
[2]) / l
;
1594 static int plane_intersect3( v4f a
, v4f b
, v4f c
, v3f p
)
1596 f32
const epsilon
= 1e-6f
;
1599 v3_cross( a
, b
, x
);
1600 f32 d
= v3_dot( x
, c
);
1602 if( (d
< epsilon
) && (d
> -epsilon
) ) return 0;
1605 v3_cross( b
, c
, v0
);
1606 v3_cross( c
, a
, v1
);
1607 v3_cross( a
, b
, v2
);
1609 v3_muls( v0
, a
[3], p
);
1610 v3_muladds( p
, v1
, b
[3], p
);
1611 v3_muladds( p
, v2
, c
[3], p
);
1617 int plane_intersect2( v4f a
, v4f b
, v3f p
, v3f n
)
1619 f32
const epsilon
= 1e-6f
;
1622 v3_cross( a
, b
, c
);
1623 f32 d
= v3_length2( c
);
1625 if( (d
< epsilon
) && (d
> -epsilon
) )
1629 v3_cross( c
, b
, v0
);
1630 v3_cross( a
, c
, v1
);
1632 v3_muls( v0
, a
[3], vx
);
1633 v3_muladds( vx
, v1
, b
[3], vx
);
1634 v3_divs( vx
, d
, p
);
1640 static int plane_segment( v4f plane
, v3f a
, v3f b
, v3f co
)
1642 f32 d0
= v3_dot( a
, plane
) - plane
[3],
1643 d1
= v3_dot( b
, plane
) - plane
[3];
1647 f32 tot
= 1.0f
/( fabsf(d0
)+fabsf(d1
) );
1649 v3_muls( a
, fabsf(d1
) * tot
, co
);
1650 v3_muladds( co
, b
, fabsf(d0
) * tot
, co
);
1657 static inline f64
plane_polarity( f64 p
[4], f64 a
[3] )
1660 (a
[0] * p
[0] + a
[1] * p
[1] + a
[2] * p
[2])
1661 -(p
[0]*p
[3] * p
[0] + p
[1]*p
[3] * p
[1] + p
[2]*p
[3] * p
[2])
1665 static f32
ray_plane( v4f plane
, v3f co
, v3f dir
){
1666 f32 d
= v3_dot( plane
, dir
);
1667 if( fabsf(d
) > 1e-6f
){
1669 v3_muls( plane
, plane
[3], v0
);
1670 v3_sub( v0
, co
, v0
);
1671 return v3_dot( v0
, plane
) / d
;
1673 else return INFINITY
;
1677 * -----------------------------------------------------------------------------
1678 * Section 5.c Closest point functions
1679 * -----------------------------------------------------------------------------
1683 * These closest point tests were learned from Real-Time Collision Detection by
1686 static f32
closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
1687 f32
*s
, f32
*t
, v3f c1
, v3f c2
)
1690 v3_sub( q1
, p1
, d1
);
1691 v3_sub( q2
, p2
, d2
);
1692 v3_sub( p1
, p2
, r
);
1694 f32 a
= v3_length2( d1
),
1695 e
= v3_length2( d2
),
1696 f
= v3_dot( d2
, r
);
1698 const f32 kEpsilon
= 0.0001f
;
1700 if( a
<= kEpsilon
&& e
<= kEpsilon
)
1708 v3_sub( c1
, c2
, v0
);
1710 return v3_length2( v0
);
1716 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
1720 f32 c
= v3_dot( d1
, r
);
1724 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
1728 f32 b
= v3_dot(d1
,d2
),
1733 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
1740 *t
= (b
*(*s
)+f
) / e
;
1745 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
1747 else if( *t
> 1.0f
)
1750 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
1755 v3_muladds( p1
, d1
, *s
, c1
);
1756 v3_muladds( p2
, d2
, *t
, c2
);
1759 v3_sub( c1
, c2
, v0
);
1760 return v3_length2( v0
);
1763 static int point_inside_aabb( boxf box
, v3f point
)
1765 if((point
[0]<=box
[1][0]) && (point
[1]<=box
[1][1]) && (point
[2]<=box
[1][2]) &&
1766 (point
[0]>=box
[0][0]) && (point
[1]>=box
[0][1]) && (point
[2]>=box
[0][2]) )
1772 static void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
1774 v3_maxv( p
, box
[0], dest
);
1775 v3_minv( dest
, box
[1], dest
);
1778 static void closest_point_obb( v3f p
, boxf box
,
1779 m4x3f mtx
, m4x3f inv_mtx
, v3f dest
)
1782 m4x3_mulv( inv_mtx
, p
, local
);
1783 closest_point_aabb( local
, box
, local
);
1784 m4x3_mulv( mtx
, local
, dest
);
1787 static f32
closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
1791 v3_sub( point
, a
, v1
);
1793 f32 t
= v3_dot( v1
, v0
) / v3_length2(v0
);
1794 t
= vg_clampf(t
,0.0f
,1.0f
);
1795 v3_muladds( a
, v0
, t
, dest
);
1799 static void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
1804 /* Region outside A */
1805 v3_sub( tri
[1], tri
[0], ab
);
1806 v3_sub( tri
[2], tri
[0], ac
);
1807 v3_sub( p
, tri
[0], ap
);
1811 if( d1
<= 0.0f
&& d2
<= 0.0f
)
1813 v3_copy( tri
[0], dest
);
1814 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1818 /* Region outside B */
1822 v3_sub( p
, tri
[1], bp
);
1823 d3
= v3_dot( ab
, bp
);
1824 d4
= v3_dot( ac
, bp
);
1826 if( d3
>= 0.0f
&& d4
<= d3
)
1828 v3_copy( tri
[1], dest
);
1829 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1833 /* Edge region of AB */
1834 f32 vc
= d1
*d4
- d3
*d2
;
1835 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
1837 f32 v
= d1
/ (d1
-d3
);
1838 v3_muladds( tri
[0], ab
, v
, dest
);
1839 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1843 /* Region outside C */
1846 v3_sub( p
, tri
[2], cp
);
1847 d5
= v3_dot(ab
, cp
);
1848 d6
= v3_dot(ac
, cp
);
1850 if( d6
>= 0.0f
&& d5
<= d6
)
1852 v3_copy( tri
[2], dest
);
1853 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1858 f32 vb
= d5
*d2
- d1
*d6
;
1859 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
1861 f32 w
= d2
/ (d2
-d6
);
1862 v3_muladds( tri
[0], ac
, w
, dest
);
1863 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1868 f32 va
= d3
*d6
- d5
*d4
;
1869 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
1871 f32 w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
1873 v3_sub( tri
[2], tri
[1], bc
);
1874 v3_muladds( tri
[1], bc
, w
, dest
);
1875 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
1879 /* P inside region, Q via barycentric coordinates uvw */
1880 f32 d
= 1.0f
/(va
+vb
+vc
),
1884 v3_muladds( tri
[0], ab
, v
, dest
);
1885 v3_muladds( dest
, ac
, w
, dest
);
1890 k_contact_type_default
,
1891 k_contact_type_disabled
,
1895 static enum contact_type
closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
1900 /* Region outside A */
1901 v3_sub( tri
[1], tri
[0], ab
);
1902 v3_sub( tri
[2], tri
[0], ac
);
1903 v3_sub( p
, tri
[0], ap
);
1907 if( d1
<= 0.0f
&& d2
<= 0.0f
)
1909 v3_copy( tri
[0], dest
);
1910 return k_contact_type_default
;
1913 /* Region outside B */
1917 v3_sub( p
, tri
[1], bp
);
1918 d3
= v3_dot( ab
, bp
);
1919 d4
= v3_dot( ac
, bp
);
1921 if( d3
>= 0.0f
&& d4
<= d3
)
1923 v3_copy( tri
[1], dest
);
1924 return k_contact_type_edge
;
1927 /* Edge region of AB */
1928 f32 vc
= d1
*d4
- d3
*d2
;
1929 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
1931 f32 v
= d1
/ (d1
-d3
);
1932 v3_muladds( tri
[0], ab
, v
, dest
);
1933 return k_contact_type_edge
;
1936 /* Region outside C */
1939 v3_sub( p
, tri
[2], cp
);
1940 d5
= v3_dot(ab
, cp
);
1941 d6
= v3_dot(ac
, cp
);
1943 if( d6
>= 0.0f
&& d5
<= d6
)
1945 v3_copy( tri
[2], dest
);
1946 return k_contact_type_edge
;
1950 f32 vb
= d5
*d2
- d1
*d6
;
1951 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
1953 f32 w
= d2
/ (d2
-d6
);
1954 v3_muladds( tri
[0], ac
, w
, dest
);
1955 return k_contact_type_edge
;
1959 f32 va
= d3
*d6
- d5
*d4
;
1960 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
1962 f32 w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
1964 v3_sub( tri
[2], tri
[1], bc
);
1965 v3_muladds( tri
[1], bc
, w
, dest
);
1966 return k_contact_type_edge
;
1969 /* P inside region, Q via barycentric coordinates uvw */
1970 f32 d
= 1.0f
/(va
+vb
+vc
),
1974 v3_muladds( tri
[0], ab
, v
, dest
);
1975 v3_muladds( dest
, ac
, w
, dest
);
1977 return k_contact_type_default
;
1980 static void closest_point_elipse( v2f p
, v2f e
, v2f o
)
1982 v2f pabs
, ei
, e2
, ve
, t
;
1985 v2_div( (v2f
){ 1.0f
, 1.0f
}, e
, ei
);
1987 v2_mul( ei
, (v2f
){ e2
[0]-e2
[1], e2
[1]-e2
[0] }, ve
);
1989 v2_fill( t
, 0.70710678118654752f
);
1991 for( int i
=0; i
<3; i
++ ){
1994 v2_mul( ve
, t
, v
); /* ve*t*t*t */
1998 v2_sub( pabs
, v
, u
);
2002 v2_sub( ud
, v
, ud
);
2004 v2_muls( u
, v2_length( ud
), u
);
2009 v2_maxv( (v2f
){0.0f
,0.0f
}, w
, t
);
2014 v2_copysign( o
, p
);
2018 * -----------------------------------------------------------------------------
2019 * Section 5.d Raycasts & Spherecasts
2020 * -----------------------------------------------------------------------------
2023 int ray_aabb1( boxf box
, v3f co
, v3f dir_inv
, f32 dist
)
2028 v3_sub( box
[0], co
, v0
);
2029 v3_sub( box
[1], co
, v1
);
2031 v3_mul( v0
, dir_inv
, v0
);
2032 v3_mul( v1
, dir_inv
, v1
);
2034 tmin
= vg_minf( v0
[0], v1
[0] );
2035 tmax
= vg_maxf( v0
[0], v1
[0] );
2036 tmin
= vg_maxf( tmin
, vg_minf( v0
[1], v1
[1] ));
2037 tmax
= vg_minf( tmax
, vg_maxf( v0
[1], v1
[1] ));
2038 tmin
= vg_maxf( tmin
, vg_minf( v0
[2], v1
[2] ));
2039 tmax
= vg_minf( tmax
, vg_maxf( v0
[2], v1
[2] ));
2041 return (tmax
>= tmin
) && (tmin
<= dist
) && (tmax
>= 0.0f
);
2044 /* Time of intersection with ray vs triangle */
2045 static int ray_tri( v3f tri
[3], v3f co
,
2046 v3f dir
, f32
*dist
, int backfaces
)
2048 f32
const kEpsilon
= 0.00001f
;
2050 v3f v0
, v1
, h
, s
, q
, n
;
2057 v3_sub( pb
, pa
, v0
);
2058 v3_sub( pc
, pa
, v1
);
2059 v3_cross( dir
, v1
, h
);
2060 v3_cross( v0
, v1
, n
);
2062 if( (v3_dot( n
, dir
) > 0.0f
) && !backfaces
) /* Backface culling */
2066 a
= v3_dot( v0
, h
);
2068 if( a
> -kEpsilon
&& a
< kEpsilon
)
2072 v3_sub( co
, pa
, s
);
2074 u
= f
* v3_dot(s
, h
);
2075 if( u
< 0.0f
|| u
> 1.0f
)
2078 v3_cross( s
, v0
, q
);
2079 v
= f
* v3_dot( dir
, q
);
2080 if( v
< 0.0f
|| u
+v
> 1.0f
)
2083 t
= f
* v3_dot(v1
, q
);
2092 /* time of intersection with ray vs sphere */
2093 static int ray_sphere( v3f c
, f32 r
,
2094 v3f co
, v3f dir
, f32
*t
)
2099 f32 b
= v3_dot( m
, dir
),
2100 c1
= v3_dot( m
, m
) - r
*r
;
2102 /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */
2103 if( c1
> 0.0f
&& b
> 0.0f
)
2106 f32 discr
= b
*b
- c1
;
2108 /* A negative discriminant corresponds to ray missing sphere */
2113 * Ray now found to intersect sphere, compute smallest t value of
2116 *t
= -b
- sqrtf( discr
);
2118 /* If t is negative, ray started inside sphere so clamp t to zero */
2126 * time of intersection of ray vs cylinder
2127 * The cylinder does not have caps but is finite
2129 * Heavily adapted from regular segment vs cylinder from:
2130 * Real-Time Collision Detection
2132 static int ray_uncapped_finite_cylinder( v3f q
, v3f p
, f32 r
,
2133 v3f co
, v3f dir
, f32
*t
)
2136 v3_muladds( co
, dir
, 1.0f
, sb
);
2140 v3_sub( sb
, co
, n
);
2142 f32 md
= v3_dot( m
, d
),
2143 nd
= v3_dot( n
, d
),
2144 dd
= v3_dot( d
, d
),
2145 nn
= v3_dot( n
, n
),
2146 mn
= v3_dot( m
, n
),
2148 k
= v3_dot( m
, m
) - r
*r
,
2151 if( fabsf(a
) < 0.00001f
)
2153 /* Segment runs parallel to cylinder axis */
2157 f32 b
= dd
*mn
- nd
*md
,
2161 return 0; /* No real roots; no intersection */
2163 *t
= (-b
- sqrtf(discr
)) / a
;
2165 return 0; /* Intersection behind ray */
2167 /* Check within cylinder segment */
2168 if( md
+ (*t
)*nd
< 0.0f
)
2171 if( md
+ (*t
)*nd
> dd
)
2174 /* Segment intersects cylinder between the endcaps; t is correct */
2179 * Time of intersection of sphere and triangle. Origin must be outside the
2180 * colliding area. This is a fairly long procedure.
2182 static int spherecast_triangle( v3f tri
[3],
2183 v3f co
, v3f dir
, f32 r
, f32
*t
, v3f n
)
2188 v3_sub( tri
[1], tri
[0], v0
);
2189 v3_sub( tri
[2], tri
[0], v1
);
2190 v3_cross( v0
, v1
, n
);
2192 v3_muladds( tri
[0], n
, r
, sum
[0] );
2193 v3_muladds( tri
[1], n
, r
, sum
[1] );
2194 v3_muladds( tri
[2], n
, r
, sum
[2] );
2197 f32 t_min
= INFINITY
,
2200 if( ray_tri( sum
, co
, dir
, &t1
, 0 ) ){
2201 t_min
= vg_minf( t_min
, t1
);
2206 * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
2209 for( int i
=0; i
<3; i
++ ){
2210 if( ray_sphere( tri
[i
], r
, co
, dir
, &t1
) ){
2211 t_min
= vg_minf( t_min
, t1
);
2217 for( int i
=0; i
<3; i
++ ){
2221 if( ray_uncapped_finite_cylinder( tri
[i0
], tri
[i1
], r
, co
, dir
, &t1
) ){
2226 v3_add( dir
, co
, co1
);
2227 v3_lerp( co
, co1
, t_min
, ct
);
2229 closest_point_segment( tri
[i0
], tri
[i1
], ct
, cx
);
2230 v3_sub( ct
, cx
, n
);
2243 * -----------------------------------------------------------------------------
2244 * Section 5.e Curves
2245 * -----------------------------------------------------------------------------
2248 static void eval_bezier_time( v3f p0
, v3f p1
, v3f h0
, v3f h1
, f32 t
, v3f p
)
2253 v3_muls( p1
, ttt
, p
);
2254 v3_muladds( p
, h1
, 3.0f
*tt
-3.0f
*ttt
, p
);
2255 v3_muladds( p
, h0
, 3.0f
*ttt
-6.0f
*tt
+3.0f
*t
, p
);
2256 v3_muladds( p
, p0
, 3.0f
*tt
-ttt
-3.0f
*t
+1.0f
, p
);
2259 static void eval_bezier3( v3f p0
, v3f p1
, v3f p2
, f32 t
, v3f p
)
2263 v3_muls( p0
, u
*u
, p
);
2264 v3_muladds( p
, p1
, 2.0f
*u
*t
, p
);
2265 v3_muladds( p
, p2
, t
*t
, p
);
2269 * -----------------------------------------------------------------------------
2270 * Section 5.f Volumes
2271 * -----------------------------------------------------------------------------
2274 static f32
vg_sphere_volume( f32 r
){
2275 return (4.0f
/3.0f
) * VG_PIf
* r
*r
*r
;
2278 static f32
vg_box_volume( boxf box
){
2280 v3_sub( box
[1], box
[0], e
);
2281 return e
[0]*e
[1]*e
[2];
2284 static f32
vg_cylinder_volume( f32 r
, f32 h
){
2285 return VG_PIf
* r
*r
* h
;
2288 static f32
vg_capsule_volume( f32 r
, f32 h
){
2289 return vg_sphere_volume( r
) + vg_cylinder_volume( r
, h
-r
*2.0f
);
2292 static void vg_sphere_bound( f32 r
, boxf out_box
){
2293 v3_fill( out_box
[0], -r
);
2294 v3_fill( out_box
[1], r
);
2297 static void vg_capsule_bound( f32 r
, f32 h
, boxf out_box
){
2298 v3_copy( (v3f
){-r
,-h
*0.5f
,r
}, out_box
[0] );
2299 v3_copy( (v3f
){-r
, h
*0.5f
,r
}, out_box
[1] );
2304 * -----------------------------------------------------------------------------
2305 * Section 5.g Inertia Tensors
2306 * -----------------------------------------------------------------------------
2310 * Translate existing inertia tensor
2312 static void vg_translate_inertia( m3x3f inout_inertia
, f32 mass
, v3f d
){
2314 * I = I_0 + m*[(d.d)E_3 - d(X)d]
2317 * I_0: original tensor
2319 * d: translation vector
2320 * (X): outer product
2321 * E_3: identity matrix
2323 m3x3f t
, outer
, scale
;
2324 m3x3_diagonal( t
, v3_dot(d
,d
) );
2325 m3x3_outer_product( outer
, d
, d
);
2326 m3x3_sub( t
, outer
, t
);
2327 m3x3_diagonal( scale
, mass
);
2328 m3x3_mul( scale
, t
, t
);
2329 m3x3_add( inout_inertia
, t
, inout_inertia
);
2333 * Rotate existing inertia tensor
2335 static void vg_rotate_inertia( m3x3f inout_inertia
, m3x3f rotation
){
2340 * I_0: original tensor
2341 * R: rotation matrix
2342 * R^T: tranposed rotation matrix
2346 m3x3_transpose( rotation
, Rt
);
2347 m3x3_mul( rotation
, inout_inertia
, inout_inertia
);
2348 m3x3_mul( inout_inertia
, Rt
, inout_inertia
);
2351 * Create inertia tensor for box
2353 static void vg_box_inertia( boxf box
, f32 mass
, m3x3f out_inertia
){
2355 v3_sub( box
[1], box
[0], e
);
2356 v3_muladds( box
[0], e
, 0.5f
, com
);
2358 f32 ex2
= e
[0]*e
[0],
2361 ix
= (ey2
+ez2
) * mass
* (1.0f
/12.0f
),
2362 iy
= (ex2
+ez2
) * mass
* (1.0f
/12.0f
),
2363 iz
= (ex2
+ey2
) * mass
* (1.0f
/12.0f
);
2365 m3x3_identity( out_inertia
);
2366 m3x3_setdiagonalv3( out_inertia
, (v3f
){ ix
, iy
, iz
} );
2367 vg_translate_inertia( out_inertia
, mass
, com
);
2371 * Create inertia tensor for sphere
2373 static void vg_sphere_inertia( f32 r
, f32 mass
, m3x3f out_inertia
){
2374 f32 ixyz
= r
*r
* mass
* (2.0f
/5.0f
);
2376 m3x3_identity( out_inertia
);
2377 m3x3_setdiagonalv3( out_inertia
, (v3f
){ ixyz
, ixyz
, ixyz
} );
2381 * Create inertia tensor for capsule
2383 static void vg_capsule_inertia( f32 r
, f32 h
, f32 mass
, m3x3f out_inertia
){
2384 f32 density
= mass
/ vg_capsule_volume( r
, h
),
2385 ch
= h
-r
*2.0f
, /* cylinder height */
2386 cm
= VG_PIf
* ch
*r
*r
* density
, /* cylinder mass */
2387 hm
= VG_TAUf
* (1.0f
/3.0f
) * r
*r
*r
* density
, /* hemisphere mass */
2390 ixz
= iy
* 0.5f
+ cm
*ch
*ch
*(1.0f
/12.0f
),
2392 aux0
= (hm
*2.0f
*r
*r
)/5.0f
;
2397 aux2
= aux0
+ hm
*(aux1
*aux1
+ 3.0f
*(1.0f
/8.0f
)*ch
*r
);
2401 m3x3_identity( out_inertia
);
2402 m3x3_setdiagonalv3( out_inertia
, (v3f
){ ixz
, iy
, ixz
} );
2406 * -----------------------------------------------------------------------------
2407 * Section 6.a PSRNG and some distributions
2408 * -----------------------------------------------------------------------------
2411 /* An implementation of the MT19937 Algorithm for the Mersenne Twister
2412 * by Evan Sultanik. Based upon the pseudocode in: M. Matsumoto and
2413 * T. Nishimura, "Mersenne Twister: A 623-dimensionally
2414 * equidistributed uniform pseudorandom number generator," ACM
2415 * Transactions on Modeling and Computer Simulation Vol. 8, No. 1,
2416 * January pp.3-30 1998.
2418 * http://www.sultanik.com/Mersenne_twister
2419 * https://github.com/ESultanik/mtwister/blob/master/mtwister.c
2422 #define MT_UPPER_MASK 0x80000000
2423 #define MT_LOWER_MASK 0x7fffffff
2424 #define MT_TEMPERING_MASK_B 0x9d2c5680
2425 #define MT_TEMPERING_MASK_C 0xefc60000
2427 #define MT_STATE_VECTOR_LENGTH 624
2429 /* changes to STATE_VECTOR_LENGTH also require changes to this */
2430 #define MT_STATE_VECTOR_M 397
2432 typedef struct vg_rand vg_rand
;
2434 u32 mt
[MT_STATE_VECTOR_LENGTH
];
2438 static void vg_rand_seed( vg_rand
*rand
, unsigned long seed
) {
2439 /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator
2440 * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer
2441 * Programming," Vol. 2 (2nd Ed.) pp.102.
2443 rand
->mt
[0] = seed
& 0xffffffff;
2444 for( rand
->index
=1; rand
->index
<MT_STATE_VECTOR_LENGTH
; rand
->index
++){
2445 rand
->mt
[rand
->index
] = (6069 * rand
->mt
[rand
->index
-1]) & 0xffffffff;
2450 * Generates a pseudo-randomly generated long.
2452 static u32
vg_randu32( vg_rand
*rand
) {
2454 /* mag[x] = x * 0x9908b0df for x = 0,1 */
2455 static u32 mag
[2] = {0x0, 0x9908b0df};
2456 if( rand
->index
>= MT_STATE_VECTOR_LENGTH
|| rand
->index
< 0 ){
2457 /* generate STATE_VECTOR_LENGTH words at a time */
2459 if( rand
->index
>= MT_STATE_VECTOR_LENGTH
+1 || rand
->index
< 0 ){
2460 vg_rand_seed( rand
, 4357 );
2462 for( kk
=0; kk
<MT_STATE_VECTOR_LENGTH
-MT_STATE_VECTOR_M
; kk
++ ){
2463 y
= (rand
->mt
[kk
] & MT_UPPER_MASK
) |
2464 (rand
->mt
[kk
+1] & MT_LOWER_MASK
);
2465 rand
->mt
[kk
] = rand
->mt
[kk
+MT_STATE_VECTOR_M
] ^ (y
>>1) ^ mag
[y
& 0x1];
2467 for( ; kk
<MT_STATE_VECTOR_LENGTH
-1; kk
++ ){
2468 y
= (rand
->mt
[kk
] & MT_UPPER_MASK
) |
2469 (rand
->mt
[kk
+1] & MT_LOWER_MASK
);
2471 rand
->mt
[ kk
+(MT_STATE_VECTOR_M
-MT_STATE_VECTOR_LENGTH
)] ^
2472 (y
>> 1) ^ mag
[y
& 0x1];
2474 y
= (rand
->mt
[MT_STATE_VECTOR_LENGTH
-1] & MT_UPPER_MASK
) |
2475 (rand
->mt
[0] & MT_LOWER_MASK
);
2476 rand
->mt
[MT_STATE_VECTOR_LENGTH
-1] =
2477 rand
->mt
[MT_STATE_VECTOR_M
-1] ^ (y
>> 1) ^ mag
[y
& 0x1];
2480 y
= rand
->mt
[rand
->index
++];
2482 y
^= (y
<< 7) & MT_TEMPERING_MASK_B
;
2483 y
^= (y
<< 15) & MT_TEMPERING_MASK_C
;
2489 * Generates a pseudo-randomly generated f64 in the range [0..1].
2491 static inline f64
vg_randf64( vg_rand
*rand
){
2492 return (f64
)vg_randu32(rand
)/(f64
)0xffffffff;
2495 static inline f64
vg_randf64_range( vg_rand
*rand
, f64 min
, f64 max
){
2496 return vg_lerp( min
, max
, (f64
)vg_randf64(rand
) );
2499 static inline void vg_rand_dir( vg_rand
*rand
, v3f dir
){
2500 dir
[0] = vg_randf64(rand
);
2501 dir
[1] = vg_randf64(rand
);
2502 dir
[2] = vg_randf64(rand
);
2504 /* warning: *could* be 0 length.
2505 * very unlikely.. 1 in (2^32)^3. but its mathematically wrong. */
2507 v3_muls( dir
, 2.0f
, dir
);
2508 v3_sub( dir
, (v3f
){1.0f
,1.0f
,1.0f
}, dir
);
2510 v3_normalize( dir
);
2513 static inline void vg_rand_sphere( vg_rand
*rand
, v3f co
){
2514 vg_rand_dir(rand
,co
);
2515 v3_muls( co
, cbrtf( vg_randf64(rand
) ), co
);
2518 static void vg_rand_disc( vg_rand
*rand
, v2f co
){
2519 f32 a
= vg_randf64(rand
) * VG_TAUf
;
2522 v2_muls( co
, sqrtf( vg_randf64(rand
) ), co
);
2525 static void vg_rand_cone( vg_rand
*rand
, v3f out_dir
, f32 angle
){
2526 f32 r
= sqrtf(vg_randf64(rand
)) * angle
* 0.5f
,
2527 a
= vg_randf64(rand
) * VG_TAUf
;
2529 out_dir
[0] = sinf(a
) * sinf(r
);
2530 out_dir
[1] = cosf(a
) * sinf(r
);
2531 out_dir
[2] = cosf(r
);
2534 static void vg_hsv_rgb( v3f hsv
, v3f rgb
){
2535 i32 i
= floorf( hsv
[0]*6.0f
);
2537 f
= hsv
[0] * 6.0f
- (f32
)i
,
2538 p
= v
* (1.0f
-hsv
[1]),
2539 q
= v
* (1.0f
-f
*hsv
[1]),
2540 t
= v
* (1.0f
-(1.0f
-f
)*hsv
[1]);
2543 case 0: rgb
[0] = v
; rgb
[1] = t
; rgb
[2] = p
; break;
2544 case 1: rgb
[0] = q
; rgb
[1] = v
; rgb
[2] = p
; break;
2545 case 2: rgb
[0] = p
; rgb
[1] = v
; rgb
[2] = t
; break;
2546 case 3: rgb
[0] = p
; rgb
[1] = q
; rgb
[2] = v
; break;
2547 case 4: rgb
[0] = t
; rgb
[1] = p
; rgb
[2] = v
; break;
2548 case 5: rgb
[0] = v
; rgb
[1] = p
; rgb
[2] = q
; break;
2552 static void vg_rgb_hsv( v3f rgb
, v3f hsv
){
2553 f32 min
= v3_minf( rgb
),
2554 max
= v3_maxf( rgb
),
2556 k_epsilon
= 0.00001f
;
2559 if( range
< k_epsilon
){
2565 if( max
> k_epsilon
){
2575 hsv
[0] = (rgb
[1]-rgb
[2])/range
;
2576 else if( max
== rgb
[1] )
2577 hsv
[0] = 2.0f
+(rgb
[2]-rgb
[0])/range
;
2579 hsv
[0] = 4.0f
+(rgb
[0]-rgb
[1])/range
;
2581 hsv
[0] = vg_fractf( hsv
[0] * (60.0f
/360.0f
) );