X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=src%2Fvg%2Fvg_m.h;h=0736752364113b85a711349926d3df5208e22f2a;hb=9917df2d289c6c35d6bf54319aed3aed421b53f1;hp=9f934163e42bd7c5204566eeb11eb637f93e4206;hpb=f9b8c958c6221365d88a248e645aaebb6f7f1b90;p=vg.git diff --git a/src/vg/vg_m.h b/src/vg/vg_m.h index 9f93416..0736752 100644 --- a/src/vg/vg_m.h +++ b/src/vg/vg_m.h @@ -1,4 +1,11 @@ -/* Copyright (C) 2021 Harry Godden (hgn) - All Rights Reserved */ +/* Copyright (C) 2021-2022 Harry Godden (hgn) - All Rights Reserved */ + +#ifndef VG_M_H +#define VG_M_H + +#include "vg_platform.h" +#include +#include #define VG_PIf 3.14159265358979323846264338327950288f #define VG_TAUf 6.28318530717958647692528676655900576f @@ -23,18 +30,20 @@ static inline float vg_signf( float a ) return a < 0.0f? -1.0f: 1.0f; } -static inline float vg_randf(void) +static inline float vg_fractf( float a ) { - return (float)rand()/(float)(RAND_MAX); + return a - floorf( a ); } -static inline float vg_randint(int max) +static float stable_force( float current, float diff ) { - return rand()%max; -} + float fnew = current + diff; -#define VG_MIN( A, B ) ((A)<(B)?(A):(B)) -#define VG_MAX( A, B ) ((A)>(B)?(A):(B)) + if( fnew * current < 0.0f ) + return 0.0f; + + return fnew; +} static inline int vg_min( int a, int b ) { @@ -59,6 +68,11 @@ static inline void v2_copy( v2f a, v2f b ) b[0] = a[0]; b[1] = a[1]; } +static inline void v2_zero( v2f a ) +{ + a[0] = 0.f; a[1] = 0.f; +} + static inline void v2i_copy( v2i a, v2i b ) { b[0] = a[0]; b[1] = a[1]; @@ -103,7 +117,7 @@ static inline float v2_dot( v2f a, v2f b ) static inline float v2_cross( v2f a, v2f b ) { - return a[0] * b[1] - a[1] * b[0]; + return a[0]*b[1] - a[1]*b[0]; } static inline void v2_add( v2f a, v2f b, v2f d ) @@ -177,6 +191,12 @@ static inline void v2_normalize( v2f a ) v2_muls( a, 1.f / v2_length( a ), a ); } +static inline void v2_floor( v2f a, v2f b ) +{ + b[0] = floorf( a[0] ); + b[1] = floorf( a[1] ); +} + /* * Vector 3 */ @@ -237,11 +257,13 @@ static inline float v3_dot( v3f a, v3f b ) return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; } -static inline void v3_cross( v3f a, v3f b, v3f d ) +static inline void v3_cross( v3f a, v3f b, v3f dest ) { - d[0] = a[1] * b[2] - a[2] * b[1]; - d[1] = a[2] * b[0] - a[0] * b[2]; - d[2] = a[0] * b[1] - a[1] * b[0]; + v3f d; + d[0] = a[1]*b[2] - a[2]*b[1]; + d[1] = a[2]*b[0] - a[0]*b[2]; + d[2] = a[0]*b[1] - a[1]*b[0]; + v3_copy( d, dest ); } static inline float v3_length2( v3f a ) @@ -276,6 +298,19 @@ static inline float vg_lerpf( float a, float b, float t ) return a + t*(b-a); } +static inline double vg_lerp( double a, double b, double t ) +{ + return a + t*(b-a); +} + +/* correctly lerp around circular period -pi -> pi */ +static float vg_alerpf( float a, float b, float t ) +{ + float d = fmodf( b-a, VG_TAUf ), + s = fmodf( 2.0f*d, VG_TAUf ) - d; + return a + s*t; +} + static inline void v3_lerp( v3f a, v3f b, float t, v3f d ) { d[0] = a[0] + t*(b[0]-a[0]); @@ -366,7 +401,15 @@ static inline void v4_zero( v4f a ) a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f; } -static inline void v4_muladds( v3f a, v3f b, float s, v3f d ) +static inline void v4_muls( v4f a, float s, v4f d ) +{ + d[0] = a[0]*s; + d[1] = a[1]*s; + d[2] = a[2]*s; + d[3] = a[3]*s; +} + +static inline void v4_muladds( v4f a, v4f b, float s, v4f d ) { d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; @@ -605,6 +648,45 @@ static inline void m4x3_invert_affine( m4x3f a, m4x3f b ) 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] ); @@ -656,6 +738,20 @@ static inline void m4x3_mulv( m4x3f m, v3f v, v3f d ) 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 */ @@ -674,6 +770,13 @@ static inline void m4x3_scale( m4x3f m, float s ) 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; @@ -743,6 +846,12 @@ static inline void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ) v3_maxv( box[1], v, box[1] ); } +static inline void box_addpt( boxf a, v3f pt ) +{ + v3_minv( a[0], pt, a[0] ); + v3_maxv( a[1], pt, a[1] ); +} + static inline void box_concat( boxf a, boxf b ) { v3_minv( a[0], b[0], a[0] ); @@ -755,6 +864,21 @@ static inline void box_copy( boxf a, boxf b ) v3_copy( a[1], b[1] ); } +static inline int box_overlap( boxf a, boxf b ) +{ + return + ( a[0][0] <= b[1][0] && a[1][0] >= b[0][0] ) && + ( a[0][1] <= b[1][1] && a[1][1] >= b[0][1] ) && + ( a[0][2] <= b[1][2] && a[1][2] >= b[0][2] ) + ; +} + +static inline void box_init_inf( boxf box ) +{ + v3_fill( box[0], INFINITY ); + v3_fill( box[1], -INFINITY ); +} + static inline void m4x3_transform_aabb( m4x3f m, boxf box ) { v3f a; v3f b; @@ -764,14 +888,35 @@ static inline void m4x3_transform_aabb( m4x3f m, boxf box ) v3_fill( box[0], INFINITY ); v3_fill( box[1], -INFINITY ); - m4x3_expand_aabb_point( m, box, a ); + 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], a[1], a[2] } ); m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], a[2] } ); - m4x3_expand_aabb_point( m, box, b ); + 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], a[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; + + v3_sub( box[0], co, v0 ); + v3_sub( box[1], co, v1 ); + v3_div( v0, dir, v0 ); + v3_div( v1, dir, v1 ); + + tmin = vg_minf( v0[0], v1[0] ); + tmax = vg_maxf( v0[0], v1[0] ); + tmin = vg_maxf( tmin, vg_minf( v0[1], v1[1] )); + tmax = vg_minf( tmax, vg_maxf( v0[1], v1[1] )); + 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; } static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up ) @@ -780,7 +925,7 @@ static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up ) v3_sub( target, pos, dir ); v3_normalize( dir ); - v3_negate( dir, m[2] ); + v3_copy( dir, m[2] ); v3_cross( up, m[2], m[0] ); v3_normalize( m[0] ); @@ -797,31 +942,35 @@ static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up ) { 0.0f, 1.0f, 0.0f, 0.0f },\ { 0.0f, 0.0f, 1.0f, 0.0f },\ { 0.0f, 0.0f, 0.0f, 1.0f }} +#define M4X4_ZERO {{0.0f, 0.0f, 0.0f, 0.0f },\ + { 0.0f, 0.0f, 0.0f, 0.0f },\ + { 0.0f, 0.0f, 0.0f, 0.0f },\ + { 0.0f, 0.0f, 0.0f, 0.0f }} static void m4x4_projection( m4x4f m, float angle, - float ratio, float near, float far ) + float ratio, float fnear, float ffar ) { - float scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * near, + float scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear, r = ratio * scale, l = -r, t = scale, b = -t; - m[0][0] = 2.0f * near / (r - l); + m[0][0] = 2.0f * fnear / (r - l); m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f; m[1][0] = 0.0f; - m[1][1] = 2.0f * near / (t - b); + m[1][1] = 2.0f * fnear / (t - b); m[1][2] = 0.0f; m[1][3] = 0.0f; m[2][0] = (r + l) / (r - l); m[2][1] = (t + b) / (t - b); - m[2][2] = -(far + near) / (far - near); + m[2][2] = -(ffar + fnear) / (ffar - fnear); m[2][3] = -1.0f; m[3][0] = 0.0f; m[3][1] = 0.0f; - m[3][2] = -2.0f * far * near / (far - near); + m[3][2] = -2.0f * ffar * fnear / (ffar - fnear); m[3][3] = 0.0f; } @@ -846,6 +995,12 @@ static inline void m4x4_identity( m4x4f a ) m4x4_copy( id, a ); } +static inline void m4x4_zero( m4x4f a ) +{ + m4x4f zero = M4X4_ZERO; + m4x4_copy( zero, a ); +} + static inline void m4x4_mul( m4x4f a, m4x4f b, m4x4f d ) { float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3], @@ -876,6 +1031,75 @@ static inline void m4x4_mul( m4x4f a, m4x4f b, m4x4f d ) d[3][3] = a03*b30 + a13*b31 + a23*b32 + a33*b33; } +static inline void m4x4_mulv( m4x4f m, v4f v, v4f d ) +{ + v4f res; + + res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]*v[3]; + res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]*v[3]; + res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]*v[3]; + res[3] = m[0][3]*v[0] + m[1][3]*v[1] + m[2][3]*v[2] + m[3][3]*v[3]; + + v4_copy( res, d ); +} + +static inline void m4x4_inv( m4x4f a, m4x4f d ) +{ + float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3], + a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3], + a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3], + a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3], + det, + t[6]; + + t[0] = a22*a33 - a32*a23; + t[1] = a21*a33 - a31*a23; + t[2] = a21*a32 - a31*a22; + t[3] = a20*a33 - a30*a23; + t[4] = a20*a32 - a30*a22; + t[5] = a20*a31 - a30*a21; + + d[0][0] = a11*t[0] - a12*t[1] + a13*t[2]; + d[1][0] =-(a10*t[0] - a12*t[3] + a13*t[4]); + d[2][0] = a10*t[1] - a11*t[3] + a13*t[5]; + d[3][0] =-(a10*t[2] - a11*t[4] + a12*t[5]); + + d[0][1] =-(a01*t[0] - a02*t[1] + a03*t[2]); + d[1][1] = a00*t[0] - a02*t[3] + a03*t[4]; + d[2][1] =-(a00*t[1] - a01*t[3] + a03*t[5]); + d[3][1] = a00*t[2] - a01*t[4] + a02*t[5]; + + t[0] = a12*a33 - a32*a13; + t[1] = a11*a33 - a31*a13; + t[2] = a11*a32 - a31*a12; + t[3] = a10*a33 - a30*a13; + t[4] = a10*a32 - a30*a12; + t[5] = a10*a31 - a30*a11; + + d[0][2] = a01*t[0] - a02*t[1] + a03*t[2]; + d[1][2] =-(a00*t[0] - a02*t[3] + a03*t[4]); + d[2][2] = a00*t[1] - a01*t[3] + a03*t[5]; + d[3][2] =-(a00*t[2] - a01*t[4] + a02*t[5]); + + t[0] = a12*a23 - a22*a13; + t[1] = a11*a23 - a21*a13; + t[2] = a11*a22 - a21*a12; + t[3] = a10*a23 - a20*a13; + t[4] = a10*a22 - a20*a12; + t[5] = a10*a21 - a20*a11; + + d[0][3] =-(a01*t[0] - a02*t[1] + a03*t[2]); + d[1][3] = a00*t[0] - a02*t[3] + a03*t[4]; + d[2][3] =-(a00*t[1] - a01*t[3] + a03*t[5]); + d[3][3] = a00*t[2] - a01*t[4] + a02*t[5]; + + det = 1.0f / (a00*d[0][0] + a01*d[1][0] + a02*d[2][0] + a03*d[3][0]); + v4_muls( d[0], det, d[0] ); + v4_muls( d[1], det, d[1] ); + v4_muls( d[2], det, d[2] ); + v4_muls( d[3], det, d[3] ); +} + /* * Planes (double precision) */ @@ -996,6 +1220,19 @@ static inline void q_inv( v4f q, v4f d ) d[3] = q[3]*s; } +static inline void q_nlerp( v4f a, v4f b, float t, v4f d ) +{ + if( v4_dot(a,b) < 0.0f ) + { + v4_muls( b, -1.0f, d ); + v4_lerp( a, d, t, d ); + } + else + v4_lerp( a, b, t, d ); + + q_normalize( d ); +} + static inline void q_m3x3( v4f q, m3x3f d ) { float @@ -1016,3 +1253,132 @@ static inline void q_m3x3( v4f q, m3x3f d ) d[2][1] = yz - wx; d[0][2] = xz - wy; } + +static void m3x3_q( m3x3f m, v4f q ) +{ + float diag, r, rinv; + + diag = m[0][0] + m[1][1] + m[2][2]; + if( diag >= 0.0f ) + { + r = sqrtf( 1.0f + diag ); + rinv = 0.5f / r; + q[0] = rinv * (m[1][2] - m[2][1]); + q[1] = rinv * (m[2][0] - m[0][2]); + q[2] = rinv * (m[0][1] - m[1][0]); + q[3] = r * 0.5f; + } + else if( m[0][0] >= m[1][1] && m[0][0] >= m[2][2] ) + { + r = sqrtf( 1.0f - m[1][1] - m[2][2] + m[0][0] ); + rinv = 0.5f / r; + q[0] = r * 0.5f; + q[1] = rinv * (m[0][1] + m[1][0]); + q[2] = rinv * (m[0][2] + m[2][0]); + q[3] = rinv * (m[1][2] - m[2][1]); + } + else if( m[1][1] >= m[2][2] ) + { + r = sqrtf( 1.0f - m[0][0] - m[2][2] + m[1][1] ); + rinv = 0.5f / r; + q[0] = rinv * (m[0][1] + m[1][0]); + q[1] = r * 0.5f; + q[2] = rinv * (m[1][2] + m[2][1]); + q[3] = rinv * (m[2][0] - m[0][2]); + } + else + { + r = sqrtf( 1.0f - m[0][0] - m[1][1] + m[2][2] ); + rinv = 0.5f / r; + q[0] = rinv * (m[0][2] + m[2][0]); + q[1] = rinv * (m[1][2] + m[2][1]); + q[2] = r * 0.5f; + q[3] = rinv * (m[0][1] - m[1][0]); + } +} + +static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist ) +{ + float const kEpsilon = 0.00001f; + + v3f v0, v1, h, s, q, n; + float a,f,u,v,t; + + float *pa = tri[0], + *pb = tri[1], + *pc = tri[2]; + + v3_sub( pb, pa, v0 ); + v3_sub( pc, pa, v1 ); + v3_cross( dir, v1, h ); + v3_cross( v0, v1, n ); + + if( v3_dot( n, dir ) > 0.0f ) /* Backface culling */ + return 0; + + /* Parralel */ + a = v3_dot( v0, h ); + if( a > -kEpsilon && a < kEpsilon ) + return 0; + + f = 1.0f/a; + v3_sub( co, pa, s ); + + u = f * v3_dot(s, h); + if( u < 0.0f || u > 1.0f ) + return 0; + + v3_cross( s, v0, q ); + v = f * v3_dot( dir, q ); + if( v < 0.0f || u+v > 1.0f ) + return 0; + + t = f * v3_dot(v1, q); + if( t > kEpsilon ) + { + *dist = t; + return 1; + } + else return 0; +} + +static inline float vg_randf(void) +{ + return (float)rand()/(float)(RAND_MAX); +} + +static inline void vg_rand_dir(v3f dir) +{ + dir[0] = vg_randf(); + dir[1] = vg_randf(); + dir[2] = vg_randf(); + + v3_muls( dir, 2.0f, dir ); + v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir ); + + v3_normalize( dir ); +} + +static inline void vg_rand_sphere( v3f co ) +{ + vg_rand_dir(co); + v3_muls( co, cbrtf( vg_randf() ), co ); +} + +static inline int vg_randint(int max) +{ + return rand()%max; +} + +static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p ) +{ + float tt = t*t, + ttt = tt*t; + + v3_muls( p1, ttt, p ); + v3_muladds( p, h1, 3.0f*tt -3.0f*ttt, p ); + v3_muladds( p, h0, 3.0f*ttt -6.0f*tt +3.0f*t, p ); + v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p ); +} + +#endif /* VG_M_H */