X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_m.h;h=368f2889aff1f23807325e92f11228f22c654305;hp=2de095eb544c3508af92b147dddafdbdc2c5eae6;hb=HEAD;hpb=49cbd13560a41b254f9b17d84c4667ae75ab71a9 diff --git a/vg_m.h b/vg_m.h index 2de095e..4af60c8 100644 --- a/vg_m.h +++ b/vg_m.h @@ -1,4 +1,4 @@ -/* Copyright (C) 2021-2023 Harry Godden (hgn) - All Rights Reserved +/* Copyright (C) 2021-2024 Harry Godden (hgn) - All Rights Reserved * * 0. Misc * 1. Scalar operations @@ -19,12 +19,12 @@ * 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 @@ -97,6 +97,10 @@ static inline f32 vg_fractf( f32 a ) return a - floorf( a ); } +static inline f64 vg_fractf64( f64 a ){ + return a - floor( a ); +} + static f32 vg_cfrictf( f32 velocity, f32 F ) { return -vg_signf(velocity) * vg_minf( F, fabsf(velocity) ); @@ -880,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] ); @@ -887,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] ); @@ -900,12 +925,12 @@ static inline void m3x3_identity( m3x3f a ) m3x3_copy( id, a ); } -static void m3x3_diagonal( m3x3f a, f32 v ) +static void m3x3_diagonal( m3x3f out_a, f32 v ) { - m3x3_identity( a ); - a[0][0] = v; - a[1][1] = v; - a[2][2] = 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 ) @@ -1479,6 +1504,36 @@ 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 @@ -1588,7 +1643,7 @@ 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 ) { f32 const epsilon = 1e-6f; @@ -1994,7 +2049,7 @@ static void closest_point_elipse( v2f p, v2f e, v2f o ) * ----------------------------------------------------------------------------- */ -int ray_aabb1( boxf box, v3f co, v3f dir_inv, f32 dist ) +static int ray_aabb1( boxf box, v3f co, v3f dir_inv, f32 dist ) { v3f v0, v1; f32 tmin, tmax; @@ -2245,9 +2300,135 @@ static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p ) * ----------------------------------------------------------------------------- */ -static float vg_sphere_volume( float radius ){ - float r3 = radius*radius*radius; - return (4.0f/3.0f) * VG_PIf * r3; +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 } ); } /* @@ -2379,4 +2560,52 @@ static void vg_rand_cone( vg_rand *rand, v3f out_dir, f32 angle ){ out_dir[2] = cosf(r); } -#endif /* VG_M_H */ +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]); + + 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; + } +} + +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) ); +}