support maths for constraints

[vg.git] / vg_m.h

/* Copyright (C) 2021-2022 Harry Godden (hgn) - All Rights Reserved */

#ifndef VG_M_H
#define VG_M_H

#include "vg_platform.h"
#include <math.h>
#include <stdlib.h>

#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;
}

static inline float vg_maxf( float a, float b )
{
   return a > b? a: b;
}

static inline float vg_clampf( float a, float min, float max )
{
   return vg_minf( max, vg_maxf( a, min ) );
}

static inline float vg_signf( float a )
{
   return a < 0.0f? -1.0f: 1.0f;
}

static inline float vg_fractf( float a )
{
   return a - floorf( a );
}

static float stable_force( float current, float diff )
{
   float fnew = current + diff;

   if( fnew * current < 0.0f )
      return 0.0f;

   return fnew;
}

static inline int vg_min( int a, int b )
{
   return a < b? a: b;
}

static inline int vg_max( int a, int b )
{
   return a > b? a: b;
}

static inline float vg_rad( float deg )
{
   return deg * VG_PIf / 180.0f;
}

/*
 * Vector 3
 */
static inline void v2_copy( v2f a, v2f b )
{
   b[0] = a[0]; b[1] = a[1];
}

static inline void v2_zero( v2f a )
{
   a[0] = 0.f; a[1] = 0.f;
}

static inline void v2i_copy( v2i a, v2i b )
{
   b[0] = a[0]; b[1] = a[1];
}

static inline int v2i_eq( v2i a, v2i b )
{
   return ((a[0] == b[0]) && (a[1] == b[1]));
}

static inline void v2i_add( v2i a, v2i b, v2i d )
{
   d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
}

static inline void v2i_sub( v2i a, v2i b, v2i d )
{
   d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
}

static inline void v2_minv( v2f a, v2f b, v2f dest ) 
{
   dest[0] = vg_minf(a[0], b[0]);
   dest[1] = vg_minf(a[1], b[1]);
}

static inline void v2_maxv( v2f a, v2f b, v2f dest ) 
{
   dest[0] = vg_maxf(a[0], b[0]);
   dest[1] = vg_maxf(a[1], b[1]);
}

static inline void v2_sub( v2f a, v2f b, v2f d )
{
   d[0] = a[0]-b[0]; d[1] = a[1]-b[1];
}

static inline float v2_dot( v2f a, v2f b )
{
   return a[0] * b[0] + a[1] * b[1];
}

static inline float v2_cross( v2f a, v2f b )
{
   return a[0]*b[1] - a[1]*b[0];
}

static inline void v2_add( v2f a, v2f b, v2f d )
{
   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;
}

static inline void v2_divs( v2f a, float s, v2f d )
{
   d[0] = a[0]/s; d[1] = a[1]/s;
}

static inline void v2_mul( v2f a, v2f b, v2f d )
{
   d[0] = a[0]*b[0]; 
   d[1] = a[1]*b[1];
}

static inline void v2_div( v2f a, v2f b, v2f d )
{
   d[0] = a[0]/b[0]; d[1] = a[1]/b[1];
}

static inline void v2_muladd( v2f a, v2f b, v2f s, v2f d )
{
   d[0] = a[0]+b[0]*s[0]; 
   d[1] = a[1]+b[1]*s[1];
}

static inline void v2_muladds( v2f a, v2f b, float s, v2f d )
{
   d[0] = a[0]+b[0]*s; 
   d[1] = a[1]+b[1]*s;
}

static inline float v2_length2( v2f a )
{
   return a[0]*a[0] + a[1]*a[1];
}

static inline float v2_length( v2f a )
{
   return sqrtf( v2_length2( a ) );
}

static inline float v2_dist2( v2f a, v2f b )
{
   v2f delta;
   v2_sub( a, b, delta );
   return v2_length2( delta );
}

static inline float v2_dist( v2f a, v2f b )
{
   return sqrtf( v2_dist2( a, b ) );
}

static inline void v2_lerp( v2f a, v2f b, float t, v2f d )
{
   d[0] = a[0] + t*(b[0]-a[0]);
   d[1] = a[1] + t*(b[1]-a[1]);
}

