X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=vg_m.h;h=65426053001da34bf4c1dccb8e90fe7b8fa7cab0;hb=ff8e8a170cb29b450f7f26580683ec3ae81cf4bd;hp=fbb3c45317915d592d4fbb8120390027999944d9;hpb=a1c1af2679576d938c8c662805516e12cf217295;p=vg.git diff --git a/vg_m.h b/vg_m.h index fbb3c45..6542605 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; @@ -262,6 +269,11 @@ 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]; @@ -275,6 +287,11 @@ 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]; @@ -1014,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; @@ -1022,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] ); @@ -1032,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 ) @@ -1249,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; - - 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]; - - d = x[0] * c[0] + x[1] * c[1] + x[2] * c[2]; + float const epsilon = 1e-6f; - if( d < epsilon && d > -epsilon ) return 0; + v3f x; + v3_cross( a, b, x ); + float d = v3_dot( x, c ); - 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 @@ -1352,6 +1397,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 @@ -1416,6 +1481,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, @@ -1510,6 +1587,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 ); @@ -1777,6 +1863,7 @@ static int ray_tri( v3f tri[3], v3f co, /* Parralel */ a = v3_dot( v0, h ); + if( a > -kEpsilon && a < kEpsilon ) return 0;