X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=vg_m.h;h=2a5082d9b813c53687fb60254b47b5c141836caa;hb=e1f9315d807caad59c038b0796cf8792e1f96a9e;hp=d6da3a1b933e2ff14790895728e6a5918d344903;hpb=fa8c2262459bbc760875536323baa5c05349ed75;p=vg.git diff --git a/vg_m.h b/vg_m.h index d6da3a1..2a5082d 100644 --- a/vg_m.h +++ b/vg_m.h @@ -56,6 +56,8 @@ static inline float vg_fractf( float a ) return a - floorf( a ); } + +__attribute__ ((deprecated)) static float stable_force( float current, float diff ) { float fnew = current + diff; @@ -66,6 +68,11 @@ static float stable_force( float current, float diff ) return fnew; } +static float vg_cfrictf( float current, float F ) +{ + return -vg_signf(current) * vg_minf( F, fabsf(current) ); +} + static inline int vg_min( int a, int b ) { return a < b? a: b; @@ -146,6 +153,12 @@ static inline void v2_add( v2f a, v2f b, v2f d ) d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; } +static inline void v2_abs( v2f a, v2f d ) +{ + d[0] = fabsf( a[0] ); + d[1] = fabsf( a[1] ); +} + static inline void v2_muls( v2f a, float s, v2f d ) { d[0] = a[0]*s; d[1] = a[1]*s; @@ -225,6 +238,19 @@ static inline void v2_floor( v2f a, v2f b ) b[1] = floorf( a[1] ); } +static inline void v2_fill( v2f a, float v ) +{ + a[0] = v; + a[1] = v; +} + +/* copysign of b to a */ +static inline void v2_copysign( v2f a, v2f b ) +{ + a[0] = copysignf( a[0], b[0] ); + a[1] = copysignf( a[1], b[1] ); +} + /* * Vector 3 */ @@ -243,11 +269,29 @@ static inline void v3_add( v3f a, v3f b, v3f d ) d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2]; } +static inline void v3i_add( v3i a, v3i b, v3i d ) +{ + d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2]; +} + +static inline void v4_add( v4f a, v4f b, v4f d ) +{ + d[0] = a[0]+b[0]; + d[1] = a[1]+b[1]; + d[2] = a[2]+b[2]; + d[3] = a[3]+b[3]; +} + static inline void v3_sub( v3f a, v3f b, v3f d ) { d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2]; } +static inline void v3i_sub( v3i a, v3i b, v3i d ) +{ + d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2]; +} + static inline void v3_mul( v3f a, v3f b, v3f d ) { d[0] = a[0]*b[0]; d[1] = a[1]*b[1]; d[2] = a[2]*b[2]; @@ -520,6 +564,27 @@ static inline void m2x2_create_rotation( m2x2f a, float theta ) { 0.0f, 0.0f, 0.0f, }} +/* a X b == [b]T a == ...*/ +static void m3x3_skew_symetric( m3x3f a, v3f v ) +{ + a[0][0] = 0.0f; + a[0][1] = v[2]; + a[0][2] = -v[1]; + a[1][0] = -v[2]; + a[1][1] = 0.0f; + a[1][2] = v[0]; + a[2][0] = v[1]; + a[2][1] = -v[0]; + a[2][2] = 0.0f; +} + +static void m3x3_add( m3x3f a, m3x3f b, m3x3f d ) +{ + v3_add( a[0], b[0], d[0] ); + v3_add( a[1], b[1], d[1] ); + v3_add( a[2], b[2], d[2] ); +} + static inline void m3x3_copy( m3x3f a, m3x3f b ) { v3_copy( a[0], b[0] ); @@ -533,6 +598,14 @@ static inline void m3x3_identity( m3x3f a ) m3x3_copy( id, a ); } +static void m3x3_diagonal( m3x3f a, float v ) +{ + m3x3_identity( a ); + a[0][0] = v; + a[1][1] = v; + a[2][2] = v; +} + static inline void m3x3_zero( m3x3f a ) { m3x3f z = M3X3_ZERO; @@ -561,6 +634,13 @@ static inline void m3x3_inv( m3x3f src, m3x3f dest ) dest[2][2] = (a*e-d*b)*det; } +static float m3x3_det( m3x3f m ) +{ + return m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]) + - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); +} + static inline void m3x3_transpose( m3x3f src, m3x3f dest ) { float a = src[0][0], b = src[0][1], c = src[0][2], @@ -951,7 +1031,7 @@ static inline void m4x3_transform_aabb( m4x3f m, boxf box ) m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], b[2] } ); } -int ray_aabb( boxf box, v3f co, v3f dir, float dist ) +int ray_aabb1( boxf box, v3f co, v3f dir_inv, float dist ) { v3f v0, v1; float tmin, tmax; @@ -959,8 +1039,8 @@ int ray_aabb( boxf box, v3f co, v3f dir, float dist ) v3_sub( box[0], co, v0 ); v3_sub( box[1], co, v1 ); - v3_div( v0, dir, v0 ); - v3_div( v1, dir, v1 ); + v3_mul( v0, dir_inv, v0 ); + v3_mul( v1, dir_inv, v1 ); tmin = vg_minf( v0[0], v1[0] ); tmax = vg_maxf( v0[0], v1[0] ); @@ -969,7 +1049,7 @@ int ray_aabb( boxf box, v3f co, v3f dir, float dist ) tmin = vg_maxf( tmin, vg_minf( v0[2], v1[2] )); tmax = vg_minf( tmax, vg_maxf( v0[2], v1[2] )); - return tmax >= tmin && tmin < dist && tmax > 0; + return (tmax >= tmin) && (tmin <= dist) && (tmax >= 0.0f); } static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up ) @@ -1186,41 +1266,69 @@ static inline void tri_to_plane( double a[3], double b[3], p[2] = p[2] / l; } -static inline int plane_intersect( double a[4], double b[4], - double c[4], double p[4] ) +static int plane_intersect3( v4f a, v4f b, v4f c, v3f p ) { - double const epsilon = 1e-8f; - - double x[3]; - double d; + float const epsilon = 1e-6f; - x[0] = a[1] * b[2] - a[2] * b[1]; - x[1] = a[2] * b[0] - a[0] * b[2]; - x[2] = a[0] * b[1] - a[1] * b[0]; + v3f x; + v3_cross( a, b, x ); + float d = v3_dot( x, c ); - d = x[0] * c[0] + x[1] * c[1] + x[2] * c[2]; - - if( d < epsilon && d > -epsilon ) return 0; - - p[0] = (b[1] * c[2] - b[2] * c[1]) * -a[3]; - p[1] = (b[2] * c[0] - b[0] * c[2]) * -a[3]; - p[2] = (b[0] * c[1] - b[1] * c[0]) * -a[3]; - - p[0] += (c[1] * a[2] - c[2] * a[1]) * -b[3]; - p[1] += (c[2] * a[0] - c[0] * a[2]) * -b[3]; - p[2] += (c[0] * a[1] - c[1] * a[0]) * -b[3]; - - p[0] += (a[1] * b[2] - a[2] * b[1]) * -c[3]; - p[1] += (a[2] * b[0] - a[0] * b[2]) * -c[3]; - p[2] += (a[0] * b[1] - a[1] * b[0]) * -c[3]; - - p[0] = -p[0] / d; - p[1] = -p[1] / d; - p[2] = -p[2] / d; + if( (d < epsilon) && (d > -epsilon) ) return 0; + + v3f v0, v1, v2; + v3_cross( b, c, v0 ); + v3_cross( c, a, v1 ); + v3_cross( a, b, v2 ); + + v3_muls( v0, a[3], p ); + v3_muladds( p, v1, b[3], p ); + v3_muladds( p, v2, c[3], p ); + v3_divs( p, d, p ); return 1; } +int plane_intersect2( v4f a, v4f b, v3f p, v3f n ) +{ + float const epsilon = 1e-6f; + + v4f c; + v3_cross( a, b, c ); + float d = v3_length2( c ); + + if( (d < epsilon) && (d > -epsilon) ) + return 0; + + v3f v0, v1, vx; + v3_cross( c, b, v0 ); + v3_cross( a, c, v1 ); + + v3_muls( v0, a[3], vx ); + v3_muladds( vx, v1, b[3], vx ); + v3_divs( vx, d, p ); + v3_copy( c, n ); + + return 1; +} + +static int plane_segment( v4f plane, v3f a, v3f b, v3f co ) +{ + float d0 = v3_dot( a, plane ) - plane[3], + d1 = v3_dot( b, plane ) - plane[3]; + + if( d0*d1 < 0.0f ) + { + float tot = 1.0f/( fabsf(d0)+fabsf(d1) ); + + v3_muls( a, fabsf(d1) * tot, co ); + v3_muladds( co, b, fabsf(d0) * tot, co ); + return 1; + } + + return 0; +} + static inline double plane_polarity( double p[4], double a[3] ) { return @@ -1278,8 +1386,7 @@ static inline void q_inv( v4f q, v4f d ) static inline void q_nlerp( v4f a, v4f b, float t, v4f d ) { - if( v4_dot(a,b) < 0.0f ) - { + if( v4_dot(a,b) < 0.0f ){ v4_muls( b, -1.0f, d ); v4_lerp( a, d, t, d ); } @@ -1289,6 +1396,26 @@ static inline void q_nlerp( v4f a, v4f b, float t, v4f d ) q_normalize( d ); } +static void euler_m3x3( v3f angles, m3x3f d ) +{ + float cosY = cosf( angles[0] ), + sinY = sinf( angles[0] ), + cosP = cosf( angles[1] ), + sinP = sinf( angles[1] ), + cosR = cosf( angles[2] ), + sinR = sinf( angles[2] ); + + d[2][0] = -sinY * cosP; + d[2][1] = sinP; + d[2][2] = cosY * cosP; + + d[0][0] = cosY * cosR; + d[0][1] = sinR; + d[0][2] = sinY * cosR; + + v3_cross( d[0], d[2], d[1] ); +} + static inline void q_m3x3( v4f q, m3x3f d ) { float @@ -1353,6 +1480,18 @@ static void m3x3_q( m3x3f m, v4f q ) } } +static void q_mulv( v4f q, v3f v, v3f d ) +{ + v3f v1, v2; + + v3_muls( q, 2.0f*v3_dot(q,v), v1 ); + v3_muls( v, q[3]*q[3] - v3_dot(q,q), v2 ); + v3_add( v1, v2, v1 ); + v3_cross( q, v, v2 ); + v3_muls( v2, 2.0f*q[3], v2 ); + v3_add( v1, v2, d ); +} + enum contact_type { k_contact_type_default, @@ -1447,6 +1586,15 @@ VG_STATIC float 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 ) +{ + 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]) ) + return 1; + else + return 0; +} + VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest ) { v3_maxv( p, box[0], dest ); @@ -1648,6 +1796,45 @@ VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) return k_contact_type_default; } + +static void closest_point_elipse( v2f p, v2f e, v2f o ) +{ + v2f pabs, ei, e2, ve, t; + + v2_abs( p, pabs ); + v2_div( (v2f){ 1.0f, 1.0f }, e, ei ); + v2_mul( e, e, e2 ); + v2_mul( ei, (v2f){ e2[0]-e2[1], e2[1]-e2[0] }, ve ); + + v2_fill( t, 0.70710678118654752f ); + + for( int i=0; i<3; i++ ) + { + v2f v, u, ud, w; + + v2_mul( ve, t, v ); /* ve*t*t*t */ + v2_mul( v, t, v ); + v2_mul( v, t, v ); + + v2_sub( pabs, v, u ); + v2_normalize( u ); + + v2_mul( t, e, ud ); + v2_sub( ud, v, ud ); + + v2_muls( u, v2_length( ud ), u ); + + v2_add( v, u, w ); + v2_mul( w, ei, w ); + + v2_maxv( (v2f){0.0f,0.0f}, w, t ); + v2_normalize( t ); + } + + v2_mul( t, e, o ); + v2_copysign( o, p ); +} + /* * Raycasts */ @@ -1675,6 +1862,7 @@ static int ray_tri( v3f tri[3], v3f co, /* Parralel */ a = v3_dot( v0, h ); + if( a > -kEpsilon && a < kEpsilon ) return 0; @@ -1857,6 +2045,7 @@ static int spherecast_triangle( v3f tri[3], static inline float vg_randf(void) { + /* TODO: replace with our own rand */ return (float)rand()/(float)(RAND_MAX); } @@ -1894,4 +2083,13 @@ static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p ) v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p ); } +static void eval_bezier3( v3f p0, v3f p1, v3f p2, float t, v3f p ) +{ + float u = 1.0f-t; + + v3_muls( p0, u*u, p ); + v3_muladds( p, p1, 2.0f*u*t, p ); + v3_muladds( p, p2, t*t, p ); +} + #endif /* VG_M_H */