static inline void v2_normalize( v2f a )
{
   v2_muls( a, 1.0f / v2_length( a ), a );
}

static void v2_normalize_clamp( v2f a )
{
   float l2 = v2_length2( a );
   if( l2 > 1.0f )
      v2_muls( a, 1.0f/sqrtf(l2), a );
}

static inline void v2_floor( v2f a, v2f b )
{
   b[0] = floorf( a[0] );
   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
 */
static inline void v3_zero( v3f a )
{
   a[0] = 0.f; a[1] = 0.f; a[2] = 0.f;
}

static inline void v3_copy( v3f a, v3f b )
{
   b[0] = a[0]; b[1] = a[1]; b[2] = a[2];
}

static inline void v3_add( 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 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 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] = 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 )
{
   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 )
{
   d[0] = a[0]+b[0]*s; d[1] = a[1]+b[1]*s; d[2] = a[2]+b[2]*s;
}

static inline void v3_muladd( v2f a, v2f b, v2f s, v2f d )
{
   d[0] = a[0]+b[0]*s[0]; 
   d[1] = a[1]+b[1]*s[1];
   d[2] = a[2]+b[2]*s[2];
}

static inline float v3_dot( v3f a, v3f b )
{
   return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

static inline void v3_cross( v3f a, v3f b, v3f dest )
{
   v3f d;
   d[0] = a[1]*b[2] - a[2]*b[1];
   d[1] = a[2]*b[0] - a[0]*b[2];
   d[2] = a[0]*b[1] - a[1]*b[0];
   v3_copy( d, dest );
}

static inline float v3_length2( v3f a )
{
   return v3_dot( a, a );
}

static inline float v3_length( v3f a )
{
   return sqrtf( v3_length2( a ) );
}

static inline float v3_dist2( v3f a, v3f b )
{
   v3f delta;
   v3_sub( a, b, delta );
   return v3_length2( delta );
}

static inline float v3_dist( v3f a, v3f b )
{
   return sqrtf( v3_dist2( a, b ) );
}

static inline void v3_normalize( v3f a )
{
   v3_muls( a, 1.f / v3_length( a ), a );
}

static inline float vg_lerpf( float a, float b, float t )
{
   return a + t*(b-a);
}

static inline double vg_lerp( double a, double b, double t )
{
   return a + t*(b-a);
}

/* correctly lerp around circular period -pi -> pi */
static float vg_alerpf( float a, float b, float t )
{
   float d = fmodf( b-a, VG_TAUf ),
         s = fmodf( 2.0f*d, VG_TAUf ) - d;
   return a + s*t;
}

static inline void v3_lerp( v3f a, v3f b, float t, v3f d )
{
   d[0] = a[0] + t*(b[0]-a[0]);
   d[1] = a[1] + t*(b[1]-a[1]);
   d[2] = a[2] + t*(b[2]-a[2]);
}

static inline void v3_minv( v3f a, v3f b, v3f dest ) 
{
   dest[0] = vg_minf(a[0], b[0]);
   dest[1] = vg_minf(a[1], b[1]);
   dest[2] = vg_minf(a[2], b[2]);
}

static inline void v3_maxv( v3f a, v3f b, v3f dest ) 
{
   dest[0] = vg_maxf(a[0], b[0]);
   dest[1] = vg_maxf(a[1], b[1]);
   dest[2] = vg_maxf(a[2], b[2]);
}

static inline float v3_minf( v3f a )
{
   return vg_minf( vg_minf( a[0], a[1] ), a[2] );
}

static inline float v3_maxf( v3f a )
{
   return vg_maxf( vg_maxf( a[0], a[1] ), a[2] );
}

static inline void v3_floor( v3f a, v3f b )
{
   b[0] = floorf( a[0] );
   b[1] = floorf( a[1] );
   b[2] = floorf( a[2] );
}

static inline void v3_ceil( v3f a, v3f b )
{
   b[0] = ceilf( a[0] );
   b[1] = ceilf( a[1] );
   b[2] = ceilf( a[2] );
}

static inline void v3_negate( v3f a, v3f b )
{
   b[0] = -a[0];
   b[1] = -a[1];
   b[2] = -a[2];
}

static inline void v3_rotate( v3f v, float angle, v3f axis, v3f d ) 
{
  v3f v1, v2, k;
  float c, s;

  c = cosf( angle );
  s = sinf( angle );

  v3_copy( axis, k );
  v3_normalize( k );
  v3_muls( v, c, v1 );
  v3_cross( k, v, v2 );
  v3_muls( v2, s, v2 );
  v3_add( v1, v2, v1 );
  v3_muls( k, v3_dot(k, v) * (1.0f - c), v2);
  v3_add( v1, v2, d );
}

/*
 * Vector 4
 */
static inline void v4_copy( v4f a, v4f b )
{
   b[0] = a[0]; b[1] = a[1]; b[2] = a[2]; b[3] = a[3];
}

static inline void v4_zero( v4f a )
{
   a[0] = 0.f; a[1] = 0.f; a[2] = 0.f; a[3] = 0.f;
}

static inline void v4_muls( v4f a, float s, v4f d )
{
   d[0] = a[0]*s; 
   d[1] = a[1]*s;
   d[2] = a[2]*s;
   d[3] = a[3]*s;
}

static inline void v4_muladds( v4f a, v4f b, float s, v4f d )
{
   d[0] = a[0]+b[0]*s; 
   d[1] = a[1]+b[1]*s;
   d[2] = a[2]+b[2]*s;
   d[3] = a[3]+b[3]*s;
}

static inline void v4_lerp( v4f a, v4f b, float t, v4f d )
{
   d[0] = a[0] + t*(b[0]-a[0]);
   d[1] = a[1] + t*(b[1]-a[1]);
   d[2] = a[2] + t*(b[2]-a[2]);
   d[3] = a[3] + t*(b[3]-a[3]);
}

static inline float v4_dot( v4f a, v4f b )
{
   return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3];
}

