heebie

[vg.git] / vg_m.h

diff --git a/vg_m.h b/vg_m.h
index fbb3c45317915d592d4fbb8120390027999944d9..3c78466b68ca046f23890a14aaf033bf36f3be55 100644 (file)
--- 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;
@@ -1014,7 +1021,7 @@ static inline void m4x3_transform_aabb( m4x3f m, boxf box )
    m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], b[2] } );
 }
 
-int ray_aabb( boxf box, v3f co, v3f dir, float dist )
+int ray_aabb1( boxf box, v3f co, v3f dir_inv, float dist )
 {
    v3f v0, v1;
    float tmin, tmax;
@@ -1022,8 +1029,8 @@ int ray_aabb( boxf box, v3f co, v3f dir, float dist )
    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] );
@@ -1032,7 +1039,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 )
@@ -1249,41 +1256,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 
@@ -1352,6 +1387,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
@@ -1416,6 +1471,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,
@@ -1510,6 +1577,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 );
@@ -1777,6 +1853,7 @@ static int ray_tri( v3f tri[3], v3f co,
    
    /* Parralel */
    a = v3_dot( v0, h );
+
    if( a > -kEpsilon && a < kEpsilon )
       return 0;