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