static inline float v4_length( v4f a )
{
   return sqrtf( v4_dot(a,a) );
}

/*
 * Matrix 2x2
 */

#define M2X2_INDENTIY    {{1.0f, 0.0f, }, \
                          { 0.0f, 1.0f, }}
                          
#define M2X2_ZERO         {{0.0f, 0.0f, }, \
                        { 0.0f, 0.0f, }}

static inline void m2x2_copy( m2x2f a, m2x2f b )
{
   v2_copy( a[0], b[0] );
   v2_copy( a[1], b[1] );
}

static inline void m2x2_identity( m2x2f a )
{
   m2x2f id = M2X2_INDENTIY;
   m2x2_copy( id, a );
}

static inline void m2x2_create_rotation( m2x2f a, float theta )
{
   float s, c;
   
   s = sinf( theta );
   c = cosf( theta );
   
   a[0][0] =  c;
   a[0][1] = -s;
   a[1][0] =  s;
   a[1][1] =  c;
}

/*
 * Matrix 3x3
 */

#define M3X3_IDENTITY   {{1.0f, 0.0f, 0.0f, },\
                        { 0.0f, 1.0f, 0.0f, },\
                        { 0.0f, 0.0f, 1.0f, }}
                        
#define M3X3_ZERO         {{0.0f, 0.0f, 0.0f, },\
                        { 0.0f, 0.0f, 0.0f, },\
                        { 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] );
   v3_copy( a[1], b[1] );
   v3_copy( a[2], b[2] );
}

static inline void m3x3_identity( m3x3f a )
{
   m3x3f id = M3X3_IDENTITY;
   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;
   m3x3_copy( z, a );
}

