X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_m.h;h=368f2889aff1f23807325e92f11228f22c654305;hp=ca3308b959be06bb9156666ef6814b50a160e43d;hb=HEAD;hpb=40382fa2a3d250ef1e0f1716f3608d0d60e0e655 diff --git a/vg_m.h b/vg_m.h index ca3308b..4af60c8 100644 --- a/vg_m.h +++ b/vg_m.h @@ -1,7 +1,30 @@ -/* Copyright (C) 2021-2022 Harry Godden (hgn) - All Rights Reserved */ +/* Copyright (C) 2021-2024 Harry Godden (hgn) - All Rights Reserved + * + * 0. Misc + * 1. Scalar operations + * 2. Vectors + * 2.a 2D Vectors + * 2.b 3D Vectors + * 2.c 4D Vectors + * 3. Quaternions + * 4. Matrices + * 4.a 2x2 matrices + * 4.b 3x3 matrices + * 4.c 4x3 matrices + * 4.d 4x4 matrices + * 5. Geometry + * 5.a Boxes + * 5.b Planes + * 5.c Closest points + * 5.d Raycast & Spherecasts + * 5.e Curves + * 5.f Volumes + * 5.g Inertia tensors + * 6. Statistics + * 6.a Random numbers + */ -#ifndef VG_M_H -#define VG_M_H +#pragma once #include "vg_platform.h" #include @@ -10,113 +33,141 @@ #define VG_PIf 3.14159265358979323846264338327950288f #define VG_TAUf 6.28318530717958647692528676655900576f -static u32 vg_ftu32( float a ) +/* + * ----------------------------------------------------------------------------- + * Section 0. Misc Operations + * ----------------------------------------------------------------------------- + */ + +/* get the f32 as the raw bits in a u32 without converting */ +static u32 vg_ftu32( f32 a ) { u32 *ptr = (u32 *)(&a); return *ptr; } -static int vg_isinff( float a ) +/* check if f32 is infinite */ +static int vg_isinff( f32 a ) { return ((vg_ftu32(a)) & 0x7FFFFFFFU) == 0x7F800000U; } -static int vg_isnanf( float a ) +/* check if f32 is not a number */ +static int vg_isnanf( f32 a ) { return !vg_isinff(a) && ((vg_ftu32(a)) & 0x7F800000U) == 0x7F800000U; } -static int vg_validf( float a ) +/* check if f32 is a number and is not infinite */ +static int vg_validf( f32 a ) { return ((vg_ftu32(a)) & 0x7F800000U) != 0x7F800000U; } -static inline float vg_minf( float a, float b ) -{ - return a < b? a: b; +static int v3_valid( v3f a ){ + for( u32 i=0; i<3; i++ ) + if( !vg_validf(a[i]) ) return 0; + return 1; } -static inline float vg_maxf( float a, float b ) -{ - return a > b? a: b; -} +/* + * ----------------------------------------------------------------------------- + * Section 1. Scalar Operations + * ----------------------------------------------------------------------------- + */ + +static inline f32 vg_minf( f32 a, f32 b ){ return a < b? a: b; } +static inline f32 vg_maxf( f32 a, f32 b ){ return a > b? a: b; } -static inline float vg_clampf( float a, float min, float max ) +static inline int vg_min( int a, int b ){ return a < b? a: b; } +static inline int vg_max( int a, int b ){ return a > b? a: b; } + +static inline f32 vg_clampf( f32 a, f32 min, f32 max ) { return vg_minf( max, vg_maxf( a, min ) ); } -static inline float vg_signf( float a ) +static inline f32 vg_signf( f32 a ) { return a < 0.0f? -1.0f: 1.0f; } -static inline float vg_fractf( float a ) +static inline f32 vg_fractf( f32 a ) { return a - floorf( a ); } - -__attribute__ ((deprecated)) -static float stable_force( float current, float diff ) -{ - float fnew = current + diff; - - if( fnew * current < 0.0f ) - return 0.0f; - - return fnew; +static inline f64 vg_fractf64( f64 a ){ + return a - floor( a ); } -static float vg_cfrictf( float current, float F ) +static f32 vg_cfrictf( f32 velocity, f32 F ) { - return -vg_signf(current) * vg_minf( F, fabsf(current) ); + return -vg_signf(velocity) * vg_minf( F, fabsf(velocity) ); } -static inline int vg_min( int a, int b ) +static inline f32 vg_rad( f32 deg ) { - return a < b? a: b; + return deg * VG_PIf / 180.0f; } -static inline int vg_max( int a, int b ) -{ - return a > b? a: b; +/* angle to reach b from a */ +static f32 vg_angle_diff( f32 a, f32 b ){ + f32 d = fmod(b,VG_TAUf)-fmodf(a,VG_TAUf); + if( fabsf(d) > VG_PIf ) + d = -vg_signf(d) * (VG_TAUf - fabsf(d)); + + return d; } -static inline float vg_rad( float deg ) -{ - return deg * VG_PIf / 180.0f; +/* + * quantize float to bit count + */ +static u32 vg_quantf( f32 a, u32 bits, f32 min, f32 max ){ + u32 mask = (0x1 << bits) - 1; + return vg_clampf((a - min) * ((f32)mask/(max-min)), 0.0f, mask ); } /* - * Vector 3 + * un-quantize discreet to float */ -static inline void v2_copy( v2f a, v2f b ) -{ - b[0] = a[0]; b[1] = a[1]; +static f32 vg_dequantf( u32 q, u32 bits, f32 min, f32 max ){ + u32 mask = (0x1 << bits) - 1; + return min + (f32)q * ((max-min) / (f32)mask); } -static inline void v2_zero( v2f a ) -{ - a[0] = 0.f; a[1] = 0.f; +/* https://iquilezles.org/articles/functions/ + * + * Use k to control the stretching of the function. Its maximum, which is 1, + * happens at exactly x = 1/k. + */ +static f32 vg_exp_impulse( f32 x, f32 k ){ + f32 h = k*x; + return h*expf(1.0f-h); } -static inline void v2i_copy( v2i a, v2i b ) +/* + * ----------------------------------------------------------------------------- + * Section 2.a 2D Vectors + * ----------------------------------------------------------------------------- + */ + +static inline void v2_copy( v2f a, v2f d ) { - b[0] = a[0]; b[1] = a[1]; + d[0] = a[0]; d[1] = a[1]; } -static inline int v2i_eq( v2i a, v2i b ) +static inline void v2_zero( v2f a ) { - return ((a[0] == b[0]) && (a[1] == b[1])); + a[0] = 0.f; a[1] = 0.f; } -static inline void v2i_add( v2i a, v2i b, v2i d ) +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 v2i_sub( v2i a, v2i b, v2i d ) +static inline void v2_sub( v2f a, v2f b, v2f d ) { d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; } @@ -133,38 +184,28 @@ static inline void v2_maxv( v2f a, v2f b, v2f dest ) dest[1] = vg_maxf(a[1], b[1]); } -static inline void v2_sub( v2f a, v2f b, v2f d ) -{ - d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; -} - -static inline float v2_dot( v2f a, v2f b ) +static inline f32 v2_dot( v2f a, v2f b ) { return a[0] * b[0] + a[1] * b[1]; } -static inline float v2_cross( v2f a, v2f b ) +static inline f32 v2_cross( v2f a, v2f b ) { return a[0]*b[1] - a[1]*b[0]; } -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 ) +static inline void v2_muls( v2f a, f32 s, v2f d ) { d[0] = a[0]*s; d[1] = a[1]*s; } -static inline void v2_divs( v2f a, float s, v2f d ) +static inline void v2_divs( v2f a, f32 s, v2f d ) { d[0] = a[0]/s; d[1] = a[1]/s; } @@ -186,35 +227,35 @@ static inline void v2_muladd( v2f a, v2f b, v2f s, v2f d ) d[1] = a[1]+b[1]*s[1]; } -static inline void v2_muladds( v2f a, v2f b, float s, v2f d ) +static inline void v2_muladds( v2f a, v2f b, f32 s, v2f d ) { d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; } -static inline float v2_length2( v2f a ) +static inline f32 v2_length2( v2f a ) { return a[0]*a[0] + a[1]*a[1]; } -static inline float v2_length( v2f a ) +static inline f32 v2_length( v2f a ) { return sqrtf( v2_length2( a ) ); } -static inline float v2_dist2( v2f a, v2f b ) +static inline f32 v2_dist2( v2f a, v2f b ) { v2f delta; v2_sub( a, b, delta ); return v2_length2( delta ); } -static inline float v2_dist( v2f a, v2f b ) +static inline f32 v2_dist( v2f a, v2f b ) { return sqrtf( v2_dist2( a, b ) ); } -static inline void v2_lerp( v2f a, v2f b, float t, v2f d ) +static inline void v2_lerp( v2f a, v2f b, f32 t, v2f d ) { d[0] = a[0] + t*(b[0]-a[0]); d[1] = a[1] + t*(b[1]-a[1]); @@ -227,7 +268,7 @@ static inline void v2_normalize( v2f a ) static void v2_normalize_clamp( v2f a ) { - float l2 = v2_length2( a ); + f32 l2 = v2_length2( a ); if( l2 > 1.0f ) v2_muls( a, 1.0f/sqrtf(l2), a ); } @@ -238,48 +279,65 @@ static inline void v2_floor( v2f a, v2f b ) b[1] = floorf( a[1] ); } -static inline void v2_fill( v2f a, float v ) +static inline void v2_fill( v2f a, f32 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 - */ -static inline void v3_zero( v3f a ) +/* integer variants + * ---------------- */ + +static inline void v2i_copy( v2i a, v2i b ) { - a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; + b[0] = a[0]; b[1] = a[1]; } +static inline int v2i_eq( v2i a, v2i b ) +{ + return ((a[0] == b[0]) && (a[1] == b[1])); +} + +static inline void v2i_add( v2i a, v2i b, v2i d ) +{ + d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; +} + +static inline void v2i_sub( v2i a, v2i b, v2i d ) +{ + d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; +} + +/* + * ----------------------------------------------------------------------------- + * Section 2.b 3D Vectors + * ----------------------------------------------------------------------------- + */ + static inline void v3_copy( v3f a, v3f b ) { b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; } -static inline void v3_add( v3f a, v3f b, v3f d ) +static inline void v3_zero( v3f a ) { - d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2]; + a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; } -static inline void v3i_add( v3i a, v3i b, v3i d ) +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 v4_add( v4f a, v4f b, v4f d ) +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]; - d[3] = a[3]+b[3]; + d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2]; } static inline void v3_sub( v3f a, v3f b, v3f d ) @@ -304,19 +362,19 @@ static inline void v3_div( v3f a, v3f b, v3f d ) d[2] = b[2]!=0.0f? a[2]/b[2]: INFINITY; } -static inline void v3_muls( v3f a, float s, v3f d ) +static inline void v3_muls( v3f a, f32 s, v3f d ) { d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s; } -static inline void v3_fill( v3f a, float v ) +static inline void v3_fill( v3f a, f32 v ) { a[0] = v; a[1] = v; a[2] = v; } -static inline void v3_divs( v3f a, float s, v3f d ) +static inline void v3_divs( v3f a, f32 s, v3f d ) { if( s == 0.0f ) v3_fill( d, INFINITY ); @@ -328,7 +386,7 @@ static inline void v3_divs( v3f a, float s, v3f d ) } } -static inline void v3_muladds( v3f a, v3f b, float s, v3f d ) +static inline void v3_muladds( v3f a, v3f b, f32 s, v3f d ) { d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; d[2] = a[2]+b[2]*s; } @@ -340,7 +398,7 @@ static inline void v3_muladd( v2f a, v2f b, v2f s, v2f d ) d[2] = a[2]+b[2]*s[2]; } -static inline float v3_dot( v3f a, v3f b ) +static inline f32 v3_dot( v3f a, v3f b ) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; } @@ -354,24 +412,24 @@ static inline void v3_cross( v3f a, v3f b, v3f dest ) v3_copy( d, dest ); } -static inline float v3_length2( v3f a ) +static inline f32 v3_length2( v3f a ) { return v3_dot( a, a ); } -static inline float v3_length( v3f a ) +static inline f32 v3_length( v3f a ) { return sqrtf( v3_length2( a ) ); } -static inline float v3_dist2( v3f a, v3f b ) +static inline f32 v3_dist2( v3f a, v3f b ) { v3f delta; v3_sub( a, b, delta ); return v3_length2( delta ); } -static inline float v3_dist( v3f a, v3f b ) +static inline f32 v3_dist( v3f a, v3f b ) { return sqrtf( v3_dist2( a, b ) ); } @@ -381,25 +439,35 @@ static inline void v3_normalize( v3f a ) v3_muls( a, 1.f / v3_length( a ), a ); } -static inline float vg_lerpf( float a, float b, float t ) -{ +static inline f32 vg_lerpf( f32 a, f32 b, f32 t ){ return a + t*(b-a); } -static inline double vg_lerp( double a, double b, double t ) +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 float vg_alerpf( float a, float b, float t ) +static f32 vg_alerpf( f32 a, f32 b, f32 t ) { - float d = fmodf( b-a, VG_TAUf ), + f32 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 ) +static inline void v3_lerp( v3f a, v3f b, f32 t, v3f d ) { d[0] = a[0] + t*(b[0]-a[0]); d[1] = a[1] + t*(b[1]-a[1]); @@ -420,12 +488,12 @@ static inline void v3_maxv( v3f a, v3f b, v3f dest ) dest[2] = vg_maxf(a[2], b[2]); } -static inline float v3_minf( v3f a ) +static inline f32 v3_minf( v3f a ) { return vg_minf( vg_minf( a[0], a[1] ), a[2] ); } -static inline float v3_maxf( v3f a ) +static inline f32 v3_maxf( v3f a ) { return vg_maxf( vg_maxf( a[0], a[1] ), a[2] ); } @@ -451,10 +519,10 @@ static inline void v3_negate( v3f a, v3f b ) b[2] = -a[2]; } -static inline void v3_rotate( v3f v, float angle, v3f axis, v3f d ) +static inline void v3_rotate( v3f v, f32 angle, v3f axis, v3f d ) { v3f v1, v2, k; - float c, s; + f32 c, s; c = cosf( angle ); s = sinf( angle ); @@ -469,20 +537,77 @@ static inline void v3_rotate( v3f v, float 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 ); +} + /* - * Vector 4 + * Compute yaw and pitch based of a normalized vector representing forward + * forward: -z + * result -> (YAW,PITCH,0.0) + */ +static void v3_angles( v3f v, v3f out_angles ){ + float yaw = atan2f( v[0], -v[2] ), + pitch = atan2f( + -v[1], + sqrtf( + v[0]*v[0] + v[2]*v[2] + ) + ); + + out_angles[0] = yaw; + out_angles[1] = pitch; + out_angles[2] = 0.0f; +} + +/* + * Compute the forward vector from (YAW,PITCH,ROLL) + * forward: -z + */ +static void v3_angles_vector( v3f angles, v3f out_v ){ + out_v[0] = sinf( angles[0] ) * cosf( angles[1] ); + out_v[1] = -sinf( angles[1] ); + out_v[2] = -cosf( angles[0] ) * cosf( angles[1] ); +} + +/* + * ----------------------------------------------------------------------------- + * Section 2.c 4D Vectors + * ----------------------------------------------------------------------------- */ + static inline void v4_copy( v4f a, v4f b ) { b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3]; } +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 v4_zero( v4f a ) { a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f; } -static inline void v4_muls( v4f a, float s, v4f d ) +static inline void v4_muls( v4f a, f32 s, v4f d ) { d[0] = a[0]*s; d[1] = a[1]*s; @@ -490,7 +615,7 @@ static inline void v4_muls( v4f a, float s, v4f d ) d[3] = a[3]*s; } -static inline void v4_muladds( v4f a, v4f b, float s, v4f d ) +static inline void v4_muladds( v4f a, v4f b, f32 s, v4f d ) { d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; @@ -498,7 +623,7 @@ static inline void v4_muladds( v4f a, v4f b, float s, v4f d ) d[3] = a[3]+b[3]*s; } -static inline void v4_lerp( v4f a, v4f b, float t, v4f d ) +static inline void v4_lerp( v4f a, v4f b, f32 t, v4f d ) { d[0] = a[0] + t*(b[0]-a[0]); d[1] = a[1] + t*(b[1]-a[1]); @@ -506,25 +631,131 @@ static inline void v4_lerp( v4f a, v4f b, float t, v4f d ) d[3] = a[3] + t*(b[3]-a[3]); } -static inline float v4_dot( v4f a, v4f b ) +static inline f32 v4_dot( v4f a, v4f b ) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]; } -static inline float v4_length( v4f a ) +static inline f32 v4_length( v4f a ) { return sqrtf( v4_dot(a,a) ); } /* - * Matrix 2x2 + * ----------------------------------------------------------------------------- + * Section 3 Quaternions + * ----------------------------------------------------------------------------- + */ + +static inline void q_identity( v4f q ) +{ + q[0] = 0.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 1.0f; +} + +static inline void q_axis_angle( v4f q, v3f axis, f32 angle ) +{ + f32 a = angle*0.5f, + c = cosf(a), + s = sinf(a); + + q[0] = s*axis[0]; + q[1] = s*axis[1]; + q[2] = s*axis[2]; + q[3] = c; +} + +static inline void q_mul( v4f q, v4f q1, v4f d ) +{ + v4f t; + t[0] = q[3]*q1[0] + q[0]*q1[3] + q[1]*q1[2] - q[2]*q1[1]; + t[1] = q[3]*q1[1] - q[0]*q1[2] + q[1]*q1[3] + q[2]*q1[0]; + t[2] = q[3]*q1[2] + q[0]*q1[1] - q[1]*q1[0] + q[2]*q1[3]; + t[3] = q[3]*q1[3] - q[0]*q1[0] - q[1]*q1[1] - q[2]*q1[2]; + v4_copy( t, d ); +} + +static inline void q_normalize( v4f q ) +{ + 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 ) +{ + f32 s = 1.0f / v4_dot(q,q); + d[0] = -q[0]*s; + d[1] = -q[1]*s; + d[2] = -q[2]*s; + d[3] = q[3]*s; +} + +static inline void q_nlerp( v4f a, v4f b, f32 t, v4f d ){ + if( v4_dot(a,b) < 0.0f ){ + v4f c; + v4_muls( b, -1.0f, c ); + v4_lerp( a, c, t, d ); + } + else + v4_lerp( a, b, t, d ); + + q_normalize( d ); +} + +static inline void q_m3x3( v4f q, m3x3f d ) +{ + f32 + l = v4_length(q), + s = l > 0.0f? 2.0f/l: 0.0f, + + xx = s*q[0]*q[0], xy = s*q[0]*q[1], wx = s*q[3]*q[0], + yy = s*q[1]*q[1], yz = s*q[1]*q[2], wy = s*q[3]*q[1], + zz = s*q[2]*q[2], xz = s*q[0]*q[2], wz = s*q[3]*q[2]; + + d[0][0] = 1.0f - yy - zz; + d[1][1] = 1.0f - xx - zz; + d[2][2] = 1.0f - xx - yy; + d[0][1] = xy + wz; + d[1][2] = yz + wx; + d[2][0] = xz + wy; + d[1][0] = xy - wz; + d[2][1] = yz - wx; + d[0][2] = xz - wy; +} + +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 ); +} + +static f32 q_dist( v4f q0, v4f q1 ){ + return acosf( 2.0f * v4_dot(q0,q1) -1.0f ); +} + +/* + * ----------------------------------------------------------------------------- + * Section 4.a 2x2 matrices + * ----------------------------------------------------------------------------- */ -#define M2X2_INDENTIY {{1.0f, 0.0f, }, \ - { 0.0f, 1.0f, }} +#define M2X2_INDENTIY {{1.0f, 0.0f, }, \ + {0.0f, 1.0f, }} -#define M2X2_ZERO {{0.0f, 0.0f, }, \ - { 0.0f, 0.0f, }} +#define M2X2_ZERO {{0.0f, 0.0f, }, \ + {0.0f, 0.0f, }} static inline void m2x2_copy( m2x2f a, m2x2f b ) { @@ -538,9 +769,9 @@ static inline void m2x2_identity( m2x2f a ) m2x2_copy( id, a ); } -static inline void m2x2_create_rotation( m2x2f a, float theta ) +static inline void m2x2_create_rotation( m2x2f a, f32 theta ) { - float s, c; + f32 s, c; s = sinf( theta ); c = cosf( theta ); @@ -551,19 +782,94 @@ static inline void m2x2_create_rotation( m2x2f a, float 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 ); +} + /* - * Matrix 3x3 + * ----------------------------------------------------------------------------- + * Section 4.b 3x3 matrices + * ----------------------------------------------------------------------------- */ #define M3X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\ { 0.0f, 1.0f, 0.0f, },\ { 0.0f, 0.0f, 1.0f, }} -#define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\ +#define M3X3_ZERO {{0.0f, 0.0f, 0.0f, },\ { 0.0f, 0.0f, 0.0f, },\ { 0.0f, 0.0f, 0.0f, }} +static void euler_m3x3( v3f angles, m3x3f d ) +{ + f32 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 void m3x3_q( m3x3f m, v4f q ) +{ + f32 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]); + } +} + /* a X b == [b]T a == ...*/ static void m3x3_skew_symetric( m3x3f a, v3f v ) { @@ -578,6 +884,20 @@ static void m3x3_skew_symetric( m3x3f a, v3f v ) a[2][2] = 0.0f; } +/* aka kronecker product */ +static void m3x3_outer_product( m3x3f out_m, v3f a, v3f b ) +{ + out_m[0][0] = a[0]*b[0]; + out_m[0][1] = a[0]*b[1]; + out_m[0][2] = a[0]*b[2]; + out_m[1][0] = a[1]*b[0]; + out_m[1][1] = a[1]*b[1]; + out_m[1][2] = a[1]*b[2]; + out_m[2][0] = a[2]*b[0]; + out_m[2][1] = a[2]*b[1]; + out_m[2][2] = a[2]*b[2]; +} + static void m3x3_add( m3x3f a, m3x3f b, m3x3f d ) { v3_add( a[0], b[0], d[0] ); @@ -585,6 +905,13 @@ static void m3x3_add( m3x3f a, m3x3f b, m3x3f d ) v3_add( a[2], b[2], d[2] ); } +static void m3x3_sub( m3x3f a, m3x3f b, m3x3f d ) +{ + v3_sub( a[0], b[0], d[0] ); + v3_sub( a[1], b[1], d[1] ); + v3_sub( a[2], b[2], d[2] ); +} + static inline void m3x3_copy( m3x3f a, m3x3f b ) { v3_copy( a[0], b[0] ); @@ -598,12 +925,19 @@ static inline void m3x3_identity( m3x3f a ) m3x3_copy( id, a ); } -static void m3x3_diagonal( m3x3f a, float v ) +static void m3x3_diagonal( m3x3f out_a, f32 v ) +{ + m3x3_identity( out_a ); + out_a[0][0] = v; + out_a[1][1] = v; + out_a[2][2] = v; +} + +static void m3x3_setdiagonalv3( m3x3f a, v3f v ) { - m3x3_identity( a ); - a[0][0] = v; - a[1][1] = v; - a[2][2] = v; + a[0][0] = v[0]; + a[1][1] = v[1]; + a[2][2] = v[2]; } static inline void m3x3_zero( m3x3f a ) @@ -614,11 +948,11 @@ static inline void m3x3_zero( m3x3f a ) static inline void m3x3_inv( m3x3f src, m3x3f dest ) { - float a = src[0][0], b = src[0][1], c = src[0][2], + f32 a = src[0][0], b = src[0][1], c = src[0][2], d = src[1][0], e = src[1][1], f = src[1][2], g = src[2][0], h = src[2][1], i = src[2][2]; - float det = 1.f / + f32 det = 1.f / (+a*(e*i-h*f) -b*(d*i-f*g) +c*(d*h-e*g)); @@ -634,7 +968,7 @@ static inline void m3x3_inv( m3x3f src, m3x3f dest ) dest[2][2] = (a*e-d*b)*det; } -static float m3x3_det( m3x3f m ) +static f32 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]) @@ -643,7 +977,7 @@ static float m3x3_det( m3x3f m ) static inline void m3x3_transpose( m3x3f src, m3x3f dest ) { - float a = src[0][0], b = src[0][1], c = src[0][2], + f32 a = src[0][0], b = src[0][1], c = src[0][2], d = src[1][0], e = src[1][1], f = src[1][2], g = src[2][0], h = src[2][1], i = src[2][2]; @@ -660,7 +994,7 @@ static inline void m3x3_transpose( m3x3f src, m3x3f dest ) static inline void m3x3_mul( m3x3f a, m3x3f b, m3x3f d ) { - float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], + f32 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], @@ -691,9 +1025,9 @@ static inline void m3x3_mulv( m3x3f m, v3f v, v3f d ) } static inline void m3x3_projection( m3x3f dst, - float const left, float const right, float const bottom, float const top ) + f32 const left, f32 const right, f32 const bottom, f32 const top ) { - float rl, tb; + f32 rl, tb; m3x3_zero( dst ); @@ -719,19 +1053,19 @@ static inline void m3x3_scale( m3x3f m, v3f v ) v3_muls( m[2], v[2], m[2] ); } -static inline void m3x3_scalef( m3x3f m, float f ) +static inline void m3x3_scalef( m3x3f m, f32 f ) { v3f v; v3_fill( v, f ); m3x3_scale( m, v ); } -static inline void m3x3_rotate( m3x3f m, float angle ) +static inline void m3x3_rotate( m3x3f m, f32 angle ) { - float m00 = m[0][0], m10 = m[1][0], + f32 m00 = m[0][0], m10 = m[1][0], m01 = m[0][1], m11 = m[1][1], m02 = m[0][2], m12 = m[1][2]; - float c, s; + f32 c, s; s = sinf( angle ); c = cosf( angle ); @@ -746,7 +1080,9 @@ static inline void m3x3_rotate( m3x3f m, float angle ) } /* - * Matrix 4x3 + * ----------------------------------------------------------------------------- + * Section 4.c 4x3 matrices + * ----------------------------------------------------------------------------- */ #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\ @@ -770,7 +1106,7 @@ static inline void m4x3_invert_affine( m4x3f a, m4x3f b ) static void m4x3_invert_full( m4x3f src, m4x3f dst ) { - float t2, t4, t5, + f32 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], @@ -821,9 +1157,9 @@ static inline void m4x3_identity( m4x3f a ) m4x3_copy( id, a ); } -static inline void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) +static void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) { - float + f32 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], @@ -847,7 +1183,10 @@ static inline void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32; } -static inline void m4x3_mulv( m4x3f m, v3f v, v3f d ) +#if 0 /* shat appf mingw wstringop-overflow */ +inline +#endif +static void m4x3_mulv( m4x3f m, v3f v, v3f d ) { v3f res; @@ -861,7 +1200,7 @@ static inline void m4x3_mulv( m4x3f m, v3f v, v3f d ) /* * Transform plane ( xyz, distance ) */ -static inline void m4x3_mulp( m4x3f m, v4f p, v4f d ) +static void m4x3_mulp( m4x3f m, v4f p, v4f d ) { v3f o; @@ -876,17 +1215,17 @@ static inline void m4x3_mulp( m4x3f m, v4f p, v4f d ) * Affine transforms */ -static inline void m4x3_translate( m4x3f m, v3f v ) +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 inline void m4x3_rotate_x( m4x3f m, float angle ) +static void m4x3_rotate_x( m4x3f m, f32 angle ) { m4x3f t = M4X3_IDENTITY; - float c, s; + f32 c, s; c = cosf( angle ); s = sinf( angle ); @@ -899,10 +1238,10 @@ static inline void m4x3_rotate_x( m4x3f m, float angle ) m4x3_mul( m, t, m ); } -static inline void m4x3_rotate_y( m4x3f m, float angle ) +static void m4x3_rotate_y( m4x3f m, f32 angle ) { m4x3f t = M4X3_IDENTITY; - float c, s; + f32 c, s; c = cosf( angle ); s = sinf( angle ); @@ -915,10 +1254,10 @@ static inline void m4x3_rotate_y( m4x3f m, float angle ) m4x3_mul( m, t, m ); } -static inline void m4x3_rotate_z( m4x3f m, float angle ) +static void m4x3_rotate_z( m4x3f m, f32 angle ) { m4x3f t = M4X3_IDENTITY; - float c, s; + f32 c, s; c = cosf( angle ); s = sinf( angle ); @@ -931,7 +1270,7 @@ static inline void m4x3_rotate_z( m4x3f m, float angle ) m4x3_mul( m, t, m ); } -static inline void m4x3_expand( m4x3f m, m4x4f d ) +static void m4x3_expand( m4x3f m, m4x4f d ) { v3_copy( m[0], d[0] ); v3_copy( m[1], d[1] ); @@ -943,104 +1282,42 @@ static inline void m4x3_expand( m4x3f m, m4x4f d ) 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] ); - v3_maxv( a[1], pt, a[1] ); -} - -static inline void box_concat( boxf a, boxf b ) +static void m4x3_decompose( m4x3f m, v3f co, v4f q, v3f s ) { - v3_minv( a[0], b[0], a[0] ); - v3_maxv( a[1], b[1], a[1] ); -} + v3_copy( m[3], co ); + s[0] = v3_length(m[0]); + s[1] = v3_length(m[1]); + s[2] = v3_length(m[2]); -static inline void box_copy( boxf a, boxf b ) -{ - v3_copy( a[0], b[0] ); - v3_copy( a[1], b[1] ); -} + 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] ); -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] ) - ; + m3x3_q( rot, q ); } -static int box_within( boxf greater, boxf lesser ) -{ - v3f a, b; - v3_sub( lesser[0], greater[0], a ); - v3_sub( lesser[1], greater[1], b ); - - if( (a[0] >= 0.0f) && (a[1] >= 0.0f) && (a[2] >= 0.0f) && - (b[0] <= 0.0f) && (b[1] <= 0.0f) && (b[2] <= 0.0f) ) - { - return 1; - } - - return 0; -} +static void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ){ + v3f v; + m4x3_mulv( m, point, v ); -static inline void box_init_inf( boxf box ) -{ - v3_fill( box[0], INFINITY ); - v3_fill( box[1], -INFINITY ); + v3_minv( box[0], v, box[0] ); + v3_maxv( box[1], v, box[1] ); } -static inline 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] } ); -} - -int ray_aabb1( boxf box, v3f co, v3f dir_inv, float dist ) -{ - v3f v0, v1; - float tmin, tmax; - - v3_sub( box[0], co, v0 ); - v3_sub( box[1], co, 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] ); - 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.0f); + 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; @@ -1057,22 +1334,24 @@ static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up ) } /* - * Matrix 4x4 + * ----------------------------------------------------------------------------- + * Section 4.d 4x4 matrices + * ----------------------------------------------------------------------------- */ #define M4X4_IDENTITY {{1.0f, 0.0f, 0.0f, 0.0f },\ { 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 },\ +#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 fnear, float ffar ) +static void m4x4_projection( m4x4f m, f32 angle, + f32 ratio, f32 fnear, f32 ffar ) { - float scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear, + f32 scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear, r = ratio * scale, l = -r, t = scale, @@ -1128,7 +1407,7 @@ static inline void m4x4_zero( m4x4f 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], + f32 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], @@ -1170,7 +1449,7 @@ static inline void m4x4_mulv( m4x4f m, v4f v, v4f 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], + f32 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], @@ -1225,15 +1504,101 @@ static inline void m4x4_inv( m4x4f a, m4x4f d ) v4_muls( d[3], det, d[3] ); } +/* + * http://www.terathon.com/lengyel/Lengyel-Oblique.pdf + */ +static void m4x4_clip_projection( m4x4f mat, v4f plane ){ + v4f c = + { + (vg_signf(plane[0]) + mat[2][0]) / mat[0][0], + (vg_signf(plane[1]) + mat[2][1]) / mat[1][1], + -1.0f, + (1.0f + mat[2][2]) / mat[3][2] + }; + + v4_muls( plane, 2.0f / v4_dot(plane,c), c ); + + mat[0][2] = c[0]; + mat[1][2] = c[1]; + mat[2][2] = c[2] + 1.0f; + mat[3][2] = c[3]; +} + +/* + * Undoes the above operation + */ +static void m4x4_reset_clipping( m4x4f mat, float ffar, float fnear ){ + mat[0][2] = 0.0f; + mat[1][2] = 0.0f; + mat[2][2] = -(ffar + fnear) / (ffar - fnear); + mat[3][2] = -2.0f * ffar * fnear / (ffar - fnear); +} + +/* + * ----------------------------------------------------------------------------- + * Section 5.a Boxes + * ----------------------------------------------------------------------------- + */ + +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] ); + v3_maxv( a[1], b[1], a[1] ); +} + +static inline void box_copy( boxf a, boxf b ) +{ + v3_copy( a[0], b[0] ); + 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 int box_within( boxf greater, boxf lesser ) +{ + v3f a, b; + v3_sub( lesser[0], greater[0], a ); + v3_sub( lesser[1], greater[1], b ); + + if( (a[0] >= 0.0f) && (a[1] >= 0.0f) && (a[2] >= 0.0f) && + (b[0] <= 0.0f) && (b[1] <= 0.0f) && (b[2] <= 0.0f) ) + { + return 1; + } + + return 0; +} + +static inline void box_init_inf( boxf box ){ + v3_fill( box[0], INFINITY ); + v3_fill( box[1], -INFINITY ); +} + /* - * Planes (double precision) + * ----------------------------------------------------------------------------- + * Section 5.b Planes + * ----------------------------------------------------------------------------- */ -static inline void tri_to_plane( double a[3], double b[3], - double c[3], double p[4] ) + +static inline void tri_to_plane( f64 a[3], f64 b[3], + f64 c[3], f64 p[4] ) { - double edge0[3]; - double edge1[3]; - double l; + f64 edge0[3]; + f64 edge1[3]; + f64 l; edge0[0] = b[0] - a[0]; edge0[1] = b[1] - a[1]; @@ -1257,11 +1622,11 @@ static inline void tri_to_plane( double a[3], double b[3], static int plane_intersect3( v4f a, v4f b, v4f c, v3f p ) { - float const epsilon = 1e-6f; + f32 const epsilon = 1e-6f; v3f x; v3_cross( a, b, x ); - float d = v3_dot( x, c ); + f32 d = v3_dot( x, c ); if( (d < epsilon) && (d > -epsilon) ) return 0; @@ -1278,219 +1643,68 @@ static int plane_intersect3( v4f a, v4f b, v4f c, v3f p ) return 1; } -int plane_intersect2( v4f a, v4f b, v3f p, v3f n ) +static int plane_intersect2( v4f a, v4f b, v3f p, v3f n ) { - float const epsilon = 1e-6f; + f32 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 - (a[0] * p[0] + a[1] * p[1] + a[2] * p[2]) - -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2]) - ; -} - -/* Quaternions */ - -static inline void q_identity( v4f q ) -{ - q[0] = 0.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 1.0f; -} - -static inline void q_axis_angle( v4f q, v3f axis, float angle ) -{ - float a = angle*0.5f, - c = cosf(a), - s = sinf(a); - - q[0] = s*axis[0]; - q[1] = s*axis[1]; - q[2] = s*axis[2]; - q[3] = c; -} - -static inline void q_mul( v4f q, v4f q1, v4f d ) -{ - v4f t; - t[0] = q[3]*q1[0] + q[0]*q1[3] + q[1]*q1[2] - q[2]*q1[1]; - t[1] = q[3]*q1[1] - q[0]*q1[2] + q[1]*q1[3] + q[2]*q1[0]; - t[2] = q[3]*q1[2] + q[0]*q1[1] - q[1]*q1[0] + q[2]*q1[3]; - t[3] = q[3]*q1[3] - q[0]*q1[0] - q[1]*q1[1] - q[2]*q1[2]; - v4_copy( t, d ); -} - -static inline void q_normalize( v4f q ) -{ - float s = 1.0f/ sqrtf(v4_dot(q,q)); - q[0] *= s; - q[1] *= s; - q[2] *= s; - q[3] *= s; -} - -static inline void q_inv( v4f q, v4f d ) -{ - float s = 1.0f / v4_dot(q,q); - d[0] = -q[0]*s; - d[1] = -q[1]*s; - d[2] = -q[2]*s; - 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 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] ); -} + f32 d = v3_length2( c ); -static inline void q_m3x3( v4f q, m3x3f d ) -{ - float - l = v4_length(q), - s = l > 0.0f? 2.0f/l: 0.0f, + if( (d < epsilon) && (d > -epsilon) ) + return 0; - xx = s*q[0]*q[0], xy = s*q[0]*q[1], wx = s*q[3]*q[0], - yy = s*q[1]*q[1], yz = s*q[1]*q[2], wy = s*q[3]*q[1], - zz = s*q[2]*q[2], xz = s*q[0]*q[2], wz = s*q[3]*q[2]; + v3f v0, v1, vx; + v3_cross( c, b, v0 ); + v3_cross( a, c, v1 ); - d[0][0] = 1.0f - yy - zz; - d[1][1] = 1.0f - xx - zz; - d[2][2] = 1.0f - xx - yy; - d[0][1] = xy + wz; - d[1][2] = yz + wx; - d[2][0] = xz + wy; - d[1][0] = xy - wz; - d[2][1] = yz - wx; - d[0][2] = xz - wy; + 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 void m3x3_q( m3x3f m, v4f q ) +static int plane_segment( v4f plane, v3f a, v3f b, v3f co ) { - float diag, r, rinv; + f32 d0 = v3_dot( a, plane ) - plane[3], + d1 = v3_dot( b, plane ) - plane[3]; - 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 + if( d0*d1 < 0.0f ) { - 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]); + f32 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 void q_mulv( v4f q, v3f v, v3f d ) +static inline f64 plane_polarity( f64 p[4], f64 a[3] ) { - 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 ); + return + (a[0] * p[0] + a[1] * p[1] + a[2] * p[2]) + -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2]) + ; } -enum contact_type -{ - k_contact_type_default, - k_contact_type_disabled, - k_contact_type_edge -}; +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; +} /* * ----------------------------------------------------------------------------- - * Closest point functions + * Section 5.c Closest point functions * ----------------------------------------------------------------------------- */ @@ -1498,19 +1712,19 @@ enum contact_type * 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) +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; v3_sub( q1, p1, d1 ); v3_sub( q2, p2, d2 ); v3_sub( p1, p2, r ); - float a = v3_length2( d1 ), + f32 a = v3_length2( d1 ), e = v3_length2( d2 ), f = v3_dot( d2, r ); - const float kEpsilon = 0.0001f; + const f32 kEpsilon = 0.0001f; if( a <= kEpsilon && e <= kEpsilon ) { @@ -1532,7 +1746,7 @@ VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, } else { - float c = v3_dot( d1, r ); + f32 c = v3_dot( d1, r ); if( e <= kEpsilon ) { *t = 0.0f; @@ -1540,7 +1754,7 @@ VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, } else { - float b = v3_dot(d1,d2), + f32 b = v3_dot(d1,d2), d = a*e-b*b; if( d != 0.0f ) @@ -1575,7 +1789,7 @@ 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 ) +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]) ) @@ -1584,13 +1798,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; @@ -1599,22 +1813,22 @@ VG_STATIC void closest_point_obb( v3f p, boxf box, m4x3_mulv( mtx, local, dest ); } -VG_STATIC float 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 ); v3_sub( point, a, v1 ); - float t = v3_dot( v1, v0 ) / v3_length2(v0); + f32 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 ) +static void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) { v3f ab, ac, ap; - float d1, d2; + f32 d1, d2; /* Region outside A */ v3_sub( tri[1], tri[0], ab ); @@ -1632,7 +1846,7 @@ VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) /* Region outside B */ v3f bp; - float d3, d4; + f32 d3, d4; v3_sub( p, tri[1], bp ); d3 = v3_dot( ab, bp ); @@ -1646,10 +1860,10 @@ VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) } /* Edge region of AB */ - float vc = d1*d4 - d3*d2; + f32 vc = d1*d4 - d3*d2; if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) { - float v = d1 / (d1-d3); + f32 v = d1 / (d1-d3); v3_muladds( tri[0], ab, v, dest ); v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest ); return; @@ -1657,7 +1871,7 @@ VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) /* Region outside C */ v3f cp; - float d5, d6; + f32 d5, d6; v3_sub( p, tri[2], cp ); d5 = v3_dot(ab, cp); d6 = v3_dot(ac, cp); @@ -1670,20 +1884,20 @@ VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) } /* Region of AC */ - float vb = d5*d2 - d1*d6; + f32 vb = d5*d2 - d1*d6; if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) { - float w = d2 / (d2-d6); + f32 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; + f32 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)); + f32 w = (d4-d3) / ((d4-d3) + (d5-d6)); v3f bc; v3_sub( tri[2], tri[1], bc ); v3_muladds( tri[1], bc, w, dest ); @@ -1692,7 +1906,7 @@ VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) } /* P inside region, Q via barycentric coordinates uvw */ - float d = 1.0f/(va+vb+vc), + f32 d = 1.0f/(va+vb+vc), v = vb*d, w = vc*d; @@ -1700,10 +1914,17 @@ VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest ) v3_muladds( dest, ac, w, dest ); } -VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) +enum contact_type +{ + k_contact_type_default, + k_contact_type_disabled, + k_contact_type_edge +}; + +static enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) { v3f ab, ac, ap; - float d1, d2; + f32 d1, d2; /* Region outside A */ v3_sub( tri[1], tri[0], ab ); @@ -1720,7 +1941,7 @@ VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) /* Region outside B */ v3f bp; - float d3, d4; + f32 d3, d4; v3_sub( p, tri[1], bp ); d3 = v3_dot( ab, bp ); @@ -1733,17 +1954,17 @@ VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) } /* Edge region of AB */ - float vc = d1*d4 - d3*d2; + f32 vc = d1*d4 - d3*d2; if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f ) { - float v = d1 / (d1-d3); + f32 v = d1 / (d1-d3); v3_muladds( tri[0], ab, v, dest ); return k_contact_type_edge; } /* Region outside C */ v3f cp; - float d5, d6; + f32 d5, d6; v3_sub( p, tri[2], cp ); d5 = v3_dot(ab, cp); d6 = v3_dot(ac, cp); @@ -1755,19 +1976,19 @@ VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) } /* Region of AC */ - float vb = d5*d2 - d1*d6; + f32 vb = d5*d2 - d1*d6; if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f ) { - float w = d2 / (d2-d6); + f32 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; + f32 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)); + f32 w = (d4-d3) / ((d4-d3) + (d5-d6)); v3f bc; v3_sub( tri[2], tri[1], bc ); v3_muladds( tri[1], bc, w, dest ); @@ -1775,7 +1996,7 @@ VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest ) } /* P inside region, Q via barycentric coordinates uvw */ - float d = 1.0f/(va+vb+vc), + f32 d = 1.0f/(va+vb+vc), v = vb*d, w = vc*d; @@ -1785,7 +2006,6 @@ 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; @@ -1797,8 +2017,7 @@ static void closest_point_elipse( v2f p, v2f e, v2f o ) v2_fill( t, 0.70710678118654752f ); - for( int i=0; i<3; i++ ) - { + for( int i=0; i<3; i++ ){ v2f v, u, ud, w; v2_mul( ve, t, v ); /* ve*t*t*t */ @@ -1825,19 +2044,42 @@ static void closest_point_elipse( v2f p, v2f e, v2f o ) } /* - * Raycasts + * ----------------------------------------------------------------------------- + * Section 5.d Raycasts & Spherecasts + * ----------------------------------------------------------------------------- */ +static int ray_aabb1( boxf box, v3f co, v3f dir_inv, f32 dist ) +{ + v3f v0, v1; + f32 tmin, tmax; + + v3_sub( box[0], co, v0 ); + v3_sub( box[1], co, 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] ); + 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.0f); +} + /* Time of intersection with ray vs triangle */ static int ray_tri( v3f tri[3], v3f co, - v3f dir, float *dist ) + v3f dir, f32 *dist, int backfaces ) { - float const kEpsilon = 0.00001f; + f32 const kEpsilon = 0.00001f; v3f v0, v1, h, s, q, n; - float a,f,u,v,t; + f32 a,f,u,v,t; - float *pa = tri[0], + f32 *pa = tri[0], *pb = tri[1], *pc = tri[2]; @@ -1846,7 +2088,7 @@ static int ray_tri( v3f tri[3], v3f co, v3_cross( dir, v1, h ); v3_cross( v0, v1, n ); - if( v3_dot( n, dir ) > 0.0f ) /* Backface culling */ + if( (v3_dot( n, dir ) > 0.0f) && !backfaces ) /* Backface culling */ return 0; /* Parralel */ @@ -1877,20 +2119,20 @@ static int ray_tri( v3f tri[3], v3f co, } /* time of intersection with ray vs sphere */ -static int ray_sphere( v3f c, float r, - v3f co, v3f dir, float *t ) +static int ray_sphere( v3f c, f32 r, + v3f co, v3f dir, f32 *t ) { v3f m; v3_sub( co, c, m ); - float b = v3_dot( m, dir ), + f32 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; + f32 discr = b*b - c1; /* A negative discriminant corresponds to ray missing sphere */ if( discr < 0.0f ) @@ -1916,8 +2158,8 @@ static int ray_sphere( v3f c, float r, * 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 ) +static int ray_uncapped_finite_cylinder( v3f q, v3f p, f32 r, + v3f co, v3f dir, f32 *t ) { v3f d, m, n, sb; v3_muladds( co, dir, 1.0f, sb ); @@ -1926,7 +2168,7 @@ static int ray_uncapped_finite_cylinder( v3f q, v3f p, float r, v3_sub( co, p, m ); v3_sub( sb, co, n ); - float md = v3_dot( m, d ), + f32 md = v3_dot( m, d ), nd = v3_dot( n, d ), dd = v3_dot( d, d ), nn = v3_dot( n, n ), @@ -1941,7 +2183,7 @@ static int ray_uncapped_finite_cylinder( v3f q, v3f p, float r, return 0; } - float b = dd*mn - nd*md, + f32 b = dd*mn - nd*md, discr = b*b - a*c; if( discr < 0.0f ) @@ -1967,7 +2209,7 @@ static int ray_uncapped_finite_cylinder( v3f q, v3f p, float r, * 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 co, v3f dir, f32 r, f32 *t, v3f n ) { v3f sum[3]; v3f v0, v1; @@ -1981,11 +2223,10 @@ static int spherecast_triangle( v3f tri[3], v3_muladds( tri[2], n, r, sum[2] ); int hit = 0; - float t_min = INFINITY, + f32 t_min = INFINITY, t1; - if( ray_tri( sum, co, dir, &t1 ) ) - { + if( ray_tri( sum, co, dir, &t1, 0 ) ){ t_min = vg_minf( t_min, t1 ); hit = 1; } @@ -1994,25 +2235,20 @@ static int spherecast_triangle( v3f tri[3], * 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 ) ) - { + 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++ ) - { + 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 ) - { + if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) ){ + if( t1 < t_min ){ t_min = t1; v3f co1, ct, cx; @@ -2032,17 +2268,270 @@ static int spherecast_triangle( v3f tri[3], return hit; } -static inline float vg_randf(void) +/* + * ----------------------------------------------------------------------------- + * Section 5.e Curves + * ----------------------------------------------------------------------------- + */ + +static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, f32 t, v3f p ) { - /* TODO: replace with our own rand */ - return (float)rand()/(float)(RAND_MAX); + f32 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 ); } -static inline void vg_rand_dir(v3f dir) +static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p ) { - dir[0] = vg_randf(); - dir[1] = vg_randf(); - dir[2] = vg_randf(); + f32 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 ); +} + +/* + * ----------------------------------------------------------------------------- + * Section 5.f Volumes + * ----------------------------------------------------------------------------- + */ + +static f32 vg_sphere_volume( f32 r ){ + return (4.0f/3.0f) * VG_PIf * r*r*r; +} + +static f32 vg_box_volume( boxf box ){ + v3f e; + v3_sub( box[1], box[0], e ); + return e[0]*e[1]*e[2]; +} + +static f32 vg_cylinder_volume( f32 r, f32 h ){ + return VG_PIf * r*r * h; +} + +static f32 vg_capsule_volume( f32 r, f32 h ){ + return vg_sphere_volume( r ) + vg_cylinder_volume( r, h-r*2.0f ); +} + +static void vg_sphere_bound( f32 r, boxf out_box ){ + v3_fill( out_box[0], -r ); + v3_fill( out_box[1], r ); +} + +static void vg_capsule_bound( f32 r, f32 h, boxf out_box ){ + v3_copy( (v3f){-r,-h*0.5f,r}, out_box[0] ); + v3_copy( (v3f){-r, h*0.5f,r}, out_box[1] ); +} + + +/* + * ----------------------------------------------------------------------------- + * Section 5.g Inertia Tensors + * ----------------------------------------------------------------------------- + */ + +/* + * Translate existing inertia tensor + */ +static void vg_translate_inertia( m3x3f inout_inertia, f32 mass, v3f d ){ + /* + * I = I_0 + m*[(d.d)E_3 - d(X)d] + * + * I: updated tensor + * I_0: original tensor + * m: scalar mass + * d: translation vector + * (X): outer product + * E_3: identity matrix + */ + m3x3f t, outer, scale; + m3x3_diagonal( t, v3_dot(d,d) ); + m3x3_outer_product( outer, d, d ); + m3x3_sub( t, outer, t ); + m3x3_diagonal( scale, mass ); + m3x3_mul( scale, t, t ); + m3x3_add( inout_inertia, t, inout_inertia ); +} + +/* + * Rotate existing inertia tensor + */ +static void vg_rotate_inertia( m3x3f inout_inertia, m3x3f rotation ){ + /* + * I = R I_0 R^T + * + * I: updated tensor + * I_0: original tensor + * R: rotation matrix + * R^T: tranposed rotation matrix + */ + + m3x3f Rt; + m3x3_transpose( rotation, Rt ); + m3x3_mul( rotation, inout_inertia, inout_inertia ); + m3x3_mul( inout_inertia, Rt, inout_inertia ); +} +/* + * Create inertia tensor for box + */ +static void vg_box_inertia( boxf box, f32 mass, m3x3f out_inertia ){ + v3f e, com; + v3_sub( box[1], box[0], e ); + v3_muladds( box[0], e, 0.5f, com ); + + f32 ex2 = e[0]*e[0], + ey2 = e[1]*e[1], + ez2 = e[2]*e[2], + ix = (ey2+ez2) * mass * (1.0f/12.0f), + iy = (ex2+ez2) * mass * (1.0f/12.0f), + iz = (ex2+ey2) * mass * (1.0f/12.0f); + + m3x3_identity( out_inertia ); + m3x3_setdiagonalv3( out_inertia, (v3f){ ix, iy, iz } ); + vg_translate_inertia( out_inertia, mass, com ); +} + +/* + * Create inertia tensor for sphere + */ +static void vg_sphere_inertia( f32 r, f32 mass, m3x3f out_inertia ){ + f32 ixyz = r*r * mass * (2.0f/5.0f); + + m3x3_identity( out_inertia ); + m3x3_setdiagonalv3( out_inertia, (v3f){ ixyz, ixyz, ixyz } ); +} + +/* + * Create inertia tensor for capsule + */ +static void vg_capsule_inertia( f32 r, f32 h, f32 mass, m3x3f out_inertia ){ + f32 density = mass / vg_capsule_volume( r, h ), + ch = h-r*2.0f, /* cylinder height */ + cm = VG_PIf * ch*r*r * density, /* cylinder mass */ + hm = VG_TAUf * (1.0f/3.0f) * r*r*r * density, /* hemisphere mass */ + + iy = r*r*cm * 0.5f, + ixz = iy * 0.5f + cm*ch*ch*(1.0f/12.0f), + + aux0= (hm*2.0f*r*r)/5.0f; + + iy += aux0 * 2.0f; + + f32 aux1= ch*0.5f, + aux2= aux0 + hm*(aux1*aux1 + 3.0f*(1.0f/8.0f)*ch*r); + + ixz += aux2*2.0f; + + m3x3_identity( out_inertia ); + m3x3_setdiagonalv3( out_inertia, (v3f){ ixz, iy, ixz } ); +} + +/* + * ----------------------------------------------------------------------------- + * Section 6.a PSRNG and some distributions + * ----------------------------------------------------------------------------- + */ + +/* An implementation of the MT19937 Algorithm for the Mersenne Twister + * by Evan Sultanik. Based upon the pseudocode in: M. Matsumoto and + * T. Nishimura, "Mersenne Twister: A 623-dimensionally + * equidistributed uniform pseudorandom number generator," ACM + * Transactions on Modeling and Computer Simulation Vol. 8, No. 1, + * January pp.3-30 1998. + * + * http://www.sultanik.com/Mersenne_twister + * https://github.com/ESultanik/mtwister/blob/master/mtwister.c + */ + +#define MT_UPPER_MASK 0x80000000 +#define MT_LOWER_MASK 0x7fffffff +#define MT_TEMPERING_MASK_B 0x9d2c5680 +#define MT_TEMPERING_MASK_C 0xefc60000 + +#define MT_STATE_VECTOR_LENGTH 624 + +/* changes to STATE_VECTOR_LENGTH also require changes to this */ +#define MT_STATE_VECTOR_M 397 + +typedef struct vg_rand vg_rand; +struct vg_rand { + u32 mt[MT_STATE_VECTOR_LENGTH]; + i32 index; +}; + +static void vg_rand_seed( vg_rand *rand, unsigned long seed ) { + /* set initial seeds to mt[STATE_VECTOR_LENGTH] using the generator + * from Line 25 of Table 1 in: Donald Knuth, "The Art of Computer + * Programming," Vol. 2 (2nd Ed.) pp.102. + */ + rand->mt[0] = seed & 0xffffffff; + for( rand->index=1; rand->indexindex++){ + rand->mt[rand->index] = (6069 * rand->mt[rand->index-1]) & 0xffffffff; + } +} + +/* + * Generates a pseudo-randomly generated long. + */ +static u32 vg_randu32( vg_rand *rand ) { + u32 y; + /* mag[x] = x * 0x9908b0df for x = 0,1 */ + static u32 mag[2] = {0x0, 0x9908b0df}; + if( rand->index >= MT_STATE_VECTOR_LENGTH || rand->index < 0 ){ + /* generate STATE_VECTOR_LENGTH words at a time */ + int kk; + if( rand->index >= MT_STATE_VECTOR_LENGTH+1 || rand->index < 0 ){ + vg_rand_seed( rand, 4357 ); + } + for( kk=0; kkmt[kk] & MT_UPPER_MASK) | + (rand->mt[kk+1] & MT_LOWER_MASK); + rand->mt[kk] = rand->mt[kk+MT_STATE_VECTOR_M] ^ (y>>1) ^ mag[y & 0x1]; + } + for( ; kkmt[kk] & MT_UPPER_MASK) | + (rand->mt[kk+1] & MT_LOWER_MASK); + rand->mt[kk] = + rand->mt[ kk+(MT_STATE_VECTOR_M-MT_STATE_VECTOR_LENGTH)] ^ + (y >> 1) ^ mag[y & 0x1]; + } + y = (rand->mt[MT_STATE_VECTOR_LENGTH-1] & MT_UPPER_MASK) | + (rand->mt[0] & MT_LOWER_MASK); + rand->mt[MT_STATE_VECTOR_LENGTH-1] = + rand->mt[MT_STATE_VECTOR_M-1] ^ (y >> 1) ^ mag[y & 0x1]; + rand->index = 0; + } + y = rand->mt[rand->index++]; + y ^= (y >> 11); + y ^= (y << 7) & MT_TEMPERING_MASK_B; + y ^= (y << 15) & MT_TEMPERING_MASK_C; + y ^= (y >> 18); + return y; +} + +/* + * Generates a pseudo-randomly generated f64 in the range [0..1]. + */ +static inline f64 vg_randf64( vg_rand *rand ){ + return (f64)vg_randu32(rand)/(f64)0xffffffff; +} + +static inline f64 vg_randf64_range( vg_rand *rand, f64 min, f64 max ){ + return vg_lerp( min, max, (f64)vg_randf64(rand) ); +} + +static inline void vg_rand_dir( vg_rand *rand, v3f dir ){ + dir[0] = vg_randf64(rand); + dir[1] = vg_randf64(rand); + dir[2] = vg_randf64(rand); + + /* warning: *could* be 0 length. + * very unlikely.. 1 in (2^32)^3. but its mathematically wrong. */ v3_muls( dir, 2.0f, dir ); v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir ); @@ -2050,35 +2539,73 @@ static inline void vg_rand_dir(v3f 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 void vg_rand_sphere( vg_rand *rand, v3f co ){ + vg_rand_dir(rand,co); + v3_muls( co, cbrtf( vg_randf64(rand) ), co ); } -static inline int vg_randint(int max) -{ - return rand()%max; +static void vg_rand_disc( vg_rand *rand, v2f co ){ + f32 a = vg_randf64(rand) * VG_TAUf; + co[0] = sinf(a); + co[1] = cosf(a); + v2_muls( co, sqrtf( vg_randf64(rand) ), co ); } -static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p ) -{ - float tt = t*t, - ttt = tt*t; +static void vg_rand_cone( vg_rand *rand, v3f out_dir, f32 angle ){ + f32 r = sqrtf(vg_randf64(rand)) * angle * 0.5f, + a = vg_randf64(rand) * VG_TAUf; - 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 ); + out_dir[0] = sinf(a) * sinf(r); + out_dir[1] = cosf(a) * sinf(r); + out_dir[2] = cosf(r); } -static void eval_bezier3( v3f p0, v3f p1, v3f p2, float t, v3f p ) -{ - float u = 1.0f-t; +static void vg_hsv_rgb( v3f hsv, v3f rgb ){ + i32 i = floorf( hsv[0]*6.0f ); + f32 v = hsv[2], + f = hsv[0] * 6.0f - (f32)i, + p = v * (1.0f-hsv[1]), + q = v * (1.0f-f*hsv[1]), + t = v * (1.0f-(1.0f-f)*hsv[1]); - v3_muls( p0, u*u, p ); - v3_muladds( p, p1, 2.0f*u*t, p ); - v3_muladds( p, p2, t*t, p ); + switch( i % 6 ){ + case 0: rgb[0] = v; rgb[1] = t; rgb[2] = p; break; + case 1: rgb[0] = q; rgb[1] = v; rgb[2] = p; break; + case 2: rgb[0] = p; rgb[1] = v; rgb[2] = t; break; + case 3: rgb[0] = p; rgb[1] = q; rgb[2] = v; break; + case 4: rgb[0] = t; rgb[1] = p; rgb[2] = v; break; + case 5: rgb[0] = v; rgb[1] = p; rgb[2] = q; break; + } } -#endif /* VG_M_H */ +static void vg_rgb_hsv( v3f rgb, v3f hsv ){ + f32 min = v3_minf( rgb ), + max = v3_maxf( rgb ), + range = max-min, + k_epsilon = 0.00001f; + + hsv[2] = max; + if( range < k_epsilon ){ + hsv[0] = 0.0f; + hsv[1] = 0.0f; + return; + } + + if( max > k_epsilon ){ + hsv[1] = range/max; + } + else { + hsv[0] = 0.0f; + hsv[1] = 0.0f; + return; + } + + if( rgb[0] >= max ) + hsv[0] = (rgb[1]-rgb[2])/range; + else if( max == rgb[1] ) + hsv[0] = 2.0f+(rgb[2]-rgb[0])/range; + else + hsv[0] = 4.0f+(rgb[0]-rgb[1])/range; + + hsv[0] = vg_fractf( hsv[0] * (60.0f/360.0f) ); +}