X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=vg_m.h;h=c753be797bc9e1da62b96c29ead2e8e800f7f0b9;hb=76d234b7dc5e6500e8a54009b367e7620f11ef97;hp=c07f38984e07ada336ffe5c8328fef6649a3243a;hpb=bafc8bc648b83f3b5f5567eedde1fc4533660e4f;p=vg.git diff --git a/vg_m.h b/vg_m.h index c07f389..c753be7 100644 --- a/vg_m.h +++ b/vg_m.h @@ -18,6 +18,7 @@ * 5.c Closest points * 5.d Raycast & Spherecasts * 5.e Curves + * 5.f Volumes * 6. Statistics * 6.a Random numbers **/ @@ -62,6 +63,12 @@ static int vg_validf( f32 a ) return ((vg_ftu32(a)) & 0x7F800000U) != 0x7F800000U; } +static int v3_valid( v3f a ){ + for( u32 i=0; i<3; i++ ) + if( !vg_validf(a[i]) ) return 0; + return 1; +} + /* * ----------------------------------------------------------------------------- * Section 1. Scalar Operations @@ -392,8 +399,7 @@ static inline void v3_normalize( v3f a ) v3_muls( a, 1.f / v3_length( a ), a ); } -static inline f32 vg_lerpf( f32 a, f32 b, f32 t ) -{ +static inline f32 vg_lerpf( f32 a, f32 b, f32 t ){ return a + t*(b-a); } @@ -402,6 +408,17 @@ static inline f64 vg_lerp( f64 a, f64 b, f64 t ) return a + t*(b-a); } +static inline void vg_slewf( f32 *a, f32 b, f32 speed ){ + f32 d = vg_signf( b-*a ), + c = *a + d*speed; + *a = vg_minf( b*d, c*d ) * d; +} + +static inline f32 vg_smoothstepf( f32 x ){ + return x*x*(3.0f - 2.0f*x); +} + + /* correctly lerp around circular period -pi -> pi */ static f32 vg_alerpf( f32 a, f32 b, f32 t ) { @@ -480,6 +497,24 @@ static inline void v3_rotate( v3f v, f32 angle, v3f axis, v3f d ) v3_add( v1, v2, d ); } +static void v3_tangent_basis( v3f n, v3f tx, v3f ty ){ + /* Compute tangent basis (box2d) */ + if( fabsf( n[0] ) >= 0.57735027f ){ + tx[0] = n[1]; + tx[1] = -n[0]; + tx[2] = 0.0f; + } + else{ + tx[0] = 0.0f; + tx[1] = n[2]; + tx[2] = -n[1]; + } + + v3_normalize( tx ); + v3_cross( n, tx, ty ); +} + + /* * ----------------------------------------------------------------------------- * Section 2.c 4D Vectors @@ -573,11 +608,15 @@ static inline void q_mul( v4f q, v4f q1, v4f d ) static inline void q_normalize( v4f q ) { - f32 s = 1.0f/ sqrtf(v4_dot(q,q)); - q[0] *= s; - q[1] *= s; - q[2] *= s; - q[3] *= s; + f32 l2 = v4_dot(q,q); + if( l2 < 0.00001f ) q_identity( q ); + else { + f32 s = 1.0f/sqrtf(l2); + q[0] *= s; + q[1] *= s; + q[2] *= s; + q[3] *= s; + } } static inline void q_inv( v4f q, v4f d ) @@ -589,11 +628,11 @@ static inline void q_inv( v4f q, v4f d ) d[3] = q[3]*s; } -static inline void q_nlerp( v4f a, v4f b, f32 t, v4f d ) -{ +static inline void q_nlerp( v4f a, v4f b, f32 t, v4f d ){ if( v4_dot(a,b) < 0.0f ){ - v4_muls( b, -1.0f, d ); - v4_lerp( a, d, t, d ); + v4f c; + v4_muls( b, -1.0f, c ); + v4_lerp( a, c, t, d ); } else v4_lerp( a, b, t, d ); @@ -671,6 +710,16 @@ static inline void m2x2_create_rotation( m2x2f a, f32 theta ) a[1][1] = c; } +static inline void m2x2_mulv( m2x2f m, v2f v, v2f d ) +{ + v2f res; + + res[0] = m[0][0]*v[0] + m[1][0]*v[1]; + res[1] = m[0][1]*v[0] + m[1][1]*v[1]; + + v2_copy( res, d ); +} + /* * ----------------------------------------------------------------------------- * Section 4.b 3x3 matrices @@ -1155,8 +1204,7 @@ static void m4x3_decompose( m4x3f m, v3f co, v4f q, v3f s ) m3x3_q( rot, q ); } -static void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ) -{ +static void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ){ v3f v; m4x3_mulv( m, point, v ); @@ -1164,26 +1212,19 @@ static void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ) v3_maxv( box[1], v, box[1] ); } -static void m4x3_transform_aabb( m4x3f m, boxf box ) -{ +static void m4x3_expand_aabb_aabb( m4x3f m, boxf boxa, boxf boxb ){ v3f a; v3f b; - - v3_copy( box[0], a ); - v3_copy( box[1], b ); - v3_fill( box[0], INFINITY ); - v3_fill( box[1], -INFINITY ); - - m4x3_expand_aabb_point( m, box, (v3f){ a[0], a[1], a[2] } ); - m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], a[2] } ); - m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], a[2] } ); - m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], a[2] } ); - - m4x3_expand_aabb_point( m, box, (v3f){ a[0], a[1], b[2] } ); - m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], b[2] } ); - m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], b[2] } ); - m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], b[2] } ); + v3_copy( boxb[0], a ); + v3_copy( boxb[1], b ); + m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], a[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], b[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], b[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], a[1], a[2] } ); + m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], a[1], b[2] } ); + m4x3_expand_aabb_point( m, boxa, (v3f){ a[0], b[1], b[2] } ); + m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], b[1], b[2] } ); + m4x3_expand_aabb_point( m, boxa, (v3f){ b[0], a[1], b[2] } ); } - static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up ) { v3f dir; @@ -1418,8 +1459,7 @@ static int box_within( boxf greater, boxf lesser ) return 0; } -static inline void box_init_inf( boxf box ) -{ +static inline void box_init_inf( boxf box ){ v3_fill( box[0], INFINITY ); v3_fill( box[1], -INFINITY ); } @@ -1506,7 +1546,7 @@ int plane_intersect2( v4f a, v4f b, v3f p, v3f n ) static int plane_segment( v4f plane, v3f a, v3f b, v3f co ) { f32 d0 = v3_dot( a, plane ) - plane[3], - d1 = v3_dot( b, plane ) - plane[3]; + d1 = v3_dot( b, plane ) - plane[3]; if( d0*d1 < 0.0f ) { @@ -1528,6 +1568,17 @@ static inline f64 plane_polarity( f64 p[4], f64 a[3] ) ; } +static f32 ray_plane( v4f plane, v3f co, v3f dir ){ + f32 d = v3_dot( plane, dir ); + if( fabsf(d) > 1e-6f ){ + v3f v0; + v3_muls( plane, plane[3], v0 ); + v3_sub( v0, co, v0 ); + return v3_dot( v0, plane ) / d; + } + else return INFINITY; +} + /* * ----------------------------------------------------------------------------- * Section 5.c Closest point functions @@ -1538,7 +1589,7 @@ static inline f64 plane_polarity( f64 p[4], f64 a[3] ) * These closest point tests were learned from Real-Time Collision Detection by * Christer Ericson */ -VG_STATIC f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, +static f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, f32 *s, f32 *t, v3f c1, v3f c2) { v3f d1,d2,r; @@ -1615,7 +1666,7 @@ VG_STATIC f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, return v3_length2( v0 ); } -VG_STATIC int point_inside_aabb( boxf box, v3f point ) +static int point_inside_aabb( boxf box, v3f point ) { if((point[0]<=box[1][0]) && (point[1]<=box[1][1]) && (point[2]<=box[1][2]) && (point[0]>=box[0][0]) && (point[1]>=box[0][1]) && (point[2]>=box[0][2]) ) @@ -1624,13 +1675,13 @@ VG_STATIC int point_inside_aabb( boxf box, v3f point ) return 0; } -VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest ) +static void closest_point_aabb( v3f p, boxf box, v3f dest ) { v3_maxv( p, box[0], dest ); v3_minv( dest, box[1], dest ); } -VG_STATIC void closest_point_obb( v3f p, boxf box, +static void closest_point_obb( v3f p, boxf box, m4x3f mtx, m4x3f inv_mtx, v3f dest ) { v3f local; @@ -1639,7 +1690,7 @@ VG_STATIC void closest_point_obb( v3f p, boxf box, m4x3_mulv( mtx, local, dest ); } -VG_STATIC f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) +static f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) { v3f v0, v1; v3_sub( b, a, v0 ); @@ -1651,7 +1702,7 @@ VG_STATIC f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) return t; } -VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) +static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) { v3f ab, ac, ap; f32 d1, d2; @@ -1747,7 +1798,7 @@ enum contact_type k_contact_type_edge }; -VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) +static enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) { v3f ab, ac, ap; f32 d1, d2; @@ -2120,6 +2171,17 @@ static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p ) v3_muladds( p, p2, t*t, p ); } +/* + * ----------------------------------------------------------------------------- + * Section 5.f Volumes + * ----------------------------------------------------------------------------- + */ + +static float vg_sphere_volume( float radius ){ + float r3 = radius*radius*radius; + return (4.0f/3.0f) * VG_PIf * r3; +} + /* * ----------------------------------------------------------------------------- * Section 6.a PSRNG and some distributions @@ -2160,7 +2222,7 @@ static void vg_rand_seed( unsigned long seed ) * Programming," Vol. 2 (2nd Ed.) pp.102. */ vg_rand.mt[0] = seed & 0xffffffff; - for( vg_rand.index=1; vg_rand.index