Refactor el big #2

[convexer.git] / src / cxr_math.h

diff --git a/src/cxr_math.h b/src/cxr_math.h
index 031c61e4d2f7d787dc53a0efe7c58e9660083956..68f32ffe035ce6c79837ab4b764bf810b299874f 100644 (file)
--- 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 )