X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=vg_m.h;h=a8b2091fe3ae51b5a6124820e1994189368e042b;hb=8c967ecac9d925e5fe14ecaeb6c0e4b2b23dfac4;hp=368f2889aff1f23807325e92f11228f22c654305;hpb=80c63a082132aa37bf0578d4d6757880db748a79;p=vg.git diff --git a/vg_m.h b/vg_m.h index 368f288..a8b2091 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,21 @@ 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 void m3x3_setdiagonalv3( m3x3f a, v3f v ) +{ + a[0][0] = v[0]; + a[1][1] = v[1]; + a[2][2] = v[2]; +} + static inline void m3x3_zero( m3x3f a ) { m3x3f z = M3X3_ZERO; @@ -561,6 +641,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], @@ -634,13 +721,16 @@ static inline void m3x3_translate( m3x3f m, v3f v ) static inline void m3x3_scale( m3x3f m, v3f v ) { - m[0][0] = m[0][0] * v[0]; - m[0][1] = m[0][1] * v[0]; - m[0][2] = m[0][2] * v[0]; + v3_muls( m[0], v[0], m[0] ); + v3_muls( m[1], v[1], m[1] ); + v3_muls( m[2], v[2], m[2] ); +} - m[1][0] = m[1][0] * v[1]; - m[1][1] = m[1][1] * v[1]; - m[1][2] = m[1][2] * v[1]; +static inline void m3x3_scalef( m3x3f m, float f ) +{ + v3f v; + v3_fill( v, f ); + m3x3_scale( m, v ); } static inline void m3x3_rotate( m3x3f m, float angle ) @@ -662,227 +752,6 @@ static inline void m3x3_rotate( m3x3f m, float angle ) m[1][2] = m02 * -s + m12 * c; } -/* - * Matrix 4x3 - */ - -#define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\ - { 0.0f, 1.0f, 0.0f, },\ - { 0.0f, 0.0f, 1.0f, },\ - { 0.0f, 0.0f, 0.0f }} - -static inline void m4x3_to_3x3( m4x3f a, m3x3f b ) -{ - v3_copy( a[0], b[0] ); - v3_copy( a[1], b[1] ); - v3_copy( a[2], b[2] ); -} - -static inline void m4x3_invert_affine( m4x3f a, m4x3f b ) -{ - m3x3_transpose( a, b ); - m3x3_mulv( b, a[3], b[3] ); - v3_negate( b[3], b[3] ); -} - -static void m4x3_invert_full( m4x3f src, m4x3f dst ) -{ - float t2, t4, t5, - det, - a = src[0][0], b = src[0][1], c = src[0][2], - e = src[1][0], f = src[1][1], g = src[1][2], - i = src[2][0], j = src[2][1], k = src[2][2], - m = src[3][0], n = src[3][1], o = src[3][2]; - - t2 = j*o - n*k; - t4 = i*o - m*k; - t5 = i*n - m*j; - - dst[0][0] = f*k - g*j; - dst[1][0] =-(e*k - g*i); - dst[2][0] = e*j - f*i; - dst[3][0] =-(e*t2 - f*t4 + g*t5); - - dst[0][1] =-(b*k - c*j); - dst[1][1] = a*k - c*i; - dst[2][1] =-(a*j - b*i); - dst[3][1] = a*t2 - b*t4 + c*t5; - - t2 = f*o - n*g; - t4 = e*o - m*g; - t5 = e*n - m*f; - - dst[0][2] = b*g - c*f ; - dst[1][2] =-(a*g - c*e ); - dst[2][2] = a*f - b*e ; - dst[3][2] =-(a*t2 - b*t4 + c * t5); - - det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]); - v3_muls( dst[0], det, dst[0] ); - v3_muls( dst[1], det, dst[1] ); - v3_muls( dst[2], det, dst[2] ); - v3_muls( dst[3], det, dst[3] ); -} - -static inline void m4x3_copy( m4x3f a, m4x3f b ) -{ - v3_copy( a[0], b[0] ); - v3_copy( a[1], b[1] ); - v3_copy( a[2], b[2] ); - v3_copy( a[3], b[3] ); -} - -static inline void m4x3_identity( m4x3f a ) -{ - m4x3f id = M4X3_IDENTITY; - m4x3_copy( id, a ); -} - -static inline void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) -{ - float - a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], - a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], - a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], - a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], - b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], - b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], - b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], - b30 = b[3][0], b31 = b[3][1], b32 = b[3][2]; - - d[0][0] = a00*b00 + a10*b01 + a20*b02; - d[0][1] = a01*b00 + a11*b01 + a21*b02; - d[0][2] = a02*b00 + a12*b01 + a22*b02; - d[1][0] = a00*b10 + a10*b11 + a20*b12; - d[1][1] = a01*b10 + a11*b11 + a21*b12; - d[1][2] = a02*b10 + a12*b11 + a22*b12; - d[2][0] = a00*b20 + a10*b21 + a20*b22; - d[2][1] = a01*b20 + a11*b21 + a21*b22; - d[2][2] = a02*b20 + a12*b21 + a22*b22; - d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30; - d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31; - d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32; -} - -static inline void m4x3_mulv( m4x3f m, v3f v, v3f d ) -{ - v3f res; - - res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]; - res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]; - res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]; - - v3_copy( res, d ); -} - -/* - * Transform plane ( xyz, distance ) - */ -static inline void m4x3_mulp( m4x3f m, v4f p, v4f d ) -{ - v3f o; - - v3_muls( p, p[3], o ); - m4x3_mulv( m, o, o ); - m3x3_mulv( m, p, d ); - - d[3] = v3_dot( o, d ); -} - -/* - * Affine transforms - */ - -static inline void m4x3_translate( m4x3f m, v3f v ) -{ - v3_muladds( m[3], m[0], v[0], m[3] ); - v3_muladds( m[3], m[1], v[1], m[3] ); - v3_muladds( m[3], m[2], v[2], m[3] ); -} - -static inline void m4x3_scale( m4x3f m, float s ) -{ - v3_muls( m[0], s, m[0] ); - v3_muls( m[1], s, m[1] ); - v3_muls( m[2], s, m[2] ); -} - -static inline void m4x3_scalev( m4x3f m, v3f v ) -{ - v3_muls(m[0], v[0], m[0]); - v3_muls(m[1], v[1], m[1]); - v3_muls(m[2], v[2], m[2]); -} - -static inline void m4x3_rotate_x( m4x3f m, float angle ) -{ - m4x3f t = M4X3_IDENTITY; - float c, s; - - c = cosf( angle ); - s = sinf( angle ); - - t[1][1] = c; - t[1][2] = s; - t[2][1] = -s; - t[2][2] = c; - - m4x3_mul( m, t, m ); -} - -static inline void m4x3_rotate_y( m4x3f m, float angle ) -{ - m4x3f t = M4X3_IDENTITY; - float c, s; - - c = cosf( angle ); - s = sinf( angle ); - - t[0][0] = c; - t[0][2] = -s; - t[2][0] = s; - t[2][2] = c; - - m4x3_mul( m, t, m ); -} - -static inline void m4x3_rotate_z( m4x3f m, float angle ) -{ - m4x3f t = M4X3_IDENTITY; - float c, s; - - c = cosf( angle ); - s = sinf( angle ); - - t[0][0] = c; - t[0][1] = s; - t[1][0] = -s; - t[1][1] = c; - - m4x3_mul( m, t, m ); -} - -static inline void m4x3_expand( m4x3f m, m4x4f d ) -{ - v3_copy( m[0], d[0] ); - v3_copy( m[1], d[1] ); - v3_copy( m[2], d[2] ); - v3_copy( m[3], d[3] ); - d[0][3] = 0.0f; - d[1][3] = 0.0f; - d[2][3] = 0.0f; - d[3][3] = 1.0f; -} - -static inline void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ) -{ - v3f v; - m4x3_mulv( m, point, v ); - - v3_minv( box[0], v, box[0] ); - v3_maxv( box[1], v, box[1] ); -} - static inline void box_addpt( boxf a, v3f pt ) { v3_minv( a[0], pt, a[0] ); @@ -931,36 +800,16 @@ static inline void box_init_inf( boxf box ) v3_fill( box[1], -INFINITY ); } -static inline void m4x3_transform_aabb( m4x3f m, boxf box ) +int ray_aabb1( boxf box, v3f co, v3f dir_inv, float dist ) { - 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] } ); -} - -int ray_aabb( boxf box, v3f co, v3f dir, float dist ) -{ - v3f v0, v1; - float tmin, tmax; + v3f v0, v1; + float tmin, tmax; 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 +818,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 +1035,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]; - - 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]; + float const epsilon = 1e-6f; - 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]; + v3f x; + v3_cross( a, b, x ); + float d = v3_dot( x, c ); - 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 +1155,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 +1165,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,7 +1249,613 @@ static void m3x3_q( m3x3f m, v4f q ) } } -static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist ) +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, + k_contact_type_disabled, + k_contact_type_edge +}; + +/* + * Matrix 4x3 + */ + +#define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\ + { 0.0f, 1.0f, 0.0f, },\ + { 0.0f, 0.0f, 1.0f, },\ + { 0.0f, 0.0f, 0.0f }} + +static inline void m4x3_to_3x3( m4x3f a, m3x3f b ) +{ + v3_copy( a[0], b[0] ); + v3_copy( a[1], b[1] ); + v3_copy( a[2], b[2] ); +} + +static inline void m4x3_invert_affine( m4x3f a, m4x3f b ) +{ + m3x3_transpose( a, b ); + m3x3_mulv( b, a[3], b[3] ); + v3_negate( b[3], b[3] ); +} + +static void m4x3_invert_full( m4x3f src, m4x3f dst ) +{ + float t2, t4, t5, + det, + a = src[0][0], b = src[0][1], c = src[0][2], + e = src[1][0], f = src[1][1], g = src[1][2], + i = src[2][0], j = src[2][1], k = src[2][2], + m = src[3][0], n = src[3][1], o = src[3][2]; + + t2 = j*o - n*k; + t4 = i*o - m*k; + t5 = i*n - m*j; + + dst[0][0] = f*k - g*j; + dst[1][0] =-(e*k - g*i); + dst[2][0] = e*j - f*i; + dst[3][0] =-(e*t2 - f*t4 + g*t5); + + dst[0][1] =-(b*k - c*j); + dst[1][1] = a*k - c*i; + dst[2][1] =-(a*j - b*i); + dst[3][1] = a*t2 - b*t4 + c*t5; + + t2 = f*o - n*g; + t4 = e*o - m*g; + t5 = e*n - m*f; + + dst[0][2] = b*g - c*f ; + dst[1][2] =-(a*g - c*e ); + dst[2][2] = a*f - b*e ; + dst[3][2] =-(a*t2 - b*t4 + c * t5); + + det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]); + v3_muls( dst[0], det, dst[0] ); + v3_muls( dst[1], det, dst[1] ); + v3_muls( dst[2], det, dst[2] ); + v3_muls( dst[3], det, dst[3] ); +} + +static inline void m4x3_copy( m4x3f a, m4x3f b ) +{ + v3_copy( a[0], b[0] ); + v3_copy( a[1], b[1] ); + v3_copy( a[2], b[2] ); + v3_copy( a[3], b[3] ); +} + +static inline void m4x3_identity( m4x3f a ) +{ + m4x3f id = M4X3_IDENTITY; + m4x3_copy( id, a ); +} + +static void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) +{ + float + a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], + a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], + a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], + a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], + b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], + b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], + b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], + b30 = b[3][0], b31 = b[3][1], b32 = b[3][2]; + + d[0][0] = a00*b00 + a10*b01 + a20*b02; + d[0][1] = a01*b00 + a11*b01 + a21*b02; + d[0][2] = a02*b00 + a12*b01 + a22*b02; + d[1][0] = a00*b10 + a10*b11 + a20*b12; + d[1][1] = a01*b10 + a11*b11 + a21*b12; + d[1][2] = a02*b10 + a12*b11 + a22*b12; + d[2][0] = a00*b20 + a10*b21 + a20*b22; + d[2][1] = a01*b20 + a11*b21 + a21*b22; + d[2][2] = a02*b20 + a12*b21 + a22*b22; + d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30; + d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31; + d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32; +} + +#if 0 /* shat appf mingw wstringop-overflow */ +inline +#endif +static void m4x3_mulv( m4x3f m, v3f v, v3f d ) +{ + v3f res; + + res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]; + res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]; + res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]; + + v3_copy( res, d ); +} + +/* + * Transform plane ( xyz, distance ) + */ +static void m4x3_mulp( m4x3f m, v4f p, v4f d ) +{ + v3f o; + + v3_muls( p, p[3], o ); + m4x3_mulv( m, o, o ); + m3x3_mulv( m, p, d ); + + d[3] = v3_dot( o, d ); +} + +/* + * Affine transforms + */ + +static void m4x3_translate( m4x3f m, v3f v ) +{ + v3_muladds( m[3], m[0], v[0], m[3] ); + v3_muladds( m[3], m[1], v[1], m[3] ); + v3_muladds( m[3], m[2], v[2], m[3] ); +} + +static void m4x3_rotate_x( m4x3f m, float angle ) +{ + m4x3f t = M4X3_IDENTITY; + float c, s; + + c = cosf( angle ); + s = sinf( angle ); + + t[1][1] = c; + t[1][2] = s; + t[2][1] = -s; + t[2][2] = c; + + m4x3_mul( m, t, m ); +} + +static void m4x3_rotate_y( m4x3f m, float angle ) +{ + m4x3f t = M4X3_IDENTITY; + float c, s; + + c = cosf( angle ); + s = sinf( angle ); + + t[0][0] = c; + t[0][2] = -s; + t[2][0] = s; + t[2][2] = c; + + m4x3_mul( m, t, m ); +} + +static void m4x3_rotate_z( m4x3f m, float angle ) +{ + m4x3f t = M4X3_IDENTITY; + float c, s; + + c = cosf( angle ); + s = sinf( angle ); + + t[0][0] = c; + t[0][1] = s; + t[1][0] = -s; + t[1][1] = c; + + m4x3_mul( m, t, m ); +} + +static void m4x3_expand( m4x3f m, m4x4f d ) +{ + v3_copy( m[0], d[0] ); + v3_copy( m[1], d[1] ); + v3_copy( m[2], d[2] ); + v3_copy( m[3], d[3] ); + d[0][3] = 0.0f; + d[1][3] = 0.0f; + d[2][3] = 0.0f; + d[3][3] = 1.0f; +} + +static void m4x3_decompose( m4x3f m, v3f co, v4f q, v3f s ) +{ + v3_copy( m[3], co ); + s[0] = v3_length(m[0]); + s[1] = v3_length(m[1]); + s[2] = v3_length(m[2]); + + m3x3f rot; + v3_divs( m[0], s[0], rot[0] ); + v3_divs( m[1], s[1], rot[1] ); + v3_divs( m[2], s[2], rot[2] ); + + m3x3_q( rot, q ); +} + +static void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ) +{ + v3f v; + m4x3_mulv( m, point, v ); + + v3_minv( box[0], v, box[0] ); + v3_maxv( box[1], v, box[1] ); +} + +static void m4x3_transform_aabb( m4x3f m, boxf box ) +{ + 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] } ); +} + +/* + * ----------------------------------------------------------------------------- + * Closest point functions + * ----------------------------------------------------------------------------- + */ + +/* + * These closest point tests were learned from Real-Time Collision Detection by + * Christer Ericson + */ +VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, + float *s, float *t, v3f c1, v3f c2) +{ + v3f d1,d2,r; + v3_sub( q1, p1, d1 ); + v3_sub( q2, p2, d2 ); + v3_sub( p1, p2, r ); + + float a = v3_length2( d1 ), + e = v3_length2( d2 ), + f = v3_dot( d2, r ); + + const float kEpsilon = 0.0001f; + + if( a <= kEpsilon && e <= kEpsilon ) + { + *s = 0.0f; + *t = 0.0f; + v3_copy( p1, c1 ); + v3_copy( p2, c2 ); + + v3f v0; + v3_sub( c1, c2, v0 ); + + return v3_length2( v0 ); + } + + if( a<= kEpsilon ) + { + *s = 0.0f; + *t = vg_clampf( f / e, 0.0f, 1.0f ); + } + else + { + float c = v3_dot( d1, r ); + if( e <= kEpsilon ) + { + *t = 0.0f; + *s = vg_clampf( -c / a, 0.0f, 1.0f ); + } + else + { + float b = v3_dot(d1,d2), + d = a*e-b*b; + + if( d != 0.0f ) + { + *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f); + } + else + { + *s = 0.0f; + } + + *t = (b*(*s)+f) / e; + + if( *t < 0.0f ) + { + *t = 0.0f; + *s = vg_clampf( -c / a, 0.0f, 1.0f ); + } + else if( *t > 1.0f ) + { + *t = 1.0f; + *s = vg_clampf((b-c)/a,0.0f,1.0f); + } + } + } + + v3_muladds( p1, d1, *s, c1 ); + v3_muladds( p2, d2, *t, c2 ); + + v3f v0; + v3_sub( c1, c2, v0 ); + 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 ); + v3_minv( dest, box[1], dest ); +} + +VG_STATIC void closest_point_obb( v3f p, boxf box, + m4x3f mtx, m4x3f inv_mtx, v3f dest ) +{ + v3f local; + m4x3_mulv( inv_mtx, p, local ); + closest_point_aabb( local, box, local ); + m4x3_mulv( mtx, local, dest ); +} + +VG_STATIC float closest_point_segment( v3f a, v3f b, v3f point, v3f dest ) +{ + v3f v0, v1; + v3_sub( b, a, v0 ); + v3_sub( point, a, v1 ); + + float t = v3_dot( v1, v0 ) / v3_length2(v0); + t = vg_clampf(t,0.0f,1.0f); + v3_muladds( a, v0, t, dest ); + return t; +} + +VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) +{ + v3f ab, ac, ap; + float d1, d2; + + /* Region outside A */ + v3_sub( tri[1], tri[0], ab ); + v3_sub( tri[2], tri[0], ac ); + v3_sub( p, tri[0], ap ); + + d1 = v3_dot(ab,ap); + d2 = v3_dot(ac,ap); + if( d1 <= 0.0f && d2 <= 0.0f ) + { + v3_copy( tri[0], dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region outside B */ + v3f bp; + float d3, d4; + + v3_sub( p, tri[1], bp ); + d3 = v3_dot( ab, bp ); + d4 = v3_dot( ac, bp ); + + if( d3 >= 0.0f && d4 <= d3 ) + { + v3_copy( tri[1], dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Edge region of AB */ + float vc = d1*d4 - d3*d2; + if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + float v = d1 / (d1-d3); + v3_muladds( tri[0], ab, v, dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region outside C */ + v3f cp; + float d5, d6; + v3_sub( p, tri[2], cp ); + d5 = v3_dot(ab, cp); + d6 = v3_dot(ac, cp); + + if( d6 >= 0.0f && d5 <= d6 ) + { + v3_copy( tri[2], dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region of AC */ + float vb = d5*d2 - d1*d6; + if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) + { + float w = d2 / (d2-d6); + v3_muladds( tri[0], ac, w, dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* Region of BC */ + float va = d3*d6 - d5*d4; + if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) + { + float w = (d4-d3) / ((d4-d3) + (d5-d6)); + v3f bc; + v3_sub( tri[2], tri[1], bc ); + v3_muladds( tri[1], bc, w, dest ); + v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); + return; + } + + /* P inside region, Q via barycentric coordinates uvw */ + float d = 1.0f/(va+vb+vc), + v = vb*d, + w = vc*d; + + v3_muladds( tri[0], ab, v, dest ); + v3_muladds( dest, ac, w, dest ); +} + +VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) +{ + v3f ab, ac, ap; + float d1, d2; + + /* Region outside A */ + v3_sub( tri[1], tri[0], ab ); + v3_sub( tri[2], tri[0], ac ); + v3_sub( p, tri[0], ap ); + + d1 = v3_dot(ab,ap); + d2 = v3_dot(ac,ap); + if( d1 <= 0.0f && d2 <= 0.0f ) + { + v3_copy( tri[0], dest ); + return k_contact_type_default; + } + + /* Region outside B */ + v3f bp; + float d3, d4; + + v3_sub( p, tri[1], bp ); + d3 = v3_dot( ab, bp ); + d4 = v3_dot( ac, bp ); + + if( d3 >= 0.0f && d4 <= d3 ) + { + v3_copy( tri[1], dest ); + return k_contact_type_edge; + } + + /* Edge region of AB */ + float vc = d1*d4 - d3*d2; + if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) + { + float v = d1 / (d1-d3); + v3_muladds( tri[0], ab, v, dest ); + return k_contact_type_edge; + } + + /* Region outside C */ + v3f cp; + float d5, d6; + v3_sub( p, tri[2], cp ); + d5 = v3_dot(ab, cp); + d6 = v3_dot(ac, cp); + + if( d6 >= 0.0f && d5 <= d6 ) + { + v3_copy( tri[2], dest ); + return k_contact_type_edge; + } + + /* Region of AC */ + float vb = d5*d2 - d1*d6; + if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) + { + float w = d2 / (d2-d6); + v3_muladds( tri[0], ac, w, dest ); + return k_contact_type_edge; + } + + /* Region of BC */ + float va = d3*d6 - d5*d4; + if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f ) + { + float w = (d4-d3) / ((d4-d3) + (d5-d6)); + v3f bc; + v3_sub( tri[2], tri[1], bc ); + v3_muladds( tri[1], bc, w, dest ); + return k_contact_type_edge; + } + + /* P inside region, Q via barycentric coordinates uvw */ + float d = 1.0f/(va+vb+vc), + v = vb*d, + w = vc*d; + + v3_muladds( tri[0], ab, v, dest ); + v3_muladds( dest, ac, w, 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 + */ + +/* Time of intersection with ray vs triangle */ +static int ray_tri( v3f tri[3], v3f co, + v3f dir, float *dist ) { float const kEpsilon = 0.00001f; @@ -1374,6 +1876,7 @@ static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist ) /* Parralel */ a = v3_dot( v0, h ); + if( a > -kEpsilon && a < kEpsilon ) return 0; @@ -1398,8 +1901,165 @@ static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist ) else return 0; } +/* time of intersection with ray vs sphere */ +static int ray_sphere( v3f c, float r, + v3f co, v3f dir, float *t ) +{ + v3f m; + v3_sub( co, c, m ); + + float b = v3_dot( m, dir ), + c1 = v3_dot( m, m ) - r*r; + + /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */ + if( c1 > 0.0f && b > 0.0f ) + return 0; + + float discr = b*b - c1; + + /* A negative discriminant corresponds to ray missing sphere */ + if( discr < 0.0f ) + return 0; + + /* + * Ray now found to intersect sphere, compute smallest t value of + * intersection + */ + *t = -b - sqrtf( discr ); + + /* If t is negative, ray started inside sphere so clamp t to zero */ + if( *t < 0.0f ) + *t = 0.0f; + + return 1; +} + +/* + * time of intersection of ray vs cylinder + * The cylinder does not have caps but is finite + * + * Heavily adapted from regular segment vs cylinder from: + * Real-Time Collision Detection + */ +static int ray_uncapped_finite_cylinder( v3f q, v3f p, float r, + v3f co, v3f dir, float *t ) +{ + v3f d, m, n, sb; + v3_muladds( co, dir, 1.0f, sb ); + + v3_sub( q, p, d ); + v3_sub( co, p, m ); + v3_sub( sb, co, n ); + + float md = v3_dot( m, d ), + nd = v3_dot( n, d ), + dd = v3_dot( d, d ), + nn = v3_dot( n, n ), + mn = v3_dot( m, n ), + a = dd*nn - nd*nd, + k = v3_dot( m, m ) - r*r, + c = dd*k - md*md; + + if( fabsf(a) < 0.00001f ) + { + /* Segment runs parallel to cylinder axis */ + return 0; + } + + float b = dd*mn - nd*md, + discr = b*b - a*c; + + if( discr < 0.0f ) + return 0; /* No real roots; no intersection */ + + *t = (-b - sqrtf(discr)) / a; + if( *t < 0.0f ) + return 0; /* Intersection behind ray */ + + /* Check within cylinder segment */ + if( md + (*t)*nd < 0.0f ) + return 0; + + if( md + (*t)*nd > dd ) + return 0; + + /* Segment intersects cylinder between the endcaps; t is correct */ + return 1; +} + +/* + * Time of intersection of sphere and triangle. Origin must be outside the + * colliding area. This is a fairly long procedure. + */ +static int spherecast_triangle( v3f tri[3], + v3f co, v3f dir, float r, float *t, v3f n ) +{ + v3f sum[3]; + v3f v0, v1; + + v3_sub( tri[1], tri[0], v0 ); + v3_sub( tri[2], tri[0], v1 ); + v3_cross( v0, v1, n ); + v3_normalize( n ); + v3_muladds( tri[0], n, r, sum[0] ); + v3_muladds( tri[1], n, r, sum[1] ); + v3_muladds( tri[2], n, r, sum[2] ); + + int hit = 0; + float t_min = INFINITY, + t1; + + if( ray_tri( sum, co, dir, &t1 ) ) + { + t_min = vg_minf( t_min, t1 ); + hit = 1; + } + + /* + * Currently disabled; ray_sphere requires |d| = 1. it is not very important. + */ +#if 0 + for( int i=0; i<3; i++ ) + { + if( ray_sphere( tri[i], r, co, dir, &t1 ) ) + { + t_min = vg_minf( t_min, t1 ); + hit = 1; + } + } +#endif + + for( int i=0; i<3; i++ ) + { + int i0 = i, + i1 = (i+1)%3; + + if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) ) + { + if( t1 < t_min ) + { + t_min = t1; + + v3f co1, ct, cx; + v3_add( dir, co, co1 ); + v3_lerp( co, co1, t_min, ct ); + + closest_point_segment( tri[i0], tri[i1], ct, cx ); + v3_sub( ct, cx, n ); + v3_normalize( n ); + } + + hit = 1; + } + } + + *t = t_min; + return hit; +} + static inline float vg_randf(void) { + /* TODO: replace with our own rand */ return (float)rand()/(float)(RAND_MAX); } @@ -1437,4 +2097,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 */