static inline void m3x3_inv( m3x3f src, m3x3f dest )
{
   float a = src[0][0], b = src[0][1], c = src[0][2],
         d = src[1][0], e = src[1][1], f = src[1][2],
         g = src[2][0], h = src[2][1], i = src[2][2];

   float det =    1.f / 
               (+a*(e*i-h*f)
                -b*(d*i-f*g)
                +c*(d*h-e*g));
                
   dest[0][0] =  (e*i-h*f)*det;
   dest[0][1] = -(b*i-c*h)*det;
   dest[0][2] =  (b*f-c*e)*det;
   dest[1][0] = -(d*i-f*g)*det;
   dest[1][1] =  (a*i-c*g)*det;
   dest[1][2] = -(a*f-d*c)*det;
   dest[2][0] =  (d*h-g*e)*det;
   dest[2][1] = -(a*h-g*b)*det;
   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],
         d = src[1][0], e = src[1][1], f = src[1][2],
         g = src[2][0], h = src[2][1], i = src[2][2];
         
   dest[0][0] = a;
   dest[0][1] = d;
   dest[0][2] = g;
   dest[1][0] = b;
   dest[1][1] = e;
   dest[1][2] = h;
   dest[2][0] = c;
   dest[2][1] = f;
   dest[2][2] = i;
}

static inline void m3x3_mul( m3x3f a, m3x3f b, m3x3f d )
{
   float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
         a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
         a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],

         b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
         b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
         b20 = b[2][0], b21 = b[2][1], b22 = b[2][2];

   d[0][0] = a00*b00 + a10*b01 + a20*b02;
   d[0][1] = a01*b00 + a11*b01 + a21*b02;
   d[0][2] = a02*b00 + a12*b01 + a22*b02;
   d[1][0] = a00*b10 + a10*b11 + a20*b12;
   d[1][1] = a01*b10 + a11*b11 + a21*b12;
   d[1][2] = a02*b10 + a12*b11 + a22*b12;
   d[2][0] = a00*b20 + a10*b21 + a20*b22;
   d[2][1] = a01*b20 + a11*b21 + a21*b22;
   d[2][2] = a02*b20 + a12*b21 + a22*b22;
}

static inline void m3x3_mulv( m3x3f m, v3f v, v3f d )
{
   v3f res;
   
   res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2];
   res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2];
   res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2];
   
   v3_copy( res, d );
}

static inline void m3x3_projection( m3x3f dst, 
      float const left, float const right, float const bottom, float const top )
{
   float rl, tb;
   
   m3x3_zero( dst );

   rl = 1.0f / (right - left);
   tb = 1.0f / (top   - bottom);

   dst[0][0] = 2.0f * rl;
   dst[1][1] = 2.0f * tb;
   dst[2][2] = 1.0f;
}

static inline void m3x3_translate( m3x3f m, v3f v )
{
   m[2][0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0];
   m[2][1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1];
   m[2][2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2];
}

static inline void m3x3_scale( m3x3f m, v3f v )
{
   m[0][0] = m[0][0] * v[0];
   m[0][1] = m[0][1] * v[0];
   m[0][2] = m[0][2] * v[0];

   m[1][0] = m[1][0] * v[1];
   m[1][1] = m[1][1] * v[1];
   m[1][2] = m[1][2] * v[1];
}

static inline void m3x3_rotate( m3x3f m, float angle )
{
   float m00 = m[0][0], m10 = m[1][0],
         m01 = m[0][1], m11 = m[1][1],
         m02 = m[0][2], m12 = m[1][2];
   float c, s;

   s = sinf( angle );
   c = cosf( angle );

   m[0][0] = m00 * c + m10 * s;
   m[0][1] = m01 * c + m11 * s;
   m[0][2] = m02 * c + m12 * s;

   m[1][0] = m00 * -s + m10 * c;
   m[1][1] = m01 * -s + m11 * c;
   m[1][2] = m02 * -s + m12 * c;
}

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

static inline void m4x3_identity( m4x3f a )
{
   m4x3f id = M4X3_IDENTITY;
   m4x3_copy( id, a );
}

