return a - floorf( a );
}
+
+__attribute__ ((deprecated))
static float stable_force( float current, float diff )
{
float fnew = current + 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;
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;
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
*/
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];
{ 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] );
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;
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],
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;
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] );
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 )
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;
+ float const epsilon = 1e-6f;
- 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];
+ v3f x;
+ v3_cross( a, b, x );
+ float d = v3_dot( x, c );
- d = x[0] * c[0] + x[1] * c[1] + x[2] * c[2];
-
- 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;
+ 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
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 );
}
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
}
}
+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,
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 );
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
*/
/* Parralel */
a = v3_dot( v0, h );
+
if( a > -kEpsilon && a < kEpsilon )
return 0;
static inline float vg_randf(void)
{
+ /* TODO: replace with our own rand */
return (float)rand()/(float)(RAND_MAX);
}
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 */
projects / vg.git / blobdiff