X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=vg_m.h;h=a8b2091fe3ae51b5a6124820e1994189368e042b;hb=8c967ecac9d925e5fe14ecaeb6c0e4b2b23dfac4;hp=3c78466b68ca046f23890a14aaf033bf36f3be55;hpb=0aea2ef68a5ed32fc940673402a1b4b67f38d4d3;p=vg.git

diff --git a/vg_m.h b/vg_m.h
index 3c78466..a8b2091 100644
--- a/vg_m.h
+++ b/vg_m.h
@@ -269,6 +269,11 @@ static inline void v3_add( 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 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];
+}
+
 static inline void v4_add( v4f a, v4f b, v4f d )
 {
    d[0] = a[0]+b[0]; 
@@ -282,6 +287,11 @@ static inline void v3_sub( 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 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];
@@ -596,6 +606,13 @@ static void m3x3_diagonal( m3x3f a, float v )
    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;
@@ -704,13 +721,16 @@ static inline void m3x3_translate( m3x3f m, v3f v )
 
 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, float f )
+{
+   v3f v;
+   v3_fill( v, f );
+   m3x3_scale( m, v );
 }
 
 static inline void m3x3_rotate( m3x3f m, float angle )
@@ -732,227 +752,6 @@ static inline void m3x3_rotate( m3x3f m, float angle )
    m[1][2] = m02 * -s + m12 * c;
 }
 