static inline void m4x3_mul( m4x3f a, m4x3f b, m4x3f d ) 
{
   float 
   a00 = a[0][0], a01 = a[0][1], a02 = a[0][2],
   a10 = a[1][0], a11 = a[1][1], a12 = a[1][2],
   a20 = a[2][0], a21 = a[2][1], a22 = a[2][2],
   a30 = a[3][0], a31 = a[3][1], a32 = a[3][2],
   b00 = b[0][0], b01 = b[0][1], b02 = b[0][2],
   b10 = b[1][0], b11 = b[1][1], b12 = b[1][2],
   b20 = b[2][0], b21 = b[2][1], b22 = b[2][2],
   b30 = b[3][0], b31 = b[3][1], b32 = b[3][2];
   
   d[0][0] = a00*b00 + a10*b01 + a20*b02;
   d[0][1] = a01*b00 + a11*b01 + a21*b02;
   d[0][2] = a02*b00 + a12*b01 + a22*b02;
   d[1][0] = a00*b10 + a10*b11 + a20*b12;
   d[1][1] = a01*b10 + a11*b11 + a21*b12;
   d[1][2] = a02*b10 + a12*b11 + a22*b12;
   d[2][0] = a00*b20 + a10*b21 + a20*b22;
   d[2][1] = a01*b20 + a11*b21 + a21*b22;
   d[2][2] = a02*b20 + a12*b21 + a22*b22;
   d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30;
   d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31;
   d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32;
}

static inline void m4x3_mulv( m4x3f m, v3f v, v3f d ) 
{
   v3f res;
  
   res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0];
   res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1];
   res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2];

   v3_copy( res, d );
}

/* 
 * Transform plane ( xyz, distance )
 */
static inline void m4x3_mulp( m4x3f m, v4f p, v4f d )
{
   v3f o;

   v3_muls( p, p[3], o );
   m4x3_mulv( m, o, o );
   m3x3_mulv( m, p, d );
   
   d[3] = v3_dot( o, d );
}

/*
 * Affine transforms
 */

static inline void m4x3_translate( m4x3f m, v3f v )
{
   v3_muladds( m[3], m[0], v[0], m[3] );
   v3_muladds( m[3], m[1], v[1], m[3] );
   v3_muladds( m[3], m[2], v[2], m[3] );
}

static inline void m4x3_scale( m4x3f m, float s )
{
   v3_muls( m[0], s, m[0] );
   v3_muls( m[1], s, m[1] );
   v3_muls( m[2], s, m[2] );
}

static inline void m4x3_scalev( m4x3f m, v3f v )
{
   v3_muls(m[0], v[0], m[0]);
   v3_muls(m[1], v[1], m[1]);
   v3_muls(m[2], v[2], m[2]);
}

static inline void m4x3_rotate_x( m4x3f m, float angle )
{
   m4x3f t = M4X3_IDENTITY;
   float c, s;

   c = cosf( angle );
   s = sinf( angle );

   t[1][1] =  c;
   t[1][2] =  s;
   t[2][1] = -s;
   t[2][2] =  c;

   m4x3_mul( m, t, m );
}

static inline void m4x3_rotate_y( m4x3f m, float angle )
{
   m4x3f t = M4X3_IDENTITY;
   float c, s;

   c = cosf( angle );
   s = sinf( angle );

   t[0][0] =  c;
   t[0][2] = -s;
   t[2][0] =  s;
   t[2][2] =  c;

   m4x3_mul( m, t, m );
}

static inline void m4x3_rotate_z( m4x3f m, float angle )
{
   m4x3f t = M4X3_IDENTITY;
   float c, s;

   c = cosf( angle );
   s = sinf( angle );

   t[0][0] =  c;
   t[0][1] =  s;
   t[1][0] = -s;
   t[1][1] =  c;

   m4x3_mul( m, t, m );
}

static inline void m4x3_expand( m4x3f m, m4x4f d )
{
   v3_copy( m[0], d[0] );
   v3_copy( m[1], d[1] );
   v3_copy( m[2], d[2] );
   v3_copy( m[3], d[3] );
   d[0][3] = 0.0f;
   d[1][3] = 0.0f;
   d[2][3] = 0.0f;
   d[3][3] = 1.0f;
}

static inline void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point )
{
   v3f v;
   m4x3_mulv( m, point, v );

   v3_minv( box[0], v, box[0] );
   v3_maxv( box[1], v, box[1] );
}

static inline void box_addpt( boxf a, v3f pt )
{
   v3_minv( a[0], pt, a[0] );
   v3_maxv( a[1], pt, a[1] );
}

static inline void box_concat( boxf a, boxf b )
{
   v3_minv( a[0], b[0], a[0] );
   v3_maxv( a[1], b[1], a[1] );
}

