#define VG_PIf 3.14159265358979323846264338327950288f
#define VG_TAUf 6.28318530717958647692528676655900576f
+static u32 vg_ftu32( float a )
+{
+ u32 *ptr = (u32 *)(&a);
+ return *ptr;
+}
+
+static int vg_isinff( float a )
+{
+ return ((vg_ftu32(a)) & 0x7FFFFFFFU) == 0x7F800000U;
+}
+
+static int vg_isnanf( float a )
+{
+ return !vg_isinff(a) && ((vg_ftu32(a)) & 0x7F800000U) == 0x7F800000U;
+}
+
+static int vg_validf( float a )
+{
+ return ((vg_ftu32(a)) & 0x7F800000U) != 0x7F800000U;
+}
+
static inline float vg_minf( float a, float b )
{
return a < b? a: b;
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];
static inline void v3_div( v3f a, v3f b, v3f d )
{
- d[0] = a[0]/b[0]; d[1] = a[1]/b[1]; d[2] = a[2]/b[2];
+ d[0] = b[0]!=0.0f? a[0]/b[0]: INFINITY;
+ d[1] = b[1]!=0.0f? a[1]/b[1]: INFINITY;
+ d[2] = b[2]!=0.0f? a[2]/b[2]: INFINITY;
}
static inline void v3_muls( v3f a, float s, v3f d )
d[0] = a[0]*s; d[1] = a[1]*s; d[2] = a[2]*s;
}
+static inline void v3_fill( v3f a, float v )
+{
+ a[0] = v;
+ a[1] = v;
+ a[2] = v;
+}
+
static inline void v3_divs( v3f a, float s, v3f d )
{
- d[0] = a[0]/s; d[1] = a[1]/s; d[2] = a[2]/s;
+ if( s == 0.0f )
+ v3_fill( d, INFINITY );
+ else
+ {
+ d[0] = a[0]/s;
+ d[1] = a[1]/s;
+ d[2] = a[2]/s;
+ }
}
static inline void v3_muladds( v3f a, v3f b, float s, v3f d )
return vg_maxf( vg_maxf( a[0], a[1] ), a[2] );
}
-static inline void v3_fill( v3f a, float v )
-{
- a[0] = v;
- a[1] = v;
- a[2] = v;
-}
-
static inline void v3_floor( v3f a, v3f b )
{
b[0] = floorf( a[0] );
static inline float v4_dot( v4f a, v4f b )
{
- return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*a[3];
+ return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
}
static inline float v4_length( v4f a )
{ 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 )
{ 0.0f, 0.0f, 0.0f, 0.0f }}
static void m4x4_projection( m4x4f m, float angle,
- float ratio, float fnear, float ffar )
+ float ratio, float fnear, float ffar )
{
float scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear,
r = ratio * scale,
t = scale,
b = -t;
- m[0][0] = 2.0f * fnear / (r - l);
- m[0][1] = 0.0f;
- m[0][2] = 0.0f;
- m[0][3] = 0.0f;
- m[1][0] = 0.0f;
- m[1][1] = 2.0f * fnear / (t - b);
- m[1][2] = 0.0f;
- m[1][3] = 0.0f;
- m[2][0] = (r + l) / (r - l);
- m[2][1] = (t + b) / (t - b);
- m[2][2] = -(ffar + fnear) / (ffar - fnear);
- m[2][3] = -1.0f;
- m[3][0] = 0.0f;
- m[3][1] = 0.0f;
- m[3][2] = -2.0f * ffar * fnear / (ffar - fnear);
+ m[0][0] = 2.0f * fnear / (r - l);
+ m[0][1] = 0.0f;
+ m[0][2] = 0.0f;
+ m[0][3] = 0.0f;
+
+ m[1][0] = 0.0f;
+ m[1][1] = 2.0f * fnear / (t - b);
+ m[1][2] = 0.0f;
+ m[1][3] = 0.0f;
+
+ m[2][0] = (r + l) / (r - l);
+ m[2][1] = (t + b) / (t - b);
+ m[2][2] = -(ffar + fnear) / (ffar - fnear);
+ m[2][3] = -1.0f;
+
+ m[3][0] = 0.0f;
+ m[3][1] = 0.0f;
+ m[3][2] = -2.0f * ffar * fnear / (ffar - fnear);
m[3][3] = 0.0f;
}
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
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 int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist )
+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,
+ k_contact_type_disabled,
+ k_contact_type_edge
+};
+
+/*
+ * -----------------------------------------------------------------------------
+ * Closest point functions
+ * -----------------------------------------------------------------------------
+ */
+
+/*
+ * These closest point tests were learned from Real-Time Collision Detection by
+ * Christer Ericson
+ */
+VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
+ float *s, float *t, v3f c1, v3f c2)
+{
+ v3f d1,d2,r;
+ v3_sub( q1, p1, d1 );
+ v3_sub( q2, p2, d2 );
+ v3_sub( p1, p2, r );
+
+ float a = v3_length2( d1 ),
+ e = v3_length2( d2 ),
+ f = v3_dot( d2, r );
+
+ const float kEpsilon = 0.0001f;
+
+ if( a <= kEpsilon && e <= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = 0.0f;
+ v3_copy( p1, c1 );
+ v3_copy( p2, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+
+ return v3_length2( v0 );
+ }
+
+ if( a<= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = vg_clampf( f / e, 0.0f, 1.0f );
+ }
+ else
+ {
+ float c = v3_dot( d1, r );
+ if( e <= kEpsilon )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else
+ {
+ float b = v3_dot(d1,d2),
+ d = a*e-b*b;
+
+ if( d != 0.0f )
+ {
+ *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+ }
+ else
+ {
+ *s = 0.0f;
+ }
+
+ *t = (b*(*s)+f) / e;
+
+ if( *t < 0.0f )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else if( *t > 1.0f )
+ {
+ *t = 1.0f;
+ *s = vg_clampf((b-c)/a,0.0f,1.0f);
+ }
+ }
+ }
+
+ v3_muladds( p1, d1, *s, c1 );
+ v3_muladds( p2, d2, *t, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+ 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 );
+ v3_minv( dest, box[1], dest );
+}
+
+VG_STATIC void closest_point_obb( v3f p, boxf box,
+ m4x3f mtx, m4x3f inv_mtx, v3f dest )
+{
+ v3f local;
+ m4x3_mulv( inv_mtx, p, local );
+ closest_point_aabb( local, box, local );
+ m4x3_mulv( mtx, local, dest );
+}
+
+VG_STATIC float closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+{
+ v3f v0, v1;
+ v3_sub( b, a, v0 );
+ v3_sub( point, a, v1 );
+
+ float t = v3_dot( v1, v0 ) / v3_length2(v0);
+ t = vg_clampf(t,0.0f,1.0f);
+ v3_muladds( a, v0, t, dest );
+ return t;
+}
+
+VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+{
+ v3f ab, ac, ap;
+ float d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
+ {
+ v3_copy( tri[0], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside B */
+ v3f bp;
+ float d3, d4;
+
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
+
+ if( d3 >= 0.0f && d4 <= d3 )
+ {
+ v3_copy( tri[1], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Edge region of AB */
+ float vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+ {
+ float v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside C */
+ v3f cp;
+ float d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
+ {
+ v3_copy( tri[2], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of AC */
+ float vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ float w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of BC */
+ float va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ float w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ float d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, dest );
+}
+
+VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
+{
+ v3f ab, ac, ap;
+ float d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
+ {
+ v3_copy( tri[0], dest );
+ return k_contact_type_default;
+ }
+
+ /* Region outside B */
+ v3f bp;
+ float d3, d4;
+
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
+
+ if( d3 >= 0.0f && d4 <= d3 )
+ {
+ v3_copy( tri[1], dest );
+ return k_contact_type_edge;
+ }
+
+ /* Edge region of AB */
+ float vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+ {
+ float v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ return k_contact_type_edge;
+ }
+
+ /* Region outside C */
+ v3f cp;
+ float d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
+ {
+ v3_copy( tri[2], dest );
+ return k_contact_type_edge;
+ }
+
+ /* Region of AC */
+ float vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ float w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ return k_contact_type_edge;
+ }
+
+ /* Region of BC */
+ float va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ float w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ return k_contact_type_edge;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ float d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, 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
+ */
+
+/* Time of intersection with ray vs triangle */
+static int ray_tri( v3f tri[3], v3f co,
+ v3f dir, float *dist )
{
float const kEpsilon = 0.00001f;
/* Parralel */
a = v3_dot( v0, h );
+
if( a > -kEpsilon && a < kEpsilon )
return 0;
else return 0;
}
+/* time of intersection with ray vs sphere */
+static int ray_sphere( v3f c, float r,
+ v3f co, v3f dir, float *t )
+{
+ v3f m;
+ v3_sub( co, c, m );
+
+ float b = v3_dot( m, dir ),
+ c1 = v3_dot( m, m ) - r*r;
+
+ /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */
+ if( c1 > 0.0f && b > 0.0f )
+ return 0;
+
+ float discr = b*b - c1;
+
+ /* A negative discriminant corresponds to ray missing sphere */
+ if( discr < 0.0f )
+ return 0;
+
+ /*
+ * Ray now found to intersect sphere, compute smallest t value of
+ * intersection
+ */
+ *t = -b - sqrtf( discr );
+
+ /* If t is negative, ray started inside sphere so clamp t to zero */
+ if( *t < 0.0f )
+ *t = 0.0f;
+
+ return 1;
+}
+
+/*
+ * time of intersection of ray vs cylinder
+ * The cylinder does not have caps but is finite
+ *
+ * Heavily adapted from regular segment vs cylinder from:
+ * Real-Time Collision Detection
+ */
+static int ray_uncapped_finite_cylinder( v3f q, v3f p, float r,
+ v3f co, v3f dir, float *t )
+{
+ v3f d, m, n, sb;
+ v3_muladds( co, dir, 1.0f, sb );
+
+ v3_sub( q, p, d );
+ v3_sub( co, p, m );
+ v3_sub( sb, co, n );
+
+ float md = v3_dot( m, d ),
+ nd = v3_dot( n, d ),
+ dd = v3_dot( d, d ),
+ nn = v3_dot( n, n ),
+ mn = v3_dot( m, n ),
+ a = dd*nn - nd*nd,
+ k = v3_dot( m, m ) - r*r,
+ c = dd*k - md*md;
+
+ if( fabsf(a) < 0.00001f )
+ {
+ /* Segment runs parallel to cylinder axis */
+ return 0;
+ }
+
+ float b = dd*mn - nd*md,
+ discr = b*b - a*c;
+
+ if( discr < 0.0f )
+ return 0; /* No real roots; no intersection */
+
+ *t = (-b - sqrtf(discr)) / a;
+ if( *t < 0.0f )
+ return 0; /* Intersection behind ray */
+
+ /* Check within cylinder segment */
+ if( md + (*t)*nd < 0.0f )
+ return 0;
+
+ if( md + (*t)*nd > dd )
+ return 0;
+
+ /* Segment intersects cylinder between the endcaps; t is correct */
+ return 1;
+}
+
+/*
+ * Time of intersection of sphere and triangle. Origin must be outside the
+ * colliding area. This is a fairly long procedure.
+ */
+static int spherecast_triangle( v3f tri[3],
+ v3f co, v3f dir, float r, float *t, v3f n )
+{
+ v3f sum[3];
+ v3f v0, v1;
+
+ v3_sub( tri[1], tri[0], v0 );
+ v3_sub( tri[2], tri[0], v1 );
+ v3_cross( v0, v1, n );
+ v3_normalize( n );
+ v3_muladds( tri[0], n, r, sum[0] );
+ v3_muladds( tri[1], n, r, sum[1] );
+ v3_muladds( tri[2], n, r, sum[2] );
+
+ int hit = 0;
+ float t_min = INFINITY,
+ t1;
+
+ if( ray_tri( sum, co, dir, &t1 ) )
+ {
+ t_min = vg_minf( t_min, t1 );
+ hit = 1;
+ }
+
+ /*
+ * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
+ */
+#if 0
+ for( int i=0; i<3; i++ )
+ {
+ if( ray_sphere( tri[i], r, co, dir, &t1 ) )
+ {
+ t_min = vg_minf( t_min, t1 );
+ hit = 1;
+ }
+ }
+#endif
+
+ for( int i=0; i<3; i++ )
+ {
+ int i0 = i,
+ i1 = (i+1)%3;
+
+ if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) )
+ {
+ if( t1 < t_min )
+ {
+ t_min = t1;
+
+ v3f co1, ct, cx;
+ v3_add( dir, co, co1 );
+ v3_lerp( co, co1, t_min, ct );
+
+ closest_point_segment( tri[i0], tri[i1], ct, cx );
+ v3_sub( ct, cx, n );
+ v3_normalize( n );
+ }
+
+ hit = 1;
+ }
+ }
+
+ *t = t_min;
+ return hit;
+}
+
static inline float vg_randf(void)
{
return (float)rand()/(float)(RAND_MAX);