-/*
- * Matrix 4x3
- */
-
-#define M4X3_IDENTITY   {{1.0f, 0.0f, 0.0f, },\
-                        { 0.0f, 1.0f, 0.0f, },\
-                        { 0.0f, 0.0f, 1.0f, },\
-                        { 0.0f, 0.0f, 0.0f }}
-
-static inline void m4x3_to_3x3( m4x3f a, m3x3f b )
-{
-   v3_copy( a[0], b[0] );
-   v3_copy( a[1], b[1] );
-   v3_copy( a[2], b[2] );
-}
-
-static inline void m4x3_invert_affine( m4x3f a, m4x3f b )
-{
-   m3x3_transpose( a, b );
-   m3x3_mulv( b, a[3], b[3] );
-   v3_negate( b[3], b[3] );
-}
-
-static void m4x3_invert_full( m4x3f src, m4x3f dst )
-{
-  float 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],
-        i = src[2][0], j = src[2][1], k = src[2][2],
-        m = src[3][0], n = src[3][1], o = src[3][2];
-
-   t2 = j*o - n*k;
-   t4 = i*o - m*k;
-   t5 = i*n - m*j;
-   
-   dst[0][0] =  f*k - g*j;
-   dst[1][0] =-(e*k - g*i);
-   dst[2][0] =  e*j - f*i;
-   dst[3][0] =-(e*t2 - f*t4 + g*t5);
-   
-   dst[0][1] =-(b*k - c*j);
-   dst[1][1] =  a*k - c*i;
-   dst[2][1] =-(a*j - b*i);
-   dst[3][1] =  a*t2 - b*t4 + c*t5;
-   
-   t2 = f*o - n*g;
-   t4 = e*o - m*g; 
-   t5 = e*n - m*f;
-   
-   dst[0][2] =  b*g - c*f ;
-   dst[1][2] =-(a*g - c*e );
-   dst[2][2] =  a*f - b*e ;
-   dst[3][2] =-(a*t2 - b*t4 + c * t5);
-
-   det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]);
-   v3_muls( dst[0], det, dst[0] );
-   v3_muls( dst[1], det, dst[1] );
-   v3_muls( dst[2], det, dst[2] );
-   v3_muls( dst[3], det, dst[3] );
-}
-
-static inline void m4x3_copy( m4x3f a, m4x3f b )
-{
-   v3_copy( a[0], b[0] );
-   v3_copy( a[1], b[1] );
-   v3_copy( a[2], b[2] );
-   v3_copy( a[3], b[3] );
-}
-
-static inline void m4x3_identity( m4x3f a )
-{
-   m4x3f id = M4X3_IDENTITY;
-   m4x3_copy( id, a );
-}
-
-static inline void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) 
-{
-   float 
-   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],
-   a30 = a[3][0], a31 = a[3][1], a32 = a[3][2],
-   b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
-   b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
-   b20 = b[2][0], b21 = b[2][1], b22 = b[2][2],
-   b30 = b[3][0], b31 = b[3][1], b32 = b[3][2];
-   
-   d[0][0] = a00*b00 + a10*b01 + a20*b02;
-   d[0][1] = a01*b00 + a11*b01 + a21*b02;
-   d[0][2] = a02*b00 + a12*b01 + a22*b02;
-   d[1][0] = a00*b10 + a10*b11 + a20*b12;
-   d[1][1] = a01*b10 + a11*b11 + a21*b12;
-   d[1][2] = a02*b10 + a12*b11 + a22*b12;
-   d[2][0] = a00*b20 + a10*b21 + a20*b22;
-   d[2][1] = a01*b20 + a11*b21 + a21*b22;
-   d[2][2] = a02*b20 + a12*b21 + a22*b22;
-   d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30;
-   d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31;
-   d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32;
-}
-
-static inline void m4x3_mulv( m4x3f m, v3f v, v3f d ) 
-{
-   v3f res;
-  
-   res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0];
-   res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1];
-   res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2];
-
-   v3_copy( res, d );
-}
-
-/* 
- * Transform plane ( xyz, distance )
- */
-static inline void m4x3_mulp( m4x3f m, v4f p, v4f d )
-{
-   v3f o;
-
-   v3_muls( p, p[3], o );
-   m4x3_mulv( m, o, o );
-   m3x3_mulv( m, p, d );
-   
-   d[3] = v3_dot( o, d );
-}
-
-/*
- * Affine transforms
- */
-
-static inline 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 )
-{
-   m4x3f t = M4X3_IDENTITY;
-   float c, s;
-
-   c = cosf( angle );
-   s = sinf( angle );
-
-   t[1][1] =  c;
-   t[1][2] =  s;
-   t[2][1] = -s;
-   t[2][2] =  c;
-
-   m4x3_mul( m, t, m );
-}
-
-static inline void m4x3_rotate_y( m4x3f m, float angle )
-{
-   m4x3f t = M4X3_IDENTITY;
-   float c, s;
-
-   c = cosf( angle );
-   s = sinf( angle );
-
-   t[0][0] =  c;
-   t[0][2] = -s;
-   t[2][0] =  s;
-   t[2][2] =  c;
-
-   m4x3_mul( m, t, m );
-}
-
-static inline void m4x3_rotate_z( m4x3f m, float angle )
-{
-   m4x3f t = M4X3_IDENTITY;
-   float c, s;
-
-   c = cosf( angle );
-   s = sinf( angle );
-
-   t[0][0] =  c;
-   t[0][1] =  s;
-   t[1][0] = -s;
-   t[1][1] =  c;
-
-   m4x3_mul( m, t, m );
-}
-
-static inline void m4x3_expand( m4x3f m, m4x4f d )
-{
-   v3_copy( m[0], d[0] );
-   v3_copy( m[1], d[1] );
-   v3_copy( m[2], d[2] );
-   v3_copy( m[3], d[3] );
-   d[0][3] = 0.0f;
-   d[1][3] = 0.0f;
-   d[2][3] = 0.0f;
-   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] );
@@ -1001,26 +800,6 @@ static inline void box_init_inf( boxf box )
    v3_fill( box[1], -INFINITY );
 }
 