static inline void box_copy( boxf a, boxf b )
{
   v3_copy( a[0], b[0] );
   v3_copy( a[1], b[1] );
}

static inline int box_overlap( boxf a, boxf b )
{
   return
   ( a[0][0] <= b[1][0] && a[1][0] >= b[0][0] ) &&
   ( a[0][1] <= b[1][1] && a[1][1] >= b[0][1] ) &&
   ( a[0][2] <= b[1][2] && a[1][2] >= b[0][2] )
   ;
}

static int box_within( boxf greater, boxf lesser )
{
   v3f a, b;
   v3_sub( lesser[0], greater[0], a );
   v3_sub( lesser[1], greater[1], b );

   if( (a[0] >= 0.0f) && (a[1] >= 0.0f) && (a[2] >= 0.0f) &&
       (b[0] <= 0.0f) && (b[1] <= 0.0f) && (b[2] <= 0.0f) )
   {
      return 1;
   }

   return 0;
}

static inline void box_init_inf( boxf box )
{
   v3_fill( box[0],  INFINITY );
   v3_fill( box[1], -INFINITY );
}

static inline void m4x3_transform_aabb( m4x3f m, boxf box )
{
   v3f a; v3f b;
   
   v3_copy( box[0], a );
   v3_copy( box[1], b );
   v3_fill( box[0],  INFINITY );
   v3_fill( box[1], -INFINITY );

   m4x3_expand_aabb_point( m, box, (v3f){ a[0], a[1], a[2] } );
   m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], a[2] } );
   m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], a[2] } );
   m4x3_expand_aabb_point( m, box, (v3f){ b[0], a[1], a[2] } );

   m4x3_expand_aabb_point( m, box, (v3f){ a[0], a[1], b[2] } );
   m4x3_expand_aabb_point( m, box, (v3f){ a[0], b[1], b[2] } );
   m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], b[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 )
{
   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 );
   
   tmin = vg_minf( v0[0], v1[0] );
   tmax = vg_maxf( v0[0], v1[0] );
   tmin = vg_maxf( tmin, vg_minf( v0[1], v1[1] ));
   tmax = vg_minf( tmax, vg_maxf( v0[1], v1[1] ));
   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;
}

static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up )
{
   v3f dir;
   v3_sub( target, pos, dir );
   v3_normalize( dir );

   v3_copy( dir, m[2] );

   v3_cross( up, m[2], m[0] );
   v3_normalize( m[0] );

   v3_cross( m[2], m[0], m[1] );
   v3_copy( pos, m[3] );
}

/*
 * Matrix 4x4
 */

#define M4X4_IDENTITY   {{1.0f, 0.0f, 0.0f, 0.0f },\
                        { 0.0f, 1.0f, 0.0f, 0.0f },\
                        { 0.0f, 0.0f, 1.0f, 0.0f },\
                        { 0.0f, 0.0f, 0.0f, 1.0f }}
#define M4X4_ZERO         {{0.0f, 0.0f, 0.0f, 0.0f },\
                        { 0.0f, 0.0f, 0.0f, 0.0f },\
                        { 0.0f, 0.0f, 0.0f, 0.0f },\
                        { 0.0f, 0.0f, 0.0f, 0.0f }}

static void m4x4_projection( m4x4f m, float angle,
                             float ratio, float fnear, float ffar )
{
   float scale = tanf( angle * 0.5f * VG_PIf / 180.0f ) * fnear,
         r = ratio * scale,
         l = -r,
         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[3][3] =  0.0f;
} 

static void m4x4_translate( m4x4f m, v3f v )
{
   v4_muladds( m[3], m[0], v[0], m[3] );
   v4_muladds( m[3], m[1], v[1], m[3] );
   v4_muladds( m[3], m[2], v[2], m[3] );
}

static inline void m4x4_copy( m4x4f a, m4x4f b )
{
   v4_copy( a[0], b[0] );
   v4_copy( a[1], b[1] );
   v4_copy( a[2], b[2] );
   v4_copy( a[3], b[3] );
}

