-}
-
-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
- */
-
-#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] );
-}