-static inline void m4x3_transform_aabb( m4x3f m, boxf box )
-{
-   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_aabb1( boxf box, v3f co, v3f dir_inv, float dist )
 {
    v3f v0, v1;
@@ -1376,8 +1155,7 @@ static inline void q_inv( v4f q, v4f d )
 
 static inline void q_nlerp( v4f a, v4f b, float t, v4f d )
 {
-   if( v4_dot(a,b) < 0.0f )
-   {
+   if( v4_dot(a,b) < 0.0f ){
       v4_muls( b, -1.0f, d );
       v4_lerp( a, d, t, d );
    }
@@ -1490,6 +1268,251 @@ enum contact_type
    k_contact_type_edge
 };
 
+/*
+ * Matrix 4x3
+ */
+
+#define M4X3_IDENTITY   {{1.0f, 0.0f, 0.0f, },\
+                        { 0.0f, 1.0f, 0.0f, },\
+                        { 0.0f, 0.0f, 1.0f, },\
+                        { 0.0f, 0.0f, 0.0f }}
+
+static inline void m4x3_to_3x3( m4x3f a, m3x3f b )
+{
+   v3_copy( a[0], b[0] );
+   v3_copy( a[1], b[1] );
+   v3_copy( a[2], b[2] );
+}
+
+static inline void m4x3_invert_affine( m4x3f a, m4x3f b )
+{
+   m3x3_transpose( a, b );
+   m3x3_mulv( b, a[3], b[3] );
+   v3_negate( b[3], b[3] );
+}
+
+static void m4x3_invert_full( m4x3f src, m4x3f dst )
+{
+  float 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],
+        i = src[2][0], j = src[2][1], k = src[2][2],
+        m = src[3][0], n = src[3][1], o = src[3][2];
+
+   t2 = j*o - n*k;
+   t4 = i*o - m*k;
+   t5 = i*n - m*j;
+   
+   dst[0][0] =  f*k - g*j;
+   dst[1][0] =-(e*k - g*i);
+   dst[2][0] =  e*j - f*i;
+   dst[3][0] =-(e*t2 - f*t4 + g*t5);
+   
+   dst[0][1] =-(b*k - c*j);
+   dst[1][1] =  a*k - c*i;
+   dst[2][1] =-(a*j - b*i);
+   dst[3][1] =  a*t2 - b*t4 + c*t5;
+   
+   t2 = f*o - n*g;
+   t4 = e*o - m*g; 
+   t5 = e*n - m*f;
+   
+   dst[0][2] =  b*g - c*f ;
+   dst[1][2] =-(a*g - c*e );
+   dst[2][2] =  a*f - b*e ;
+   dst[3][2] =-(a*t2 - b*t4 + c * t5);
+
+   det = 1.0f / (a * dst[0][0] + b * dst[1][0] + c * dst[2][0]);
+   v3_muls( dst[0], det, dst[0] );
+   v3_muls( dst[1], det, dst[1] );
+   v3_muls( dst[2], det, dst[2] );
+   v3_muls( dst[3], det, dst[3] );
+}
+
+static inline void m4x3_copy( m4x3f a, m4x3f b )
+{
+   v3_copy( a[0], b[0] );
+   v3_copy( a[1], b[1] );
+   v3_copy( a[2], b[2] );
+   v3_copy( a[3], b[3] );
+}
+
+static inline void m4x3_identity( m4x3f a )
+{
+   m4x3f id = M4X3_IDENTITY;
+   m4x3_copy( id, a );
+}
+
+static void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) 
+{
+   float 
+   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],
+   a30 = a[3][0], a31 = a[3][1], a32 = a[3][2],
+   b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
+   b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
+   b20 = b[2][0], b21 = b[2][1], b22 = b[2][2],
+   b30 = b[3][0], b31 = b[3][1], b32 = b[3][2];
+   
+   d[0][0] = a00*b00 + a10*b01 + a20*b02;
+   d[0][1] = a01*b00 + a11*b01 + a21*b02;
+   d[0][2] = a02*b00 + a12*b01 + a22*b02;
+   d[1][0] = a00*b10 + a10*b11 + a20*b12;
+   d[1][1] = a01*b10 + a11*b11 + a21*b12;
+   d[1][2] = a02*b10 + a12*b11 + a22*b12;
+   d[2][0] = a00*b20 + a10*b21 + a20*b22;
+   d[2][1] = a01*b20 + a11*b21 + a21*b22;
+   d[2][2] = a02*b20 + a12*b21 + a22*b22;
+   d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30;
+   d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31;
+   d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32;
+}
+
+#if 0 /* shat appf mingw wstringop-overflow */
+inline
+#endif
+static void m4x3_mulv( m4x3f m, v3f v, v3f d ) 
+{
+   v3f res;
+  
+   res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0];
+   res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1];
+   res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2];
+
+   v3_copy( res, d );
+}
+
+/* 
+ * Transform plane ( xyz, distance )
+ */
+static void m4x3_mulp( m4x3f m, v4f p, v4f d )
+{
+   v3f o;
+
+   v3_muls( p, p[3], o );
+   m4x3_mulv( m, o, o );
+   m3x3_mulv( m, p, d );
+   
+   d[3] = v3_dot( o, d );
+}
+
+/*
+ * Affine transforms
+ */
+
+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 void m4x3_rotate_x( m4x3f m, float angle )
+{
+   m4x3f t = M4X3_IDENTITY;
+   float c, s;
+
+   c = cosf( angle );
+   s = sinf( angle );
+
+   t[1][1] =  c;
+   t[1][2] =  s;
+   t[2][1] = -s;
+   t[2][2] =  c;
+
+   m4x3_mul( m, t, m );
+}
+
+static void m4x3_rotate_y( m4x3f m, float angle )
+{
+   m4x3f t = M4X3_IDENTITY;
+   float c, s;
+
+   c = cosf( angle );
+   s = sinf( angle );
+
+   t[0][0] =  c;
+   t[0][2] = -s;
+   t[2][0] =  s;
+   t[2][2] =  c;
+
+   m4x3_mul( m, t, m );
+}
+
+static void m4x3_rotate_z( m4x3f m, float angle )
+{
+   m4x3f t = M4X3_IDENTITY;
+   float c, s;
+
+   c = cosf( angle );
+   s = sinf( angle );
+
+   t[0][0] =  c;
+   t[0][1] =  s;
+   t[1][0] = -s;
+   t[1][1] =  c;
+
+   m4x3_mul( m, t, m );
+}
+
+static void m4x3_expand( m4x3f m, m4x4f d )
+{
+   v3_copy( m[0], d[0] );
+   v3_copy( m[1], d[1] );
+   v3_copy( m[2], d[2] );
+   v3_copy( m[3], d[3] );
+   d[0][3] = 0.0f;
+   d[1][3] = 0.0f;
+   d[2][3] = 0.0f;
+   d[3][3] = 1.0f;
+}
+
+static void m4x3_decompose( m4x3f m, v3f co, v4f q, v3f s )
+{
+   v3_copy( m[3], co );
+   s[0] = v3_length(m[0]);
+   s[1] = v3_length(m[1]);
+   s[2] = v3_length(m[2]);
+
+   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] );
+
+   m3x3_q( rot, q );
+}
+
+static 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 void m4x3_transform_aabb( m4x3f m, boxf box )
+{
+   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] } );
+}
+
 /*
  * -----------------------------------------------------------------------------
  *                        Closest point functions
@@ -2036,6 +2059,7 @@ static int spherecast_triangle( v3f tri[3],
 
 static inline float vg_randf(void)
 {
+   /* TODO: replace with our own rand */
    return (float)rand()/(float)(RAND_MAX);
 }
 
@@ -2073,4 +2097,13 @@ static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p )
    v3_muladds( p, p0, 3.0f*tt  -ttt -3.0f*t +1.0f, p );
 }
 
+static void eval_bezier3( v3f p0, v3f p1, v3f p2, float t, v3f p )
+{
+   float 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 );
+}
+
 #endif /* VG_M_H */