static inline void m4x4_identity( m4x4f a )
{
   m4x4f id = M4X4_IDENTITY;
   m4x4_copy( id, a );
}

static inline void m4x4_zero( m4x4f a )
{
   m4x4f zero = M4X4_ZERO;
   m4x4_copy( zero, a );
}

static inline void m4x4_mul( m4x4f a, m4x4f b, m4x4f d )
{
   float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3],
         a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3],
         a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3],
         a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3],

         b00 = b[0][0], b01 = b[0][1], b02 = b[0][2], b03 = b[0][3],
         b10 = b[1][0], b11 = b[1][1], b12 = b[1][2], b13 = b[1][3],
         b20 = b[2][0], b21 = b[2][1], b22 = b[2][2], b23 = b[2][3],
         b30 = b[3][0], b31 = b[3][1], b32 = b[3][2], b33 = b[3][3];

  d[0][0] = a00*b00 + a10*b01 + a20*b02 + a30*b03;
  d[0][1] = a01*b00 + a11*b01 + a21*b02 + a31*b03;
  d[0][2] = a02*b00 + a12*b01 + a22*b02 + a32*b03;
  d[0][3] = a03*b00 + a13*b01 + a23*b02 + a33*b03;
  d[1][0] = a00*b10 + a10*b11 + a20*b12 + a30*b13;
  d[1][1] = a01*b10 + a11*b11 + a21*b12 + a31*b13;
  d[1][2] = a02*b10 + a12*b11 + a22*b12 + a32*b13;
  d[1][3] = a03*b10 + a13*b11 + a23*b12 + a33*b13;
  d[2][0] = a00*b20 + a10*b21 + a20*b22 + a30*b23;
  d[2][1] = a01*b20 + a11*b21 + a21*b22 + a31*b23;
  d[2][2] = a02*b20 + a12*b21 + a22*b22 + a32*b23;
  d[2][3] = a03*b20 + a13*b21 + a23*b22 + a33*b23;
  d[3][0] = a00*b30 + a10*b31 + a20*b32 + a30*b33;
  d[3][1] = a01*b30 + a11*b31 + a21*b32 + a31*b33;
  d[3][2] = a02*b30 + a12*b31 + a22*b32 + a32*b33;
  d[3][3] = a03*b30 + a13*b31 + a23*b32 + a33*b33;
}

static inline void m4x4_mulv( m4x4f m, v4f v, v4f d ) 
{
   v4f res;
  
   res[0] = m[0][0]*v[0] + m[1][0]*v[1] + m[2][0]*v[2] + m[3][0]*v[3];
   res[1] = m[0][1]*v[0] + m[1][1]*v[1] + m[2][1]*v[2] + m[3][1]*v[3];
   res[2] = m[0][2]*v[0] + m[1][2]*v[1] + m[2][2]*v[2] + m[3][2]*v[3];
   res[3] = m[0][3]*v[0] + m[1][3]*v[1] + m[2][3]*v[2] + m[3][3]*v[3];

   v4_copy( res, d );
}

