X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=vg_m.h;h=8e6e56c0c425d00f3b07c789c4d3bbcc6dd601fa;hb=c70327d950761c26f2a7c55eb622be7c042bb550;hp=d6da3a1b933e2ff14790895728e6a5918d344903;hpb=fa8c2262459bbc760875536323baa5c05349ed75;p=vg.git

diff --git a/vg_m.h b/vg_m.h
index d6da3a1..8e6e56c 100644
--- a/vg_m.h
+++ b/vg_m.h
@@ -56,6 +56,8 @@ static inline float vg_fractf( float a )
    return a - floorf( a );
 }
 
+
+__attribute__ ((deprecated))
 static float stable_force( float current, float diff )
 {
    float fnew = current + diff;
@@ -66,6 +68,11 @@ static float stable_force( float current, float diff )
    return fnew;
 }
 
+static float vg_cfrictf( float current, float F )
+{
+   return -vg_signf(current) * vg_minf( F, fabsf(current) );
+}
+
 static inline int vg_min( int a, int b )
 {
    return a < b? a: b;
@@ -146,6 +153,12 @@ 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 v2_abs( v2f a, v2f d )
+{
+   d[0] = fabsf( a[0] );
+   d[1] = fabsf( a[1] );
+}
+
 static inline void v2_muls( v2f a, float s, v2f d )
 {
    d[0] = a[0]*s; d[1] = a[1]*s;
@@ -225,6 +238,19 @@ static inline void v2_floor( v2f a, v2f b )
    b[1] = floorf( a[1] );
 }
 
+static inline void v2_fill( v2f a, float v )
+{
+   a[0] = v;
+   a[1] = v;
+}
+
+/* copysign of b to a */
+static inline void v2_copysign( v2f a, v2f b )
+{
+   a[0] = copysignf( a[0], b[0] );
+   a[1] = copysignf( a[1], b[1] );
+}
+
 /*
  * Vector 3
  */
@@ -243,11 +269,29 @@ 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]; 
+   d[1] = a[1]+b[1];
+   d[2] = a[2]+b[2];
+   d[3] = a[3]+b[3];
+}
+
 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];
@@ -520,6 +564,27 @@ static inline void m2x2_create_rotation( m2x2f a, float theta )
                         { 0.0f, 0.0f, 0.0f, }}
 
 
+/* 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;
+}
+
+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 inline void m3x3_copy( m3x3f a, m3x3f b )
 {
    v3_copy( a[0], b[0] );
@@ -533,6 +598,14 @@ static inline void m3x3_identity( m3x3f a )
    m3x3_copy( id, a );
 }
 
+static void m3x3_diagonal( m3x3f a, float v )
+{
+   m3x3_identity( a );
+   a[0][0] = v;
+   a[1][1] = v;
+   a[2][2] = v;
+}
+
 static inline void m3x3_zero( m3x3f a )
 {
    m3x3f z = M3X3_ZERO;
@@ -561,6 +634,13 @@ static inline void m3x3_inv( m3x3f src, m3x3f dest )
    dest[2][2] =  (a*e-d*b)*det;
 }
 
+static float 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 )
 {
    float a = src[0][0], b = src[0][1], c = src[0][2],
@@ -634,13 +714,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 )
@@ -662,227 +745,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] );
@@ -931,36 +793,16 @@ static inline void box_init_inf( boxf box )
    v3_fill( box[1], -INFINITY );
 }
 
-static inline void m4x3_transform_aabb( m4x3f m, boxf box )
+int ray_aabb1( boxf box, v3f co, v3f dir_inv, float dist )
 {
-   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;
+   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 );
+   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] );
@@ -969,7 +811,7 @@ int ray_aabb( boxf box, v3f co, v3f dir, float dist )
    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;
+   return (tmax >= tmin) && (tmin <= dist) && (tmax >= 0.0f);
 }
 
 static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up )
@@ -1186,41 +1028,69 @@ static inline void tri_to_plane( double a[3], double b[3],
    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;
-   
-   double x[3];
-   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];
+   float const epsilon = 1e-6f;
    
-   if( d < epsilon && d > -epsilon ) return 0;
+   v3f x;
+   v3_cross( a, b, x );
+   float d = v3_dot( x, c );
    
-   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;
 }
 
+int plane_intersect2( v4f a, v4f b, v3f p, v3f n )
+{
+   float const epsilon = 1e-6f;
+
+   v4f c;
+   v3_cross( a, b, c );
+   float 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 )
+{
+   float d0 = v3_dot( a, plane ) - plane[3],
+         d1 = v3_dot( b, plane ) - plane[3];
+
+   if( d0*d1 < 0.0f )
+   {
+      float 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 double plane_polarity( double p[4], double a[3] )
 {
    return 
@@ -1278,8 +1148,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 );
    }
@@ -1289,6 +1158,26 @@ static inline void q_nlerp( v4f a, v4f b, float t, v4f d )
    q_normalize( d );
 }
 
+static void euler_m3x3( v3f angles, m3x3f d )
+{
+   float cosY = cosf( angles[0] ),
+         sinY = sinf( angles[0] ),
+         cosP = cosf( angles[1] ),
+         sinP = sinf( angles[1] ),
+         cosR = cosf( angles[2] ),
+         sinR = sinf( angles[2] );
+
+   d[2][0] = -sinY * cosP;
+   d[2][1] =  sinP;
+   d[2][2] =  cosY * cosP;
+
+   d[0][0] =  cosY * cosR;
+   d[0][1] =  sinR;
+   d[0][2] =  sinY * cosR;
+
+   v3_cross( d[0], d[2], d[1] );
+}
+
 static inline void q_m3x3( v4f q, m3x3f d )
 {
    float
@@ -1353,6 +1242,18 @@ static void m3x3_q( m3x3f m, v4f q )
    }
 }
 
+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 );
+}
+
 enum contact_type
 {
    k_contact_type_default,
@@ -1360,6 +1261,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
@@ -1447,6 +1593,15 @@ VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
    return v3_length2( v0 );
 }
 
+VG_STATIC int point_inside_aabb( boxf box, v3f point )
+{
+   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;
+}
+
 VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest )
 {
    v3_maxv( p, box[0], dest );
@@ -1648,6 +1803,45 @@ VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f 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 );
+}
+
 /*
  * Raycasts
  */
@@ -1675,6 +1869,7 @@ static int ray_tri( v3f tri[3], v3f co,
    
    /* Parralel */
    a = v3_dot( v0, h );
+
    if( a > -kEpsilon && a < kEpsilon )
       return 0;
 
@@ -1857,6 +2052,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);
 }
 
@@ -1894,4 +2090,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 */