-/* 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
* 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 <math.h>
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] );
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] );
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 )
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
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;
* -----------------------------------------------------------------------------
*/
-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;
* -----------------------------------------------------------------------------
*/
-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 } );
}
/*
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) );
+}