X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=src%2Fcxr_math.h;fp=src%2Fcxr_math.h;h=68f32ffe035ce6c79837ab4b764bf810b299874f;hb=e17cd5d7861480f19148b1eb5b15a520df838b1f;hp=031c61e4d2f7d787dc53a0efe7c58e9660083956;hpb=b5ba4ec14b8e215a38cd9072de17fe40f48cea48;p=convexer.git diff --git a/src/cxr_math.h b/src/cxr_math.h index 031c61e..68f32ff 100644 --- a/src/cxr_math.h +++ b/src/cxr_math.h @@ -1,9 +1,15 @@ -// Copyright (C) 2021 Harry Godden (hgn) +/* Copyright (C) 2021 Harry Godden (hgn) + * + * Straightforward implementations for: + * Vector 2,3,4 + * Simple Matrices in 3x3 and 4x3 + * Plane maths + * Other useful geometric functions + */ #define CXR_INLINE static inline #define CXR_PIf 3.14159265358979323846264338327950288f -// Simple min/max replacements CXR_INLINE double cxr_minf( double a, double b ) { return a < b? a: b; @@ -29,15 +35,14 @@ CXR_INLINE double cxr_clampf( double v, double a, double b ) return cxr_minf( b, cxr_maxf( a, v ) ); } -// Convert degrees to radians CXR_INLINE double cxr_rad( double deg ) { return deg * CXR_PIf / 180.0f; } -// Vector 2 -// ================================================================================================================== - +/* + * Vector 2 Functions + */ CXR_INLINE void v2_zero( v2f a ) { a[0] = 0.0; a[1] = 0.0; @@ -132,8 +137,9 @@ CXR_INLINE void v2_normalize( v2f a ) v2_muls( a, 1.0 / v2_length( a ), a ); } -// Vector 3 -// ================================================================================================================== +/* + * Vector 3 Functions + */ CXR_INLINE void v3_zero( v3f a ) { @@ -267,9 +273,9 @@ CXR_INLINE void v3_fill( v3f a, double v ) a[2] = v; } -// Vector 4 -// ================================================================================================================== - +/* + * Vector 4 Functions + */ CXR_INLINE void v4_copy( v4f a, v4f b ) { b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3]; @@ -285,8 +291,9 @@ CXR_INLINE void v4_muls( v4f a, double s, v4f d ) d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s; d[3] = a[3]*s; } -// Matrix 3x3 -//====================================================================================================== +/* + * Matrix 3x3 + */ CXR_INLINE void m3x3_inv_transpose( m3x3f src, m3x3f dest ) { @@ -321,8 +328,9 @@ CXR_INLINE void m3x3_mulv( m3x3f m, v3f v, v3f d ) v3_copy( res, d ); } -// Matrix 4x3 -// ================================================================================================================== +/* + * Matrix 4x3 + */ #define M4X3_IDENTITY {{1.0f, 0.0f, 0.0f, },\ { 0.0f, 1.0f, 0.0f, },\ @@ -387,8 +395,9 @@ CXR_INLINE void m4x3_mulv( m4x3f m, v3f v, v3f d ) v3_copy( res, d ); } -// Affine transforms - +/* + * Affine transformations + */ CXR_INLINE void m4x3_translate( m4x3f m, v3f v ) { v3_muladds( m[3], m[0], v[0], m[3] ); @@ -451,7 +460,6 @@ CXR_INLINE void m4x3_rotate_z( m4x3f m, double angle ) m4x3_mul( m, t, m ); } -// Warning: These functions are unoptimized.. CXR_INLINE void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point ) { v3f v; @@ -507,63 +515,28 @@ CXR_INLINE void tri_to_plane( v3f a, v3f b, v3f c, v4f plane ) plane[3] = v3_dot( plane, a ); } -// TODO update this code to use normal v3_x functions -CXR_INLINE void tri_to_plane1( double a[3], double b[3], double c[3], double p[4] ) -{ - double edge0[3]; - double edge1[3]; - double l; - - edge0[0] = b[0] - a[0]; - edge0[1] = b[1] - a[1]; - edge0[2] = b[2] - a[2]; - - edge1[0] = c[0] - a[0]; - edge1[1] = c[1] - a[1]; - edge1[2] = c[2] - a[2]; - - p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1]; - p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2]; - p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0]; - - l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]); - p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l; - - p[0] = p[0] / l; - p[1] = p[1] / l; - p[2] = p[2] / l; -} - -CXR_INLINE int plane_intersect( double a[4], double b[4], double c[4], double p[3] ) +CXR_INLINE int plane_intersect( v4f a, v4f b, v4f c, v3f p ) { double const epsilon = 0.001; - double x[3]; + v3f x, bc, ca, ab; double d; - 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]; + v3_cross( a, b, x ); + d = v3_dot( x, c ); 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; + v3_cross(b,c,bc); + v3_cross(c,a,ca); + v3_cross(a,b,ab); + + v3_muls( bc, -a[3], p ); + v3_muladds( p, ca, -b[3], p ); + v3_muladds( p, ab, -c[3], p ); + + v3_negate( p, p ); + v3_divs( p, d, p ); return 1; } @@ -574,12 +547,11 @@ CXR_INLINE void normal_to_plane( v3f normal, v3f p, v4f plane ) plane[3] = v3_dot( normal, p ); } -CXR_INLINE double plane_polarity( double p[4], double a[3] ) +CXR_INLINE double plane_polarity( v4f p, v3f a ) { 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]) - ; + v3_dot( a, p ) + - (p[0]*p[3]*p[0] + p[1]*p[3]*p[1] + p[2]*p[3]*p[2]); } CXR_INLINE void plane_project_point( v4f plane, v3f a, v3f d )