- * Planes (double precision)
- */
-static inline void tri_to_plane( double a[3], double b[3],
- double c[3], double p[4] )
-{
- double edge0[3];
- double edge1[3];
- double l;
-
- edge0[0] = b[0] - a[0];
- edge0[1] = b[1] - a[1];
- edge0[2] = b[2] - a[2];
-
- edge1[0] = c[0] - a[0];
- edge1[1] = c[1] - a[1];
- edge1[2] = c[2] - a[2];
-
- p[0] = edge0[1] * edge1[2] - edge0[2] * edge1[1];
- p[1] = edge0[2] * edge1[0] - edge0[0] * edge1[2];
- p[2] = edge0[0] * edge1[1] - edge0[1] * edge1[0];
-
- l = sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]);
- p[3] = (p[0] * a[0] + p[1] * a[1] + p[2] * a[2]) / l;
-
- p[0] = p[0] / l;
- p[1] = p[1] / l;
- p[2] = p[2] / l;
-}
-
-static int plane_intersect3( v4f a, v4f b, v4f c, v3f p )
-{
- float const epsilon = 1e-6f;
-
- v3f x;
- v3_cross( a, b, x );
- float d = v3_dot( x, c );
-
- 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
- (a[0] * p[0] + a[1] * p[1] + a[2] * p[2])
- -(p[0]*p[3] * p[0] + p[1]*p[3] * p[1] + p[2]*p[3] * p[2])
- ;
-}
-
-/* Quaternions */
-
-static inline void q_identity( v4f q )
-{
- q[0] = 0.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 1.0f;
-}
-
-static inline void q_axis_angle( v4f q, v3f axis, float angle )
-{
- float a = angle*0.5f,
- c = cosf(a),
- s = sinf(a);
-
- q[0] = s*axis[0];
- q[1] = s*axis[1];
- q[2] = s*axis[2];
- q[3] = c;
-}
-
-static inline void q_mul( v4f q, v4f q1, v4f d )
-{
- v4f t;
- t[0] = q[3]*q1[0] + q[0]*q1[3] + q[1]*q1[2] - q[2]*q1[1];
- t[1] = q[3]*q1[1] - q[0]*q1[2] + q[1]*q1[3] + q[2]*q1[0];
- t[2] = q[3]*q1[2] + q[0]*q1[1] - q[1]*q1[0] + q[2]*q1[3];
- t[3] = q[3]*q1[3] - q[0]*q1[0] - q[1]*q1[1] - q[2]*q1[2];
- v4_copy( t, d );
-}
-
-static inline void q_normalize( v4f q )
-{
- float s = 1.0f/ sqrtf(v4_dot(q,q));
- q[0] *= s;
- q[1] *= s;
- q[2] *= s;
- q[3] *= s;
-}
-
-static inline void q_inv( v4f q, v4f d )
-{
- float s = 1.0f / v4_dot(q,q);
- d[0] = -q[0]*s;
- d[1] = -q[1]*s;
- d[2] = -q[2]*s;
- d[3] = q[3]*s;
-}
-
-static inline void q_nlerp( v4f a, v4f b, float t, v4f d )
-{
- if( v4_dot(a,b) < 0.0f ){
- v4_muls( b, -1.0f, d );
- v4_lerp( a, d, t, d );
- }
- else
- v4_lerp( a, b, 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
- l = v4_length(q),
- s = l > 0.0f? 2.0f/l: 0.0f,
-
- xx = s*q[0]*q[0], xy = s*q[0]*q[1], wx = s*q[3]*q[0],
- yy = s*q[1]*q[1], yz = s*q[1]*q[2], wy = s*q[3]*q[1],
- zz = s*q[2]*q[2], xz = s*q[0]*q[2], wz = s*q[3]*q[2];
-
- d[0][0] = 1.0f - yy - zz;
- d[1][1] = 1.0f - xx - zz;
- d[2][2] = 1.0f - xx - yy;
- d[0][1] = xy + wz;
- d[1][2] = yz + wx;
- d[2][0] = xz + wy;
- d[1][0] = xy - wz;
- d[2][1] = yz - wx;
- d[0][2] = xz - wy;
-}
-
-static void m3x3_q( m3x3f m, v4f q )
-{
- float diag, r, rinv;
-
- diag = m[0][0] + m[1][1] + m[2][2];
- if( diag >= 0.0f )
- {
- r = sqrtf( 1.0f + diag );
- rinv = 0.5f / r;
- q[0] = rinv * (m[1][2] - m[2][1]);
- q[1] = rinv * (m[2][0] - m[0][2]);
- q[2] = rinv * (m[0][1] - m[1][0]);
- q[3] = r * 0.5f;
- }
- else if( m[0][0] >= m[1][1] && m[0][0] >= m[2][2] )
- {
- r = sqrtf( 1.0f - m[1][1] - m[2][2] + m[0][0] );
- rinv = 0.5f / r;
- q[0] = r * 0.5f;
- q[1] = rinv * (m[0][1] + m[1][0]);
- q[2] = rinv * (m[0][2] + m[2][0]);
- q[3] = rinv * (m[1][2] - m[2][1]);
- }
- else if( m[1][1] >= m[2][2] )
- {
- r = sqrtf( 1.0f - m[0][0] - m[2][2] + m[1][1] );
- rinv = 0.5f / r;
- q[0] = rinv * (m[0][1] + m[1][0]);
- q[1] = r * 0.5f;
- q[2] = rinv * (m[1][2] + m[2][1]);
- q[3] = rinv * (m[2][0] - m[0][2]);
- }
- else
- {
- r = sqrtf( 1.0f - m[0][0] - m[1][1] + m[2][2] );
- rinv = 0.5f / r;
- q[0] = rinv * (m[0][2] + m[2][0]);
- q[1] = rinv * (m[1][2] + m[2][1]);
- q[2] = r * 0.5f;
- q[3] = rinv * (m[0][1] - m[1][0]);
- }
-}
-
-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
-};
-
-/*
- * Matrix 4x3