static inline void m4x4_inv( m4x4f a, m4x4f d )
{
   float a00 = a[0][0], a01 = a[0][1], a02 = a[0][2], a03 = a[0][3],
         a10 = a[1][0], a11 = a[1][1], a12 = a[1][2], a13 = a[1][3],
         a20 = a[2][0], a21 = a[2][1], a22 = a[2][2], a23 = a[2][3],
         a30 = a[3][0], a31 = a[3][1], a32 = a[3][2], a33 = a[3][3],
         det,
         t[6];

   t[0] = a22*a33 - a32*a23;
   t[1] = a21*a33 - a31*a23;
   t[2] = a21*a32 - a31*a22;
   t[3] = a20*a33 - a30*a23;
   t[4] = a20*a32 - a30*a22;
   t[5] = a20*a31 - a30*a21;

   d[0][0] =  a11*t[0] - a12*t[1] + a13*t[2];
   d[1][0] =-(a10*t[0] - a12*t[3] + a13*t[4]);
   d[2][0] =  a10*t[1] - a11*t[3] + a13*t[5];
   d[3][0] =-(a10*t[2] - a11*t[4] + a12*t[5]);

   d[0][1] =-(a01*t[0] - a02*t[1] + a03*t[2]);
   d[1][1] =  a00*t[0] - a02*t[3] + a03*t[4];
   d[2][1] =-(a00*t[1] - a01*t[3] + a03*t[5]);
   d[3][1] =  a00*t[2] - a01*t[4] + a02*t[5];

   t[0] = a12*a33 - a32*a13;
   t[1] = a11*a33 - a31*a13;
   t[2] = a11*a32 - a31*a12;
   t[3] = a10*a33 - a30*a13;
   t[4] = a10*a32 - a30*a12;
   t[5] = a10*a31 - a30*a11;

   d[0][2] =  a01*t[0] - a02*t[1] + a03*t[2];
   d[1][2] =-(a00*t[0] - a02*t[3] + a03*t[4]);
   d[2][2] =  a00*t[1] - a01*t[3] + a03*t[5];
   d[3][2] =-(a00*t[2] - a01*t[4] + a02*t[5]);

   t[0] = a12*a23 - a22*a13;
   t[1] = a11*a23 - a21*a13;
   t[2] = a11*a22 - a21*a12;
   t[3] = a10*a23 - a20*a13;
   t[4] = a10*a22 - a20*a12;
   t[5] = a10*a21 - a20*a11;

   d[0][3] =-(a01*t[0] - a02*t[1] + a03*t[2]);
   d[1][3] =  a00*t[0] - a02*t[3] + a03*t[4];
   d[2][3] =-(a00*t[1] - a01*t[3] + a03*t[5]);
   d[3][3] =  a00*t[2] - a01*t[4] + a02*t[5];
   
   det = 1.0f / (a00*d[0][0] + a01*d[1][0] + a02*d[2][0] + a03*d[3][0]);
   v4_muls( d[0], det, d[0] );
   v4_muls( d[1], det, d[1] );
   v4_muls( d[2], det, d[2] );
   v4_muls( d[3], det, d[3] );
}

/*
 * 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 inline int plane_intersect( double a[4], double b[4], 
      double c[4], double p[4] )
{
   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];
   
   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;
   
   return 1;
}

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

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 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;

   v3f v0, v1, h, s, q, n;
   float a,f,u,v,t;

   float *pa = tri[0],
         *pb = tri[1],
         *pc = tri[2];

   v3_sub( pb, pa, v0 );
   v3_sub( pc, pa, v1 );
   v3_cross( dir, v1, h );
   v3_cross( v0, v1, n );

   if( v3_dot( n, dir ) > 0.0f ) /* Backface culling */
      return 0;
   
   /* Parralel */
   a = v3_dot( v0, h );
   if( a > -kEpsilon && a < kEpsilon )
      return 0;

   f = 1.0f/a;
   v3_sub( co, pa, s );

   u = f * v3_dot(s, h);
   if( u < 0.0f || u > 1.0f )
      return 0;
   
   v3_cross( s, v0, q );
   v = f * v3_dot( dir, q );
   if( v < 0.0f || u+v > 1.0f )
      return 0;

   t = f * v3_dot(v1, q);
   if( t > kEpsilon )
   {
      *dist = t;
      return 1;
   }
   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);
}

static inline void vg_rand_dir(v3f dir)
{
   dir[0] = vg_randf();
   dir[1] = vg_randf();
   dir[2] = vg_randf();

   v3_muls( dir, 2.0f, dir );
   v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir );

   v3_normalize( dir );
}

static inline void vg_rand_sphere( v3f co )
{
   vg_rand_dir(co);
   v3_muls( co, cbrtf( vg_randf() ), co );
}

static inline int vg_randint(int max)
{
   return rand()%max;
}

static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p )
{
   float tt = t*t,
         ttt = tt*t;

   v3_muls( p1, ttt, p );
   v3_muladds( p, h1, 3.0f*tt  -3.0f*ttt, p );
   v3_muladds( p, h0, 3.0f*ttt -6.0f*tt  +3.0f*t, p );
   v3_muladds( p, p0, 3.0f*tt  -ttt -3.0f*t +1.0f, p );
}

#endif /* VG_M_H */