-/* 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 <math.h>
#define VG_PIf 3.14159265358979323846264338327950288f
#define VG_TAUf 6.28318530717958647692528676655900576f
-static inline float vg_minf( float a, float b )
+/*
+ * -----------------------------------------------------------------------------
+ * Section 0. Misc Operations
+ * -----------------------------------------------------------------------------
+ */
+
+/* get the f32 as the raw bits in a u32 without converting */
+static u32 vg_ftu32( f32 a )
{
- return a < b? a: b;
+ u32 *ptr = (u32 *)(&a);
+ return *ptr;
}
-static inline float vg_maxf( float a, float b )
+/* check if f32 is infinite */
+static int vg_isinff( f32 a )
{
- return a > b? a: b;
+ return ((vg_ftu32(a)) & 0x7FFFFFFFU) == 0x7F800000U;
}
-static inline float vg_clampf( float a, float min, float max )
+/* check if f32 is not a number */
+static int vg_isnanf( f32 a )
{
- return vg_minf( max, vg_maxf( a, min ) );
+ return !vg_isinff(a) && ((vg_ftu32(a)) & 0x7F800000U) == 0x7F800000U;
}
-static inline float vg_signf( float a )
+/* check if f32 is a number and is not infinite */
+static int vg_validf( f32 a )
{
- return a < 0.0f? -1.0f: 1.0f;
+ return ((vg_ftu32(a)) & 0x7F800000U) != 0x7F800000U;
}
-static inline float vg_fractf( float a )
-{
- return a - floorf( a );
+static int v3_valid( v3f a ){
+ for( u32 i=0; i<3; i++ )
+ if( !vg_validf(a[i]) ) return 0;
+ return 1;
}
-static float stable_force( float current, float diff )
-{
- float fnew = current + diff;
+/*
+ * -----------------------------------------------------------------------------
+ * Section 1. Scalar Operations
+ * -----------------------------------------------------------------------------
+ */
- if( fnew * current < 0.0f )
- return 0.0f;
+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 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 ) );
+}
- return fnew;
+static inline f32 vg_signf( f32 a )
+{
+ return a < 0.0f? -1.0f: 1.0f;
}
-static inline int vg_min( int a, int b )
+static inline f32 vg_fractf( f32 a )
{
- return a < b? a: b;
+ return a - floorf( a );
}
-static inline int vg_max( int a, int b )
+static inline f64 vg_fractf64( f64 a ){
+ return a - floor( a );
+}
+
+static f32 vg_cfrictf( f32 velocity, f32 F )
{
- return a > b? a: b;
+ return -vg_signf(velocity) * vg_minf( F, fabsf(velocity) );
}
-static inline float vg_rad( float deg )
+static inline f32 vg_rad( f32 deg )
{
return deg * VG_PIf / 180.0f;
}
+/* 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;
+}
+
/*
- * Vector 3
+ * quantize float to bit count
*/
-static inline void v2_copy( v2f a, v2f b )
-{
- b[0] = a[0]; b[1] = a[1];
+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 );
}
-static inline void v2_zero( v2f a )
-{
- a[0] = 0.f; a[1] = 0.f;
+/*
+ * un-quantize discreet to float
+ */
+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 v2i_copy( v2i a, v2i b )
+/* 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);
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * 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];
}
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 )
+static inline void v2_abs( v2f a, v2f d )
{
- d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
+ 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;
}
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]);
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 );
}
b[1] = floorf( a[1] );
}
+static inline void v2_fill( v2f a, f32 v )
+{
+ a[0] = v;
+ a[1] = v;
+}
+
+static inline void v2_copysign( v2f a, v2f b )
+{
+ a[0] = copysignf( a[0], b[0] );
+ a[1] = copysignf( a[1], b[1] );
+}
+
+/* integer variants
+ * ---------------- */
+
+static inline void v2i_copy( v2i a, v2i b )
+{
+ 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];
+}
+
/*
- * Vector 3
+ * -----------------------------------------------------------------------------
+ * 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_zero( v3f a )
{
a[0] = 0.f; a[1] = 0.f; a[2] = 0.f;
}
-static inline void v3_copy( v3f a, v3f b )
+static inline void v3_add( v3f a, v3f b, v3f d )
{
- b[0] = a[0]; b[1] = a[1]; b[2] = a[2];
+ d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2];
}
-static inline void v3_add( v3f a, v3f b, v3f 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[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
}
+static inline void v3i_sub( v3i a, v3i b, v3i d )
+{
+ d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
+}
+
static inline void v3_mul( v3f a, v3f b, v3f d )
{
d[0] = a[0]*b[0]; d[1] = a[1]*b[1]; d[2] = a[2]*b[2];
static inline void v3_div( v3f a, v3f b, v3f d )
{
- d[0] = a[0]/b[0]; d[1] = a[1]/b[1]; d[2] = a[2]/b[2];
+ d[0] = b[0]!=0.0f? a[0]/b[0]: INFINITY;
+ d[1] = b[1]!=0.0f? a[1]/b[1]: INFINITY;
+ 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_divs( v3f a, float s, v3f d )
+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, f32 s, v3f d )
{
- d[0] = a[0]/s; d[1] = a[1]/s; d[2] = a[2]/s;
+ if( s == 0.0f )
+ v3_fill( d, INFINITY );
+ else
+ {
+ d[0] = a[0]/s;
+ d[1] = a[1]/s;
+ d[2] = a[2]/s;
+ }
}
-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;
}
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];
}
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 ) );
}
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]);
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] );
}
-static inline void v3_fill( v3f a, float v )
-{
- a[0] = v;
- a[1] = v;
- a[2] = v;
-}
-
static inline void v3_floor( v3f a, v3f b )
{
b[0] = floorf( a[0] );
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 );
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 );
+}
+
+/*
+ * 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] );
+}
+
/*
- * Vector 4
+ * -----------------------------------------------------------------------------
+ * 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;
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;
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]);
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]*a[3];
+ 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 )
{
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 );
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 inline void m3x3_copy( m3x3f a, m3x3f b )
-{
- v3_copy( a[0], b[0] );
- v3_copy( a[1], b[1] );
- v3_copy( a[2], b[2] );
-}
-
-static inline void m3x3_identity( m3x3f a )
+static void euler_m3x3( v3f angles, m3x3f d )
{
- m3x3f id = M3X3_IDENTITY;
- m3x3_copy( id, a );
-}
+ 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] );
-static inline void m3x3_zero( m3x3f a )
-{
- m3x3f z = M3X3_ZERO;
- m3x3_copy( z, a );
-}
+ d[2][0] = -sinY * cosP;
+ d[2][1] = sinP;
+ d[2][2] = cosY * cosP;
-static inline void m3x3_inv( m3x3f src, m3x3f dest )
-{
- float 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];
+ d[0][0] = cosY * cosR;
+ d[0][1] = sinR;
+ d[0][2] = sinY * cosR;
- float det = 1.f /
- (+a*(e*i-h*f)
- -b*(d*i-f*g)
- +c*(d*h-e*g));
-
- dest[0][0] = (e*i-h*f)*det;
- dest[0][1] = -(b*i-c*h)*det;
- dest[0][2] = (b*f-c*e)*det;
- dest[1][0] = -(d*i-f*g)*det;
- dest[1][1] = (a*i-c*g)*det;
- dest[1][2] = -(a*f-d*c)*det;
- dest[2][0] = (d*h-g*e)*det;
- dest[2][1] = -(a*h-g*b)*det;
- dest[2][2] = (a*e-d*b)*det;
+ v3_cross( d[0], d[2], d[1] );
}
-static inline void m3x3_transpose( m3x3f src, m3x3f dest )
+static void m3x3_q( m3x3f m, v4f q )
{
- float 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];
-
- dest[0][0] = a;
+ 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 )
+{
+ a[0][0] = 0.0f;
+ a[0][1] = v[2];
+ a[0][2] = -v[1];
+ a[1][0] = -v[2];
+ a[1][1] = 0.0f;
+ a[1][2] = v[0];
+ a[2][0] = v[1];
+ a[2][1] = -v[0];
+ a[2][2] = 0.0f;
+}
+
+/* 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[1], b[1], d[1] );
+ 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] );
+ v3_copy( a[1], b[1] );
+ v3_copy( a[2], b[2] );
+}
+
+static inline void m3x3_identity( m3x3f a )
+{
+ m3x3f id = M3X3_IDENTITY;
+ m3x3_copy( id, a );
+}
+
+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 )
+{
+ a[0][0] = v[0];
+ a[1][1] = v[1];
+ a[2][2] = v[2];
+}
+
+static inline void m3x3_zero( m3x3f a )
+{
+ m3x3f z = M3X3_ZERO;
+ m3x3_copy( z, a );
+}
+
+static inline void m3x3_inv( m3x3f src, m3x3f dest )
+{
+ 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];
+
+ f32 det = 1.f /
+ (+a*(e*i-h*f)
+ -b*(d*i-f*g)
+ +c*(d*h-e*g));
+
+ dest[0][0] = (e*i-h*f)*det;
+ dest[0][1] = -(b*i-c*h)*det;
+ dest[0][2] = (b*f-c*e)*det;
+ dest[1][0] = -(d*i-f*g)*det;
+ dest[1][1] = (a*i-c*g)*det;
+ dest[1][2] = -(a*f-d*c)*det;
+ dest[2][0] = (d*h-g*e)*det;
+ dest[2][1] = -(a*h-g*b)*det;
+ dest[2][2] = (a*e-d*b)*det;
+}
+
+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])
+ + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
+}
+
+static inline void m3x3_transpose( m3x3f src, m3x3f dest )
+{
+ 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];
+
+ dest[0][0] = a;
dest[0][1] = d;
dest[0][2] = g;
dest[1][0] = b;
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],
}
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 );
static inline void m3x3_scale( m3x3f m, v3f v )
{
- m[0][0] = m[0][0] * v[0];
- m[0][1] = m[0][1] * v[0];
- m[0][2] = m[0][2] * v[0];
+ v3_muls( m[0], v[0], m[0] );
+ v3_muls( m[1], v[1], m[1] );
+ v3_muls( m[2], v[2], m[2] );
+}
- m[1][0] = m[1][0] * v[1];
- m[1][1] = m[1][1] * v[1];
- m[1][2] = m[1][2] * v[1];
+static inline void m3x3_scalef( m3x3f m, 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 );
}
/*
- * Matrix 4x3
+ * -----------------------------------------------------------------------------
+ * Section 4.c 4x3 matrices
+ * -----------------------------------------------------------------------------
*/
#define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\
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],
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],
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;
/*
* 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;
* 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_scale( m4x3f m, float s )
-{
- v3_muls( m[0], s, m[0] );
- v3_muls( m[1], s, m[1] );
- v3_muls( m[2], s, m[2] );
-}
-
-static inline void m4x3_scalev( m4x3f m, v3f v )
-{
- v3_muls(m[0], v[0], m[0]);
- v3_muls(m[1], v[1], m[1]);
- v3_muls(m[2], v[2], m[2]);
-}
-
-static inline void m4x3_rotate_x( m4x3f m, float angle )
+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 );
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 );
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 );
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] );
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_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;
+ 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;
}
/*
- * 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,
b = -t;
- 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 * 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] = -(ffar + fnear) / (ffar - fnear);
- m[2][3] = -1.0f;
- m[3][0] = 0.0f;
- m[3][1] = 0.0f;
- m[3][2] = -2.0f * ffar * fnear / (ffar - fnear);
+ 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 * 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] = -(ffar + fnear) / (ffar - fnear);
+ m[2][3] = -1.0f;
+
+ m[3][0] = 0.0f;
+ m[3][1] = 0.0f;
+ m[3][2] = -2.0f * ffar * fnear / (ffar - fnear);
m[3][3] = 0.0f;
}
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],
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],
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];
p[2] = p[2] / l;
}
-static inline int plane_intersect( double a[4], double b[4],
- double c[4], double p[4] )
+static int plane_intersect3( v4f a, v4f b, v4f c, v3f p )
{
- double const epsilon = 1e-8f;
+ f32 const epsilon = 1e-6f;
- double x[3];
- double d;
+ v3f x;
+ v3_cross( a, b, x );
+ f32 d = v3_dot( x, c );
- x[0] = a[1] * b[2] - a[2] * b[1];
- x[1] = a[2] * b[0] - a[0] * b[2];
- x[2] = a[0] * b[1] - a[1] * b[0];
-
- d = x[0] * c[0] + x[1] * c[1] + x[2] * c[2];
-
- if( d < epsilon && d > -epsilon ) return 0;
-
- p[0] = (b[1] * c[2] - b[2] * c[1]) * -a[3];
- p[1] = (b[2] * c[0] - b[0] * c[2]) * -a[3];
- p[2] = (b[0] * c[1] - b[1] * c[0]) * -a[3];
-
- p[0] += (c[1] * a[2] - c[2] * a[1]) * -b[3];
- p[1] += (c[2] * a[0] - c[0] * a[2]) * -b[3];
- p[2] += (c[0] * a[1] - c[1] * a[0]) * -b[3];
-
- p[0] += (a[1] * b[2] - a[2] * b[1]) * -c[3];
- p[1] += (a[2] * b[0] - a[0] * b[2]) * -c[3];
- p[2] += (a[0] * b[1] - a[1] * b[0]) * -c[3];
-
- p[0] = -p[0] / d;
- p[1] = -p[1] / d;
- p[2] = -p[2] / d;
+ if( (d < epsilon) && (d > -epsilon) ) return 0;
+
+ v3f v0, v1, v2;
+ v3_cross( b, c, v0 );
+ v3_cross( c, a, v1 );
+ v3_cross( a, b, v2 );
+
+ v3_muls( v0, a[3], p );
+ v3_muladds( p, v1, b[3], p );
+ v3_muladds( p, v2, c[3], p );
+ v3_divs( p, d, p );
return 1;
}
-static inline double plane_polarity( double p[4], double a[3] )
+static int plane_intersect2( v4f a, v4f b, v3f p, v3f n )
+{
+ f32 const epsilon = 1e-6f;
+
+ v4f c;
+ v3_cross( a, b, c );
+ f32 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 )
+{
+ f32 d0 = v3_dot( a, plane ) - plane[3],
+ d1 = v3_dot( b, plane ) - plane[3];
+
+ if( d0*d1 < 0.0f )
+ {
+ 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 inline f64 plane_polarity( f64 p[4], f64 a[3] )
{
return
(a[0] * p[0] + a[1] * p[1] + a[2] * p[2])
;
}
-/* Quaternions */
+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;
+}
-static inline void q_identity( v4f q )
+/*
+ * -----------------------------------------------------------------------------
+ * Section 5.c Closest point functions
+ * -----------------------------------------------------------------------------
+ */
+
+/*
+ * These closest point tests were learned from Real-Time Collision Detection by
+ * Christer Ericson
+ */
+static f32 closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
+ f32 *s, f32 *t, v3f c1, v3f c2)
{
- q[0] = 0.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 1.0f;
+ v3f d1,d2,r;
+ v3_sub( q1, p1, d1 );
+ v3_sub( q2, p2, d2 );
+ v3_sub( p1, p2, r );
+
+ f32 a = v3_length2( d1 ),
+ e = v3_length2( d2 ),
+ f = v3_dot( d2, r );
+
+ const f32 kEpsilon = 0.0001f;
+
+ if( a <= kEpsilon && e <= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = 0.0f;
+ v3_copy( p1, c1 );
+ v3_copy( p2, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+
+ return v3_length2( v0 );
+ }
+
+ if( a<= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = vg_clampf( f / e, 0.0f, 1.0f );
+ }
+ else
+ {
+ f32 c = v3_dot( d1, r );
+ if( e <= kEpsilon )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else
+ {
+ f32 b = v3_dot(d1,d2),
+ d = a*e-b*b;
+
+ if( d != 0.0f )
+ {
+ *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+ }
+ else
+ {
+ *s = 0.0f;
+ }
+
+ *t = (b*(*s)+f) / e;
+
+ if( *t < 0.0f )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else if( *t > 1.0f )
+ {
+ *t = 1.0f;
+ *s = vg_clampf((b-c)/a,0.0f,1.0f);
+ }
+ }
+ }
+
+ v3_muladds( p1, d1, *s, c1 );
+ v3_muladds( p2, d2, *t, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+ return v3_length2( v0 );
}
-static inline void q_axis_angle( v4f q, v3f axis, float angle )
+static int point_inside_aabb( boxf box, v3f point )
{
- 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;
+ if((point[0]<=box[1][0]) && (point[1]<=box[1][1]) && (point[2]<=box[1][2]) &&
+ (point[0]>=box[0][0]) && (point[1]>=box[0][1]) && (point[2]>=box[0][2]) )
+ return 1;
+ else
+ return 0;
}
-static inline void q_mul( v4f q, v4f q1, v4f d )
+static void closest_point_aabb( v3f p, boxf box, v3f dest )
{
- 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 );
+ v3_maxv( p, box[0], dest );
+ v3_minv( dest, box[1], dest );
}
-static inline void q_normalize( v4f q )
+static void closest_point_obb( v3f p, boxf box,
+ m4x3f mtx, m4x3f inv_mtx, v3f dest )
{
- float s = 1.0f/ sqrtf(v4_dot(q,q));
- q[0] *= s;
- q[1] *= s;
- q[2] *= s;
- q[3] *= s;
+ v3f local;
+ m4x3_mulv( inv_mtx, p, local );
+ closest_point_aabb( local, box, local );
+ m4x3_mulv( mtx, local, dest );
}
-static inline void q_inv( v4f q, v4f d )
+static f32 closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
{
- 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;
+ v3f v0, v1;
+ v3_sub( b, a, v0 );
+ v3_sub( point, a, v1 );
+
+ 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;
}
-static inline void q_nlerp( v4f a, v4f b, float t, v4f d )
+static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
{
- if( v4_dot(a,b) < 0.0f )
+ v3f ab, ac, ap;
+ f32 d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
{
- v4_muls( b, -1.0f, d );
- v4_lerp( a, d, t, d );
+ v3_copy( tri[0], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
}
- else
- v4_lerp( a, b, t, d );
- q_normalize( d );
-}
+ /* Region outside B */
+ v3f bp;
+ f32 d3, d4;
-static inline void q_m3x3( v4f q, m3x3f d )
-{
- float
- l = v4_length(q),
- s = l > 0.0f? 2.0f/l: 0.0f,
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
- 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];
+ if( d3 >= 0.0f && d4 <= d3 )
+ {
+ v3_copy( tri[1], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Edge region of AB */
+ f32 vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+ {
+ f32 v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
- 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;
+ /* Region outside C */
+ v3f cp;
+ f32 d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
+ {
+ v3_copy( tri[2], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of AC */
+ f32 vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ f32 w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of BC */
+ f32 va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ f32 w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ f32 d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, dest );
}
-static void m3x3_q( m3x3f m, v4f q )
+enum contact_type
{
- float diag, r, rinv;
+ k_contact_type_default,
+ k_contact_type_disabled,
+ k_contact_type_edge
+};
- diag = m[0][0] + m[1][1] + m[2][2];
- if( diag >= 0.0f )
+static enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
+{
+ v3f ab, ac, ap;
+ f32 d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
{
- 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] )
+ v3_copy( tri[0], dest );
+ return k_contact_type_default;
+ }
+
+ /* Region outside B */
+ v3f bp;
+ f32 d3, d4;
+
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
+
+ if( d3 >= 0.0f && d4 <= d3 )
{
- 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] )
+ v3_copy( tri[1], dest );
+ return k_contact_type_edge;
+ }
+
+ /* Edge region of AB */
+ f32 vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
{
- 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
+ f32 v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ return k_contact_type_edge;
+ }
+
+ /* Region outside C */
+ v3f cp;
+ f32 d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
{
- 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]);
+ v3_copy( tri[2], dest );
+ return k_contact_type_edge;
+ }
+
+ /* Region of AC */
+ f32 vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ f32 w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ return k_contact_type_edge;
+ }
+
+ /* Region of BC */
+ f32 va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ f32 w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ return k_contact_type_edge;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ f32 d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, dest );
+
+ return k_contact_type_default;
+}
+
+static void closest_point_elipse( v2f p, v2f e, v2f o )
+{
+ v2f pabs, ei, e2, ve, t;
+
+ v2_abs( p, pabs );
+ v2_div( (v2f){ 1.0f, 1.0f }, e, ei );
+ v2_mul( e, e, e2 );
+ v2_mul( ei, (v2f){ e2[0]-e2[1], e2[1]-e2[0] }, ve );
+
+ v2_fill( t, 0.70710678118654752f );
+
+ for( int i=0; i<3; i++ ){
+ v2f v, u, ud, w;
+
+ v2_mul( ve, t, v ); /* ve*t*t*t */
+ v2_mul( v, t, v );
+ v2_mul( v, t, v );
+
+ v2_sub( pabs, v, u );
+ v2_normalize( u );
+
+ v2_mul( t, e, ud );
+ v2_sub( ud, v, ud );
+
+ v2_muls( u, v2_length( ud ), u );
+
+ v2_add( v, u, w );
+ v2_mul( w, ei, w );
+
+ v2_maxv( (v2f){0.0f,0.0f}, w, t );
+ v2_normalize( t );
}
+
+ v2_mul( t, e, o );
+ v2_copysign( o, p );
+}
+
+/*
+ * -----------------------------------------------------------------------------
+ * 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);
}
-static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist )
+/* Time of intersection with ray vs triangle */
+static int ray_tri( v3f tri[3], v3f co,
+ 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];
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 */
a = v3_dot( v0, h );
+
if( a > -kEpsilon && a < kEpsilon )
return 0;
else return 0;
}
-static inline float vg_randf(void)
+/* time of intersection with ray vs sphere */
+static int ray_sphere( v3f c, f32 r,
+ v3f co, v3f dir, f32 *t )
{
- return (float)rand()/(float)(RAND_MAX);
-}
+ v3f m;
+ v3_sub( co, c, m );
-static inline void vg_rand_dir(v3f dir)
-{
- dir[0] = vg_randf();
- dir[1] = vg_randf();
- dir[2] = vg_randf();
+ f32 b = v3_dot( m, dir ),
+ c1 = v3_dot( m, m ) - r*r;
- v3_muls( dir, 2.0f, dir );
- v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir );
+ /* 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;
+
+ f32 discr = b*b - c1;
- v3_normalize( dir );
+ /* A negative discriminant corresponds to ray missing sphere */
+ if( discr < 0.0f )
+ return 0;
+
+ /*
+ * Ray now found to intersect sphere, compute smallest t value of
+ * intersection
+ */
+ *t = -b - sqrtf( discr );
+
+ /* If t is negative, ray started inside sphere so clamp t to zero */
+ if( *t < 0.0f )
+ *t = 0.0f;
+
+ return 1;
}
-static inline void vg_rand_sphere( v3f co )
+/*
+ * time of intersection of ray vs cylinder
+ * The cylinder does not have caps but is finite
+ *
+ * Heavily adapted from regular segment vs cylinder from:
+ * Real-Time Collision Detection
+ */
+static int ray_uncapped_finite_cylinder( v3f q, v3f p, f32 r,
+ v3f co, v3f dir, f32 *t )
{
- vg_rand_dir(co);
- v3_muls( co, cbrtf( vg_randf() ), co );
+ v3f d, m, n, sb;
+ v3_muladds( co, dir, 1.0f, sb );
+
+ v3_sub( q, p, d );
+ v3_sub( co, p, m );
+ v3_sub( sb, co, n );
+
+ f32 md = v3_dot( m, d ),
+ nd = v3_dot( n, d ),
+ dd = v3_dot( d, d ),
+ nn = v3_dot( n, n ),
+ mn = v3_dot( m, n ),
+ a = dd*nn - nd*nd,
+ k = v3_dot( m, m ) - r*r,
+ c = dd*k - md*md;
+
+ if( fabsf(a) < 0.00001f )
+ {
+ /* Segment runs parallel to cylinder axis */
+ return 0;
+ }
+
+ f32 b = dd*mn - nd*md,
+ discr = b*b - a*c;
+
+ if( discr < 0.0f )
+ return 0; /* No real roots; no intersection */
+
+ *t = (-b - sqrtf(discr)) / a;
+ if( *t < 0.0f )
+ return 0; /* Intersection behind ray */
+
+ /* Check within cylinder segment */
+ if( md + (*t)*nd < 0.0f )
+ return 0;
+
+ if( md + (*t)*nd > dd )
+ return 0;
+
+ /* Segment intersects cylinder between the endcaps; t is correct */
+ return 1;
}
-static inline int vg_randint(int max)
+/*
+ * Time of intersection of sphere and triangle. Origin must be outside the
+ * colliding area. This is a fairly long procedure.
+ */
+static int spherecast_triangle( v3f tri[3],
+ v3f co, v3f dir, f32 r, f32 *t, v3f n )
{
- return rand()%max;
+ v3f sum[3];
+ v3f v0, v1;
+
+ v3_sub( tri[1], tri[0], v0 );
+ v3_sub( tri[2], tri[0], v1 );
+ v3_cross( v0, v1, n );
+ v3_normalize( n );
+ v3_muladds( tri[0], n, r, sum[0] );
+ v3_muladds( tri[1], n, r, sum[1] );
+ v3_muladds( tri[2], n, r, sum[2] );
+
+ int hit = 0;
+ f32 t_min = INFINITY,
+ t1;
+
+ if( ray_tri( sum, co, dir, &t1, 0 ) ){
+ t_min = vg_minf( t_min, t1 );
+ hit = 1;
+ }
+
+ /*
+ * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
+ */
+#if 0
+ for( int i=0; i<3; i++ ){
+ if( ray_sphere( tri[i], r, co, dir, &t1 ) ){
+ t_min = vg_minf( t_min, t1 );
+ hit = 1;
+ }
+ }
+#endif
+
+ for( int i=0; i<3; i++ ){
+ int i0 = i,
+ i1 = (i+1)%3;
+
+ if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) ){
+ if( t1 < t_min ){
+ t_min = t1;
+
+ v3f co1, ct, cx;
+ v3_add( dir, co, co1 );
+ v3_lerp( co, co1, t_min, ct );
+
+ closest_point_segment( tri[i0], tri[i1], ct, cx );
+ v3_sub( ct, cx, n );
+ v3_normalize( n );
+ }
+
+ hit = 1;
+ }
+ }
+
+ *t = t_min;
+ return hit;
}
-static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p )
+/*
+ * -----------------------------------------------------------------------------
+ * Section 5.e Curves
+ * -----------------------------------------------------------------------------
+ */
+
+static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, f32 t, v3f p )
{
- float tt = t*t,
+ f32 tt = t*t,
ttt = tt*t;
v3_muls( p1, ttt, p );
v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p );
}
-#endif /* VG_M_H */
+static void eval_bezier3( v3f p0, v3f p1, v3f p2, f32 t, v3f p )
+{
+ 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->index<MT_STATE_VECTOR_LENGTH; rand->index++){
+ 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; kk<MT_STATE_VECTOR_LENGTH-MT_STATE_VECTOR_M; kk++ ){
+ y = (rand->mt[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( ; kk<MT_STATE_VECTOR_LENGTH-1; kk++ ){
+ y = (rand->mt[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 );
+
+ v3_normalize( dir );
+}
+
+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 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 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;
+
+ out_dir[0] = sinf(a) * sinf(r);
+ out_dir[1] = cosf(a) * sinf(r);
+ out_dir[2] = cosf(r);
+}
+
+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) );
+}
projects / vg.git / blobdiff