X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=7f7187daf7aab46e9fc353032b956a42043f6cb5;hb=b4c9550f206c476bb38b0bb2855d35e6b31bee83;hp=d911688beca8a5e6fc7e98399aa0739b029f02eb;hpb=98ecdd9f0377f1563566fe4d3d3b8185a055ec4d;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index d911688..7f7187d 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -5,6 +5,7 @@
 
 #include "common.h"
 #include "bvh.h"
+#include "scene.h"
 
 static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
 static bh_system bh_system_rigidbodies;
@@ -12,11 +13,13 @@ static bh_system bh_system_rigidbodies;
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
-#define RB_DEPR 
+//#define RB_DEPR 
 #define k_rb_rate  60.0f
 #define k_rb_delta (1.0f/k_rb_rate)
 
 typedef struct rigidbody rigidbody;
+typedef struct contact rb_ct;
+
 struct rigidbody
 {
    v3f co, v, w;
@@ -24,9 +27,10 @@ struct rigidbody
 
    enum rb_shape
    {
-      k_rb_shape_box,
-      k_rb_shape_sphere,
-      k_rb_shape_capsule
+      k_rb_shape_box     = 0,
+      k_rb_shape_sphere  = 1,
+      k_rb_shape_capsule = 2,
+      k_rb_shape_scene   = 3
    } 
    type;
    
@@ -43,30 +47,65 @@ struct rigidbody
          float height, radius;
       } 
       capsule;
+
+      struct rb_scene
+      {
+         scene *pscene;
+      }
+      scene;
    }
    inf;
 
    v3f right, up, forward;
    
+   int is_world;
+
    boxf bbx, bbx_world;
    float inv_mass;
 
-   v3f delta;  /* where is the origin of this in relation to a parent body */
+   /* inertia model and inverse world tensor */
+   v3f I;
+   m3x3f iI, iIw;
+
    m4x3f to_world, to_local;
 };
 
+#ifdef RB_DEPR
+/*
+ * Impulses on static objects get re-routed here
+ */
+static rigidbody rb_static_null = 
+{
+   .co={0.0f,0.0f,0.0f},
+   .q={0.0f,0.0f,0.0f,1.0f},
+   .v={0.0f,0.0f,0.0f},
+   .w={0.0f,0.0f,0.0f},
+   .is_world = 1,
+   .inv_mass = 0.0f
+};
+#endif
+
 static void rb_debug( rigidbody *rb, u32 colour );
 
 static struct contact
 {
-   rigidbody *rba;
-   v3f co, n, delta;
+   rigidbody *rba, *rbb;
+   v3f co, n;
    v3f t[2];
-   float bias, norm_impulse, tangent_impulse[2];
+   float p, bias, norm_impulse, tangent_impulse[2],
+         normal_mass, tangent_mass[2];
+
+   u32 element_id;
 }
 rb_contact_buffer[256];
 static int rb_contact_count = 0;
 
+static void rb_update_bounds( rigidbody *rb )
+{
+   box_copy( rb->bbx, rb->bbx_world );
+   m4x3_transform_aabb( rb->to_world, rb->bbx_world );
+}
+
 static void rb_update_transform( rigidbody *rb )
 {
    q_normalize( rb->q );
@@ -75,12 +114,14 @@ static void rb_update_transform( rigidbody *rb )
 
    m4x3_invert_affine( rb->to_world, rb->to_local );
 
-   box_copy( rb->bbx, rb->bbx_world );
-   m4x3_transform_aabb( rb->to_world, rb->bbx_world );
-
    m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f, 0.0f}, rb->right );
    m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f, 0.0f}, rb->up );
    m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,-1.0f}, rb->forward );
+
+   m3x3_mul( rb->iI, rb->to_local, rb->iIw );
+   m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
+
+   rb_update_bounds( rb );
 }
 
 static float sphere_volume( float radius )
@@ -98,12 +139,17 @@ static void rb_init( rigidbody *rb )
       v3f dims;
       v3_sub( rb->bbx[1], rb->bbx[0], dims );
       volume = dims[0]*dims[1]*dims[2];
+      
+      if( !rb->is_world )
+         vg_info( "Box volume: %f\n", volume );
    }
    else if( rb->type == k_rb_shape_sphere )
    {
       volume = sphere_volume( rb->inf.sphere.radius );
       v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
       v3_fill( rb->bbx[1],  rb->inf.sphere.radius );
+
+      vg_info( "Sphere volume: %f\n", volume );
    }
    else if( rb->type == k_rb_shape_capsule )
    {
@@ -116,8 +162,43 @@ static void rb_init( rigidbody *rb )
       rb->bbx[0][1] = -h;
       rb->bbx[1][1] =  h;
    }
+   else if( rb->type == k_rb_shape_scene )
+   {
+      rb->is_world = 1;
+      box_copy( rb->inf.scene.pscene->bbx, rb->bbx );
+   }
 
-   rb->inv_mass = 1.0f/(8.0f*volume);
+   if( rb->is_world )
+   {
+      rb->inv_mass = 0.0f;
+      v3_zero( rb->I );
+      m3x3_zero(rb->iI);
+   }
+   else
+   {
+      float mass = 2.0f*volume;
+      rb->inv_mass = 1.0f/mass;
+
+      v3f extent;
+      v3_sub( rb->bbx[1], rb->bbx[0], extent );
+      v3_muls( extent, 0.5f, extent );
+
+      /* local intertia tensor */
+      float scale = 4.0f;
+      float ex2 = scale*extent[0]*extent[0],
+            ey2 = scale*extent[1]*extent[1],
+            ez2 = scale*extent[2]*extent[2];
+
+      rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
+      rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
+      rb->I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
+
+      m3x3_identity( rb->iI );
+      rb->iI[0][0] = rb->I[0];
+      rb->iI[1][1] = rb->I[1];
+      rb->iI[2][2] = rb->I[2];
+      m3x3_inv( rb->iI, rb->iI );
+   }
 
    v3_zero( rb->v );
    v3_zero( rb->w );
@@ -127,7 +208,7 @@ static void rb_init( rigidbody *rb )
 
 static void rb_iter( rigidbody *rb )
 {
-   v3f gravity = { 0.0f, -9.6f, 0.0f };
+   v3f gravity = { 0.0f, -9.8f, 0.0f };
    v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
 
    /* intergrate velocity */
@@ -174,10 +255,17 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 }
 
 static void rb_solver_reset(void);
+#ifdef RB_DEPR
 static void rb_build_manifold_terrain( rigidbody *rb );
 static void rb_build_manifold_terrain_sphere( rigidbody *rb );
-static void rb_solve_contacts(void);
+#endif
+static void rb_solve_contacts( rb_ct *buf, int len );
+static void rb_presolve_contacts( rb_ct *buffer, int len );
 
+/* 
+ * These closest point tests were learned from Real-Time Collision Detection by 
+ * Christer Ericson 
+ */
 static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, 
    float *s, float *t, v3f c1, v3f c2)
 {
@@ -255,17 +343,32 @@ static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
    return v3_length2( v0 );
 }
 
-static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+static void closest_point_aabb( v3f p, boxf box, v3f dest )
+{
+   v3_maxv( p, box[0], dest );
+   v3_minv( dest, box[1], dest );
+}
+
+static void closest_point_obb( v3f p, rigidbody *rb, v3f dest )
+{
+   v3f local;
+   m4x3_mulv( rb->to_local, p, local );
+   closest_point_aabb( local, rb->bbx, local );
+   m4x3_mulv( rb->to_world, local, dest );
+}
+
+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);
-   v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
+   t = vg_clampf(t,0.0f,1.0f);
+   v3_muladds( a, v0, t, dest );
+   return t;
 }
 
-/* Real-Time Collision Detection */
 static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
 {
    v3f ab, ac, ap;
@@ -310,49 +413,1021 @@ static void closest_on_triangle( v3f p, v3f tri[3], v3f 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 )
+   /* 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 );
+}
+
+/* TODO */
+static void 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;
+   }
+
+   /* 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;
+   }
+   
+   /* 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;
+   }
+
+   /* 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;
+   }
+
+   /* 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;
+   }
+
+   /* 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;
+   }
+
+   /* 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 );
+}
+
+static int rb_intersect_planes( v4f p0, v4f p1, v4f p2, v3f p )
+{
+   v3f u;
+   v3_cross( p1, p2, u );
+   float d = v3_dot( p0, u );
+
+   if( fabsf(d) < 0.0001f ) 
+      return 0;
+
+   v3_muls( u, p0[3], p );
+
+   v3f v0, v1;
+   v3_muls( p1, p2[3], v0 );
+   v3_muladds( v0, p2, -p1[3], v0 );
+   v3_cross( p0, v0, v1 );
+   v3_add( v1, p, p );
+   v3_muls( p, 1.0f/d, p );
+
+   return 1;
+}
+
+int rb_intersect_planes_1( v4f a, v4f b, v4f c, v3f p )
+{
+   float const epsilon = 0.001;
+        
+   v3f x, bc, ca, ab;
+   float d;
+        
+   v3_cross( a, b, x );
+   d = v3_dot( x, c );
+        
+   if( d < epsilon && d > -epsilon ) return 0;
+        
+   v3_cross(b,c,bc);
+   v3_cross(c,a,ca);
+   v3_cross(a,b,ab);
+
+   v3_muls(       bc, -a[3], p );
+   v3_muladds( p, ca, -b[3], p );
+   v3_muladds( p, ab, -c[3], p );
+
+   v3_negate( p, p );
+   v3_divs( p, d, p );
+        
+   return 1;
+}
+/*
+ * Contact generators
+ *
+ * These do not automatically allocate contacts, an appropriately sized 
+ * buffer must be supplied. The function returns the size of the manifold
+ * which was generated.
+ *
+ * The values set on the contacts are: n, co, p, rba, rbb
+ */
+
+static void rb_debug_contact( rb_ct *ct )
+{
+   v3f p1;
+   v3_muladds( ct->co, ct->n, 0.1f, p1 );
+   vg_line_pt3( ct->co, 0.025f, 0xff0000ff );
+   vg_line( ct->co, p1, 0xffffffff );
+}
+
+/* 
+ * By collecting the minimum(time) and maximum(time) pairs of points, we
+ * build a reduced and stable exact manifold. 
+ * 
+ * tx: time at point
+ * rx: minimum distance of these points
+ * dx: the delta between the two points
+ *
+ * pairs will only ammend these if they are creating a collision
+ */
+typedef struct capsule_manifold capsule_manifold;
+struct capsule_manifold
+{
+   float t0, t1;
+   float r0, r1;
+   v3f d0, d1;
+};
+
+/* 
+ * Expand a line manifold with a new pair. t value is the time along segment
+ * on the oriented object which created this pair.
+ */
+static void rb_capsule_manifold( v3f pa, v3f pb, float t, float r, 
+                                 capsule_manifold *manifold )
+{
+   v3f delta;
+   v3_sub( pa, pb, delta );
+
+   if( v3_length2(delta) < r*r )
+   {
+      if( t < manifold->t0 )
+      {
+         v3_copy( delta, manifold->d0 );
+         manifold->t0 = t;
+         manifold->r0 = r;
+      }
+
+      if( t > manifold->t1 )
+      {
+         v3_copy( delta, manifold->d1 );
+         manifold->t1 = t;
+         manifold->r1 = r;
+      }
+   }
+}
+
+static void rb_capsule_manifold_init( capsule_manifold *manifold )
+{
+   manifold->t0 =  INFINITY;
+   manifold->t1 = -INFINITY;
+}
+
+static int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb, 
+                                     capsule_manifold *manifold, rb_ct *buf )
+{
+   float h = rba->inf.capsule.height,
+        ra = rba->inf.capsule.radius;
+
+   v3f p0, p1;
+   v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
+   v3_muladds( rba->co, rba->up,  h*0.5f-ra, p1 );
+
+   int count = 0;
+   if( manifold->t0 <= 1.0f )
+   {
+      rb_ct *ct = buf;
+
+      v3f pa;
+      v3_muls( p0, 1.0f-manifold->t0, pa );
+      v3_muladds( pa, p1, manifold->t0, pa );
+
+      float d = v3_length( manifold->d0 );
+      v3_muls( manifold->d0, 1.0f/d, ct->n );
+      v3_muladds( pa, ct->n, -ra, ct->co );
+
+      ct->p = manifold->r0 - d;
+      ct->rba = rba;
+      ct->rbb = rbb;
+
+      count ++;
+   }
+
+   if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
+   {
+      rb_ct *ct = buf+count;
+
+      v3f pa;
+      v3_muls( p0, 1.0f-manifold->t1, pa );
+      v3_muladds( pa, p1, manifold->t1, pa );
+
+      float d = v3_length( manifold->d1 );
+      v3_muls( manifold->d1, 1.0f/d, ct->n );
+      v3_muladds( pa, ct->n, -ra, ct->co );
+
+      ct->p = manifold->r1 - d;
+      ct->rba = rba;
+      ct->rbb = rbb;
+
+      count ++;
+   }
+
+   /*
+    * Debugging
+    */
+
+   if( count == 2 )
+      vg_line( buf[0].co, buf[1].co, 0xff0000ff );
+
+   return count;
+}
+
+static int rb_capsule_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   float h = rba->inf.capsule.height,
+        ra = rba->inf.capsule.radius,
+        rb = rbb->inf.sphere.radius;
+
+   v3f p0, p1;
+   v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
+   v3_muladds( rba->co, rba->up,  h*0.5f-ra, p1 );
+
+   v3f c, delta;
+   closest_point_segment( p0, p1, rbb->co, c );
+   v3_sub( c, rbb->co, delta );
+
+   float d2 = v3_length2(delta),
+          r = ra + rb;
+
+   if( d2 < r*r )
+   {
+      float d = sqrtf(d2);
+             
+      rb_ct *ct = buf;
+      v3_muls( delta, 1.0f/d, ct->n );
+      ct->p = r-d;
+
+      v3f p0, p1;
+      v3_muladds( c, ct->n, -ra, p0 );
+      v3_muladds( rbb->co, ct->n, rb,  p1 );
+      v3_add( p0, p1, ct->co );
+      v3_muls( ct->co, 0.5f, ct->co );
+
+      ct->rba = rba;
+      ct->rbb = rbb;
+
+      return 1;
+   }
+
+   return 0;
+}
+
+static int rb_capsule_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   float ha = rba->inf.capsule.height,
+         hb = rbb->inf.capsule.height,
+         ra = rba->inf.capsule.radius,
+         rb = rbb->inf.capsule.radius,
+          r = ra+rb;
+
+   v3f p0, p1, p2, p3;
+   v3_muladds( rba->co, rba->up, -ha*0.5f+ra, p0 );
+   v3_muladds( rba->co, rba->up,  ha*0.5f-ra, p1 );
+   v3_muladds( rbb->co, rbb->up, -hb*0.5f+rb, p2 );
+   v3_muladds( rbb->co, rbb->up,  hb*0.5f-rb, p3 );
+
+   capsule_manifold manifold;
+   rb_capsule_manifold_init( &manifold );
+
+   v3f pa, pb;
+   float ta, tb;
+   closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb );
+   rb_capsule_manifold( pa, pb, ta, r, &manifold );
+
+   ta = closest_point_segment( p0, p1, p2, pa );
+   tb = closest_point_segment( p0, p1, p3, pb );
+   rb_capsule_manifold( pa, p2, ta, r, &manifold );
+   rb_capsule_manifold( pb, p3, tb, r, &manifold );
+
+   closest_point_segment( p2, p3, p0, pa );
+   closest_point_segment( p2, p3, p1, pb );
+   rb_capsule_manifold( p0, pa, 0.0f, r, &manifold );
+   rb_capsule_manifold( p1, pb, 1.0f, r, &manifold );
+   
+   return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
+}
+
+/*
+ * Generates up to two contacts; optimised for the most stable manifold
+ */
+static int rb_capsule_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   float h = rba->inf.capsule.height,
+         r = rba->inf.capsule.radius;
+
+   /*
+    * Solving this in symetric local space of the cube saves us some time and a 
+    * couple branches when it comes to the quad stage.
+    */
+   v3f centroid;
+   v3_add( rbb->bbx[0], rbb->bbx[1], centroid );
+   v3_muls( centroid, 0.5f, centroid );
+
+   boxf bbx;
+   v3_sub( rbb->bbx[0], centroid, bbx[0] );
+   v3_sub( rbb->bbx[1], centroid, bbx[1] );
+   
+   v3f pc, p0w, p1w, p0, p1;
+   v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
+   v3_muladds( rba->co, rba->up,  h*0.5f-r, p1w );
+
+   m4x3_mulv( rbb->to_local, p0w, p0 );
+   m4x3_mulv( rbb->to_local, p1w, p1 );
+   v3_sub( p0, centroid, p0 );
+   v3_sub( p1, centroid, p1 );
+   v3_add( p0, p1, pc );
+   v3_muls( pc, 0.5f, pc );
+
+   /* 
+    * Finding an appropriate quad to collide lines with
+    */
+   v3f region;
+   v3_div( pc, bbx[1], region );
+   
+   v3f quad[4];
+   if( (fabsf(region[0]) > fabsf(region[1])) && 
+       (fabsf(region[0]) > fabsf(region[2])) )
+   {
+      float px = vg_signf(region[0]) * bbx[1][0];
+      v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
+      v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
+      v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
+      v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
+   }
+   else if( fabsf(region[1]) > fabsf(region[2]) )
+   {
+      float py = vg_signf(region[1]) * bbx[1][1];
+      v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
+      v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
+      v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
+      v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
+   }
+   else
+   {
+      float pz = vg_signf(region[2]) * bbx[1][2];
+      v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
+      v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
+      v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
+      v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
+   }
+
+   capsule_manifold manifold;
+   rb_capsule_manifold_init( &manifold );
+   
+   v3f c0, c1;
+   closest_point_aabb( p0, bbx, c0 );
+   closest_point_aabb( p1, bbx, c1 );
+
+   v3f d0, d1, da;
+   v3_sub( c0, p0, d0 );
+   v3_sub( c1, p1, d1 );
+   v3_sub( p1, p0, da );
+   
+   /* TODO: ? */
+   v3_normalize(d0);
+   v3_normalize(d1);
+   v3_normalize(da);
+
+   if( v3_dot( da, d0 ) <= 0.01f )
+      rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
+
+   if( v3_dot( da, d1 ) >= -0.01f )
+      rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
+
+   for( int i=0; i<4; i++ )
+   {
+      int i0 = i,
+          i1 = (i+1)%4;
+
+      v3f ca, cb;
+      float ta, tb;
+      closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
+      rb_capsule_manifold( ca, cb, ta, r, &manifold );
+   }
+
+   /*
+    * Create final contacts based on line manifold
+    */
+   m3x3_mulv( rbb->to_world, manifold.d0, manifold.d0 );
+   m3x3_mulv( rbb->to_world, manifold.d1, manifold.d1 );
+
+   /*
+    * Debugging
+    */
+
+#if 0
+   for( int i=0; i<4; i++ )
+   {
+      v3f q0, q1;
+      int i0 = i,
+          i1 = (i+1)%4;
+
+      v3_add( quad[i0], centroid, q0 );
+      v3_add( quad[i1], centroid, q1 );
+      
+      m4x3_mulv( rbb->to_world, q0, q0 );
+      m4x3_mulv( rbb->to_world, q1, q1 );
+      
+      vg_line( q0, q1, 0xffffffff );
+   }
+#endif
+
+   return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
+}
+
+static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   v3f co, delta;
+
+   closest_point_obb( rba->co, rbb, co );
+   v3_sub( rba->co, co, delta );
+
+   float d2 = v3_length2(delta),
+          r = rba->inf.sphere.radius;
+
+   if( d2 <= r*r )
+   {
+      float d;
+
+      rb_ct *ct = buf;
+      if( d2 <= 0.0001f )
+      {
+         v3_sub( rba->co, rbb->co, delta );
+
+         /* 
+          * some extra testing is required to find the best axis to push the
+          * object back outside the box. Since there isnt a clear seperating
+          * vector already, especially on really high aspect boxes.
+          */
+         float lx = v3_dot( rbb->right, delta ),
+               ly = v3_dot( rbb->up, delta ),
+               lz = v3_dot( rbb->forward, delta ),
+               px = rbb->bbx[1][0] - fabsf(lx),
+               py = rbb->bbx[1][1] - fabsf(ly),
+               pz = rbb->bbx[1][2] - fabsf(lz);
+
+         if( px < py && px < pz )
+            v3_muls( rbb->right, vg_signf(lx), ct->n );
+         else if( py < pz )
+            v3_muls( rbb->up, vg_signf(ly), ct->n );
+         else
+            v3_muls( rbb->forward, vg_signf(lz), ct->n );
+
+         v3_muladds( rba->co, ct->n, -r, ct->co );
+         ct->p = r;
+      }
+      else
+      {
+         d = sqrtf(d2);
+         v3_muls( delta, 1.0f/d, ct->n );
+         ct->p = r-d;
+         v3_copy( co, ct->co );
+      }
+
+      ct->rba = rba;
+      ct->rbb = rbb;
+      return 1;
+   }
+
+   return 0;
+}
+
+static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   v3f delta;
+   v3_sub( rba->co, rbb->co, delta );
+
+   float d2 = v3_length2(delta),
+          r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
+
+   if( d2 < r*r )
+   {
+      float d = sqrtf(d2);
+
+      rb_ct *ct = buf;
+      v3_muls( delta, 1.0f/d, ct->n );
+
+      v3f p0, p1;
+      v3_muladds( rba->co, ct->n,-rba->inf.sphere.radius, p0 );
+      v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
+      v3_add( p0, p1, ct->co );
+      v3_muls( ct->co, 0.5f, ct->co );
+      ct->p = r-d;
+      ct->rba = rba;
+      ct->rbb = rbb;
+      return 1;
+   }
+
+   return 0;
+}
+
+/* TODO: these guys */
+
+static int rb_capsule_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   u32 geo[128];
+   v3f tri[3];
+   int len = bh_select( &rbb->inf.scene.pscene->bhtris, 
+                         rba->bbx_world, geo, 128 );
+
+   return 0;
+}
+
+static int rb_sphere_vs_triangle( rigidbody *rba, rigidbody *rbb, 
+                                  v3f tri[3], rb_ct *buf )
+{
+   v3f delta, co;
+
+   closest_on_triangle( rba->co, tri, co );
+   v3_sub( rba->co, co, delta );
+
+   vg_line( rba->co, co, 0xffff0000 );
+   vg_line_pt3( rba->co, 0.1f, 0xff00ffff );
+
+   float d2 = v3_length2( delta ),
+          r = rba->inf.sphere.radius;
+
+   if( d2 < r*r )
+   {
+      rb_ct *ct = buf;
+
+      v3f ab, ac, tn;
+      v3_sub( tri[2], tri[0], ab );
+      v3_sub( tri[1], tri[0], ac );
+      v3_cross( ac, ab, tn );
+      v3_copy( tn, ct->n );
+      v3_normalize( ct->n );
+
+      float d = sqrtf(d2);
+
+      v3_copy( co, ct->co );
+      ct->p = r-d;
+      ct->rba = rba;
+      ct->rbb = rbb;
+      return 1;
+   }
+
+   return 0;
+}
+
+static int rb_sphere_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   scene *sc = rbb->inf.scene.pscene;
+   
+   u32 geo[128];
+   v3f tri[3];
+   int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
+
+   int count = 0;
+
+   for( int i=0; i<len; i++ )
+   {
+      u32 *ptri = &sc->indices[ geo[i]*3 ];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( sc->verts[ptri[j]].co, tri[j] );
+
+      vg_line(tri[0],tri[1],0xff00ff00 );
+      vg_line(tri[1],tri[2],0xff00ff00 );
+      vg_line(tri[2],tri[0],0xff00ff00 );
+      
+      buf[count].element_id = ptri[0];
+      count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
+
+      if( count == 12 )
+      {
+         vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+         return count;
+      }
+   }
+
+   return count;
+}
+
+static float rb_box_plane_interval( rigidbody *rba, v4f p )
+{
+   /* TODO: Make boxes COG aligned as is every other shape. 
+    *       or create COG vector.
+    * TODO: Make forward actually point in the right fucking direction. */
+   v3f e,c;
+   v3_sub( rba->bbx[1], rba->bbx[0], e );
+   v3_muls( e, 0.5f, e );
+   v3_add( rba->bbx[0], e, c );
+   m4x3_mulv( rba->to_world, c, c );
+
+   float r = 
+          e[0]*fabsf( v3_dot(p, rba->right)) +
+          e[1]*fabsf( v3_dot(p, rba->up)) +
+          e[2]*fabsf(-v3_dot(p, rba->forward)),
+   s = v3_dot( p, c ) - p[3];
+   
+   return r-s;
+}
+
+static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] )
+{
+   float 
+
+   r = extent[0] * fabsf(axis[0]) +
+       extent[1] * fabsf(axis[1]) +
+       extent[2] * fabsf(axis[2]),
+
+   p0 = v3_dot( axis, tri[0] ),
+   p1 = v3_dot( axis, tri[1] ),
+   p2 = v3_dot( axis, tri[2] ),
+
+   e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2)));
+
+   if( e > r ) return 0;
+   else return 1;
+}
+
+static int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] )
+{
+   v3f tri[3];
+
+   v3f extent, c;
+   v3_sub( rba->bbx[1], rba->bbx[0], extent );
+   v3_muls( extent, 0.5f, extent );
+   v3_add( rba->bbx[0], extent, c );
+
+   for( int i=0; i<3; i++ )
+   {
+      m4x3_mulv( rba->to_local, tri_src[i], tri[i] );
+      v3_sub( tri[i], c, tri[i] );
+   }
+
+   /* u0, u1, u2 */
+   if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0;
+   if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0;
+   if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0;
+
+   v3f v0,v1,v2,n, e0,e1,e2;
+   v3_sub( tri[1], tri[0], v0 );
+   v3_sub( tri[2], tri[0], v1 );
+   v3_sub( tri[2], tri[1], v2 );
+   v3_normalize( v0 );
+   v3_normalize( v1 );
+   v3_normalize( v2 );
+   v3_cross( v0, v1, n );
+   v3_cross( v0, n, e0 );
+   v3_cross( n, v1, e1 );
+   v3_cross( v2, n, e2 );
+
+   /* normal */
+   if(!rb_box_triangle_interval( extent, n, tri )) return 0;
+
+   v3f axis[9];
+   v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
+   v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
+   v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
+   v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
+   v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
+   v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
+   v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
+   v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
+   v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
+   
+   for( int i=0; i<9; i++ )
+      if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
+
+   return 1;
+}
+
+static int rb_box_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   scene *sc = rbb->inf.scene.pscene;
+   
+   u32 geo[128];
+   v3f tri[3];
+   int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
+
+   int count = 0;
+
+   for( int i=0; i<len; i++ )
+   {
+      u32 *ptri = &sc->indices[ geo[i]*3 ];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( sc->verts[ptri[j]].co, tri[j] );
+
+      if( rb_box_triangle_sat( rba, tri ) )
+      {
+         vg_line(tri[0],tri[1],0xff50ff00 );
+         vg_line(tri[1],tri[2],0xff50ff00 );
+         vg_line(tri[2],tri[0],0xff50ff00 );
+      }
+      else
+      {
+         vg_line(tri[0],tri[1],0xff0000ff );
+         vg_line(tri[1],tri[2],0xff0000ff );
+         vg_line(tri[2],tri[0],0xff0000ff );
+
+         continue;
+      }
+
+      v3f v0,v1,n;
+      v3_sub( tri[1], tri[0], v0 );
+      v3_sub( tri[2], tri[0], v1 );
+      v3_cross( v0, v1, n );
+      v3_normalize( n );
+
+      /* find best feature */
+      float best = v3_dot( rba->right, n );
+      int axis = 0;
+      
+      float cy = v3_dot( rba->up, n );
+      if( fabsf(cy) > fabsf(best) )
+      {
+         best = cy;
+         axis = 1;
+      }
+      
+      /* TODO: THIS IS WRONG DIRECTION */
+      float cz = -v3_dot( rba->forward, n );
+      if( fabsf(cz) > fabsf(best) )
+      {
+         best = cz;
+         axis = 2;
+      }
+
+      v3f manifold[4];
+
+      if( axis == 0 )
+      {
+         float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
+         manifold[0][0] = px;
+         manifold[0][1] = rba->bbx[0][1];
+         manifold[0][2] = rba->bbx[0][2];
+         manifold[1][0] = px;
+         manifold[1][1] = rba->bbx[1][1];
+         manifold[1][2] = rba->bbx[0][2];
+         manifold[2][0] = px;
+         manifold[2][1] = rba->bbx[1][1];
+         manifold[2][2] = rba->bbx[1][2];
+         manifold[3][0] = px;
+         manifold[3][1] = rba->bbx[0][1];
+         manifold[3][2] = rba->bbx[1][2];
+      }
+      else if( axis == 1 )
+      {
+         float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
+         manifold[0][0] = rba->bbx[0][0];
+         manifold[0][1] = py;
+         manifold[0][2] = rba->bbx[0][2];
+         manifold[1][0] = rba->bbx[1][0];
+         manifold[1][1] = py;
+         manifold[1][2] = rba->bbx[0][2];
+         manifold[2][0] = rba->bbx[1][0];
+         manifold[2][1] = py;
+         manifold[2][2] = rba->bbx[1][2];
+         manifold[3][0] = rba->bbx[0][0];
+         manifold[3][1] = py;
+         manifold[3][2] = rba->bbx[1][2];
+      }
+      else
+      {
+         float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
+         manifold[0][0] = rba->bbx[0][0];
+         manifold[0][1] = rba->bbx[0][1];
+         manifold[0][2] = pz;
+         manifold[1][0] = rba->bbx[1][0];
+         manifold[1][1] = rba->bbx[0][1];
+         manifold[1][2] = pz;
+         manifold[2][0] = rba->bbx[1][0];
+         manifold[2][1] = rba->bbx[1][1];
+         manifold[2][2] = pz;
+         manifold[3][0] = rba->bbx[0][0];
+         manifold[3][1] = rba->bbx[1][1];
+         manifold[3][2] = pz;
+      }
+   
+      for( int j=0; j<4; j++ )
+         m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
+
+      vg_line( manifold[0], manifold[1], 0xffffffff );
+      vg_line( manifold[1], manifold[2], 0xffffffff );
+      vg_line( manifold[2], manifold[3], 0xffffffff );
+      vg_line( manifold[3], manifold[0], 0xffffffff );
+
+      for( int j=0; j<4; j++ )
+      {
+         rb_ct *ct = buf+count;
+
+         v3_copy( manifold[j], ct->co );
+         v3_copy( n, ct->n );
+
+         float l0 = v3_dot( tri[0], n ),
+               l1 = v3_dot( manifold[j], n );
+
+         ct->p = (l0-l1)*0.5f;
+         if( ct->p < 0.0f )
+            continue;
+
+         ct->rba = rba;
+         ct->rbb = rbb;
+         count ++;
+
+         if( count >= 12 )
+            return count;
+      }
+   }
+   return count;
+}
+
+static int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   vg_error( "Collision type is unimplemented between types %d and %d\n",
+             rba->type, rbb->type );
+
+   return 0;
+}
+
+static int rb_sphere_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   return rb_capsule_vs_sphere( rbb, rba, buf );
+}
+
+static int rb_box_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   return rb_capsule_vs_box( rbb, rba, buf );
+}
+
+static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   return rb_sphere_vs_box( rbb, rba, buf );
+}
+
+static int rb_scene_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   return rb_box_vs_scene( rbb, rba, buf );
+}
+
+static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )= 
+{             /* box */         /* Sphere */         /* Capsule */         /* Mesh */
+/*box    */ { RB_MATRIX_ERROR,  rb_box_vs_sphere,    rb_box_vs_capsule,    rb_box_vs_scene },
+/*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, rb_sphere_vs_scene },
+/*capsule*/ { rb_capsule_vs_box,rb_capsule_vs_sphere,rb_capsule_vs_capsule,RB_MATRIX_ERROR },
+/*mesh   */ { rb_scene_vs_box,  RB_MATRIX_ERROR,     RB_MATRIX_ERROR,      RB_MATRIX_ERROR }
+};
+
+static int rb_collide( rigidbody *rba, rigidbody *rbb )
+{
+   int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf ) 
+      = rb_jump_table[rba->type][rbb->type];
+
+   /* 
+    * 12 is the maximum manifold size we can generate, so we are forced to abort
+    * potentially checking any more.
+    */
+   if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
    {
-      v3_copy( tri[2], dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
+      vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
+               rb_contact_count, vg_list_size(rb_contact_buffer) );
+      return 0;
    }
 
-   /* Region of AC */
-   float vb = d5*d2 - d1*d6;
-   if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+   /*
+    * TODO: Replace this with a more dedicated broad phase pass
+    */
+   if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
    {
-      float w = d2 / (d2-d6);
-      v3_muladds( tri[0], ac, w, dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
+      int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count); 
+      rb_contact_count += count;
+      return count;
    }
+   else
+      return 0;
+}
 
-   /* Region of BC */
-   float va = d3*d6 - d5*d4;
-   if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+/*
+ * Generic functions
+ */
+
+#ifdef RB_DEPR
+/* 
+ * This function does not accept triangle as a dynamic object, it is assumed
+ * to always be static.
+ * 
+ * The triangle is also assumed to be one sided for better detection
+ */
+static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
+{
+   v3f delta, co;
+
+   closest_on_triangle( rba->co, tri, co );
+   v3_sub( rba->co, co, delta );
+
+   float d2 = v3_length2( delta ),
+          r = rba->inf.sphere.radius;
+
+   if( d2 < r*r )
    {
-      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;
-   }
+      v3f ab, ac, tn;
+      v3_sub( tri[1], tri[0], ab );
+      v3_sub( tri[2], tri[0], ac );
+      v3_cross( ac, ab, tn );
 
-   /* P inside region, Q via barycentric coordinates uvw */
-   float d = 1.0f/(va+vb+vc),
-         v = vb*d,
-         w = vc*d;
+      if( v3_dot( delta, tn ) > 0.0f )
+         v3_muls( delta, -1.0f, delta );
 
-   v3_muladds( tri[0], ab, v, dest );
-   v3_muladds( dest, ac, w, dest );
+      float d = sqrtf(d2);
+
+      rb_ct *ct = buf;
+      v3_muls( delta, 1.0f/d, ct->n );
+      v3_copy( co, ct->co );
+      ct->p = r-d;
+      ct->rba = rba;
+      ct->rbb = &rb_static_null;
+      return 1;
+   }
+
+   return 0;
 }
 
 static int sphere_vs_triangle( v3f c, float r, v3f tri[3], 
@@ -388,12 +1463,19 @@ static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
 }
 
 #include "world.h"
+#endif
 
 static void rb_solver_reset(void)
 {
    rb_contact_count = 0;
 }
 
+static rb_ct *rb_global_ct(void)
+{
+   return rb_contact_buffer + rb_contact_count;
+}
+
+#ifdef RB_DEPR
 static struct contact *rb_start_contact(void)
 {
    if( rb_contact_count == vg_list_size(rb_contact_buffer) )
@@ -454,8 +1536,6 @@ static void rb_build_manifold_terrain_sphere( rigidbody *rb )
             ct->rba = rb;
             v3_copy( co, ct->co );
             v3_copy( norm, ct->n );
-
-            v3_sub( co, rb->co, ct->delta );
             rb_commit_contact( ct, p );
          }
       }
@@ -463,7 +1543,6 @@ static void rb_build_manifold_terrain_sphere( rigidbody *rb )
 
 }
 
-RB_DEPR
 static void rb_build_manifold_terrain( rigidbody *rb )
 {
    v3f *box = rb->bbx;
@@ -520,7 +1599,6 @@ static void rb_build_manifold_terrain( rigidbody *rb )
          v3_copy( hit.normal, ct->n );
          v3_add( point, surface, ct->co );
          v3_muls( ct->co, 0.5f, ct->co );
-         v3_sub( ct->co, rb->co, ct->delta );
 
          rb_commit_contact( ct, p );
          count ++;
@@ -529,67 +1607,167 @@ static void rb_build_manifold_terrain( rigidbody *rb )
       }
    }
 }
+#endif
+
+/*
+ * Initializing things like tangent vectors
+ */
+
+static void rb_presolve_contacts( rb_ct *buffer, int len )
+{
+   for( int i=0; i<len; i++ )
+   {
+      rb_ct *ct = &buffer[i];
+      ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.01f);
+      rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
+
+      ct->norm_impulse = 0.0f;
+      ct->tangent_impulse[0] = 0.0f;
+      ct->tangent_impulse[1] = 0.0f;
+
+      v3f ra, rb, raCn, rbCn, raCt, rbCt;
+      v3_sub( ct->co, ct->rba->co, ra );
+      v3_sub( ct->co, ct->rbb->co, rb );
+      v3_cross( ra, ct->n, raCn );
+      v3_cross( rb, ct->n, rbCn );
+      
+      /* orient inverse inertia tensors */
+      v3f raCnI, rbCnI;
+      m3x3_mulv( ct->rba->iIw, raCn, raCnI );
+      m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI );
+
+      ct->normal_mass  = ct->rba->inv_mass + ct->rbb->inv_mass;
+      ct->normal_mass += v3_dot( raCn, raCnI );
+      ct->normal_mass += v3_dot( rbCn, rbCnI );
+      ct->normal_mass  = 1.0f/ct->normal_mass;
+
+      for( int j=0; j<2; j++ )
+      {
+         v3f raCtI, rbCtI;
+         v3_cross( ct->t[j], ra, raCt );
+         v3_cross( ct->t[j], rb, rbCt );
+         m3x3_mulv( ct->rba->iIw, raCt, raCtI );
+         m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI );
+         
+         ct->tangent_mass[j]  = ct->rba->inv_mass + ct->rbb->inv_mass;
+         ct->tangent_mass[j] += v3_dot( raCt, raCtI );
+         ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
+         ct->tangent_mass[j]  = 1.0f/ct->tangent_mass[j];
+      }
+
+      rb_debug_contact( ct );
+   }
+}
+
+/*
+ * Creates relative contact velocity vector, and offsets between each body
+ */
+static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
+{
+   rigidbody *rba = ct->rba,
+             *rbb = ct->rbb;
+
+   v3_sub( ct->co, rba->co, da );
+   v3_sub( ct->co, rbb->co, db );
+   
+   v3f rva, rvb;
+   v3_cross( rba->w, da, rva );
+   v3_add( rba->v, rva, rva );
+   v3_cross( rbb->w, db, rvb );
+   v3_add( rbb->v, rvb, rvb );
+
+   v3_sub( rva, rvb, rv );
+}
+
+/*
+ * Apply regular and angular velocity impulses to objects involved in contact
+ */
+
+/* TODO REMOVEEE................... */
+static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
+{
+   rigidbody *rba = ct->rba,
+             *rbb = ct->rbb;
+
+   v3_muladds( rba->v, impulse,  rba->inv_mass, rba->v );
+   v3_muladds( rbb->v, impulse, -rbb->inv_mass, rbb->v );
+   
+   /* Angular velocity */
+   v3f wa, wb, invim;
+   v3_cross( da, impulse, wa );
+   v3_negate( impulse, invim );
+   v3_cross( db, invim, wb );
+
+   m3x3_mulv( ct->rba->iIw, wa, wa );
+   m3x3_mulv( ct->rbb->iIw, wb, wb );
+   v3_add( rba->w, wa, rba->w );
+   v3_add( rbb->w, wb, rbb->w );
+}
+
+/* ......... USE THIS */
+static void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
+{
+   /* linear */
+   v3_muladds( rb->v, impulse,  rb->inv_mass, rb->v );
+   
+   /* Angular velocity */
+   v3f wa;
+   v3_cross( delta, impulse, wa );
+
+   m3x3_mulv( rb->iIw, wa, wa );
+   v3_add( rb->w, wa, rb->w );
+}
 
-static void rb_solve_contacts(void)
+/*
+ * One iteration to solve the contact constraint
+ */
+static void rb_solve_contacts( rb_ct *buf, int len )
 {
-   float k_friction = 0.1f;
+   float k_friction = 0.2f;
 
-   /* Friction Impulse */
-   for( int i=0; i<rb_contact_count; i++ )
+   for( int i=0; i<len; i++ )
    {
-      struct contact *ct = &rb_contact_buffer[i];
+      struct contact *ct = &buf[i];
       rigidbody *rb = ct->rba;
 
-      v3f dv;
-      v3_cross( rb->w, ct->delta, dv );
-      v3_add( rb->v, dv, dv );
+      v3f rv, da, db;
+      rb_rcv( ct, rv, da, db );
       
+      /* Friction */
       for( int j=0; j<2; j++ )
       {
-         float vt = vg_clampf( -v3_dot( dv, ct->t[j] ), 
-               -k_friction, k_friction );
-
-         vt = -v3_dot( dv, ct->t[j] );
+         float     f = k_friction * ct->norm_impulse,
+                  vt = v3_dot( rv, ct->t[j] ),
+              lambda = ct->tangent_mass[j] * -vt;
          
          float temp = ct->tangent_impulse[j];
-         ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
-         vt = ct->tangent_impulse[j] - temp;
+         ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
+         lambda = ct->tangent_impulse[j] - temp;
 
          v3f impulse;
-
-         v3_muls( ct->t[j], vt, impulse );
-         v3_add( impulse, rb->v, rb->v );
-         v3_cross( ct->delta, impulse, impulse );
-         v3_add( impulse, rb->w, rb->w );
+         v3_muls( ct->t[j], lambda, impulse );
+         rb_standard_impulse( ct, da, db, impulse );
       }
-   }
-
-   /* Normal Impulse */
-   for( int i=0; i<rb_contact_count; i++ )
-   {
-      struct contact *ct = &rb_contact_buffer[i];
-      rigidbody *rb = ct->rba;
-
-      v3f dv;
-      v3_cross( rb->w, ct->delta, dv );
-      v3_add( rb->v, dv, dv );
 
-      float vn = -v3_dot( dv, ct->n );
-      vn += ct->bias;
+      /* Normal */
+      rb_rcv( ct, rv, da, db );
+      float     vn = v3_dot( rv, ct->n ),
+            lambda = ct->normal_mass * (-vn + ct->bias);
 
       float temp = ct->norm_impulse;
-      ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
-      vn = ct->norm_impulse - temp;
+      ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
+      lambda = ct->norm_impulse - temp;
 
       v3f impulse;
-
-      v3_muls( ct->n, vn, impulse );
-      v3_add( impulse, rb->v, rb->v );
-      v3_cross( ct->delta, impulse, impulse );
-      v3_add( impulse, rb->w, rb->w );
+      v3_muls( ct->n, lambda, impulse );
+      rb_standard_impulse( ct, da, db, impulse );
    }
 }
 
+/*
+ * The following ventures into not really very sophisticated at all maths
+ */
+
 struct rb_angle_limit
 {
    rigidbody *rba, *rbb;
@@ -630,10 +1808,9 @@ static int rb_angle_limit_force(  rigidbody *rba, v3f va,
 
 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 {
-
+   
 }
 
-RB_DEPR
 static void rb_constraint_angle( rigidbody *rba, v3f va,
                                  rigidbody *rbb, v3f vb, 
                                  float max, float spring )
@@ -668,20 +1845,153 @@ static void rb_constraint_angle( rigidbody *rba, v3f va,
    }
 }
 
-static void rb_relative_velocity( rigidbody *ra, v3f lca,
-                                  rigidbody *rb, v3f lcb, v3f rcv )
+static void draw_angle_limit( v3f c, v3f major, v3f minor, 
+                              float amin, float amax, float measured,
+                              u32 colour )
+{
+   float f = 0.05f;
+   v3f ay, ax;
+   v3_muls( major, f, ay );
+   v3_muls( minor, f, ax );
+
+   for( int x=0; x<16; x++ )
+   {
+      float t0 = (float)x / 16.0f,
+            t1 = (float)(x+1) / 16.0f,
+            a0 = vg_lerpf( amin, amax, t0 ),
+            a1 = vg_lerpf( amin, amax, t1 );
+
+      v3f p0, p1;
+      v3_muladds(  c, ay, cosf(a0), p0 );
+      v3_muladds( p0, ax, sinf(a0), p0 );
+      v3_muladds(  c, ay, cosf(a1), p1 );
+      v3_muladds( p1, ax, sinf(a1), p1 );
+      
+      vg_line( p0, p1, colour );
+
+      if( x == 0 )
+         vg_line( c, p0, colour );
+      if( x == 15 )
+         vg_line( c, p1, colour );
+   }
+
+   v3f p2;
+   v3_muladds(  c, ay, cosf(measured)*1.2f, p2 );
+   v3_muladds( p2, ax, sinf(measured)*1.2f, p2 );
+   vg_line( c, p2, colour );
+}
+
+static void rb_debug_constraint_limits( rigidbody *ra, rigidbody *rb, v3f lca,
+                                        v3f limits[2] )
+{
+   v3f ax, ay, az, bx, by, bz;
+   m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+   m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+   v2f px, py, pz;
+   px[0] = v3_dot( ay, by );
+   px[1] = v3_dot( az, by );
+
+   py[0] = v3_dot( az, bz );
+   py[1] = v3_dot( ax, bz );
+
+   pz[0] = v3_dot( ax, bx );
+   pz[1] = v3_dot( ay, bx );
+
+   float r0 = atan2f( px[1], px[0] ),
+         r1 = atan2f( py[1], py[0] ),
+         r2 = atan2f( pz[1], pz[0] );
+
+   v3f c;
+   m4x3_mulv( ra->to_world, lca, c );
+   draw_angle_limit( c, ay, az, limits[0][0], limits[1][0], r0, 0xff0000ff );
+   draw_angle_limit( c, az, ax, limits[0][1], limits[1][1], r1, 0xff00ff00 );
+   draw_angle_limit( c, ax, ay, limits[0][2], limits[1][2], r2, 0xffff0000 );
+}
+
+static void rb_limit_cure( rigidbody *ra, rigidbody *rb, v3f axis, float d )
+{
+   if( d != 0.0f )
+   {
+      float avx = v3_dot( ra->w, axis ) - v3_dot( rb->w, axis );
+      float joint_mass = rb->inv_mass + ra->inv_mass;
+      joint_mass = 1.0f/joint_mass;
+
+      float bias = (0.04f * k_rb_rate) * d,
+            lambda = -(avx + bias) * joint_mass;
+
+      /* Angular velocity */
+      v3f wa, wb;
+      v3_muls( axis,  lambda * ra->inv_mass, wa );
+      v3_muls( axis, -lambda * rb->inv_mass, wb );
+
+      v3_add( ra->w, wa, ra->w );
+      v3_add( rb->w, wb, rb->w );
+   }
+}
+
+static void rb_constraint_limits( rigidbody *ra, v3f lca, 
+                                  rigidbody *rb, v3f lcb, v3f limits[2] )
 {
+   /* TODO: Code dupe remover */
+   v3f ax, ay, az, bx, by, bz;
+   m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+   m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+   v2f px, py, pz;
+   px[0] = v3_dot( ay, by );
+   px[1] = v3_dot( az, by );
+
+   py[0] = v3_dot( az, bz );
+   py[1] = v3_dot( ax, bz );
+
+   pz[0] = v3_dot( ax, bx );
+   pz[1] = v3_dot( ay, bx );
+
+   float r0 = atan2f( px[1], px[0] ),
+         r1 = atan2f( py[1], py[0] ),
+         r2 = atan2f( pz[1], pz[0] );
+
+   /* calculate angle deltas */
+   float dx = 0.0f, dy = 0.0f, dz = 0.0f;
+
+   if( r0 < limits[0][0] ) dx = limits[0][0] - r0;
+   if( r0 > limits[1][0] ) dx = limits[1][0] - r0;
+   if( r1 < limits[0][1] ) dy = limits[0][1] - r1;
+   if( r1 > limits[1][1] ) dy = limits[1][1] - r1;
+   if( r2 < limits[0][2] ) dz = limits[0][2] - r2;
+   if( r2 > limits[1][2] ) dz = limits[1][2] - r2;
+
    v3f wca, wcb;
    m3x3_mulv( ra->to_world, lca, wca );
    m3x3_mulv( rb->to_world, lcb, wcb );
 
-   v3_sub( ra->v, rb->v, rcv );
+   rb_limit_cure( ra, rb, ax, dx );
+   rb_limit_cure( ra, rb, ay, dy );
+   rb_limit_cure( ra, rb, az, dz );
+}
 
-   v3f rcv_Ra, rcv_Rb;
-   v3_cross( ra->w, wca, rcv_Ra );
-   v3_cross( rb->w, wcb, rcv_Rb );
-   v3_add( rcv_Ra, rcv, rcv );
-   v3_sub( rcv, rcv_Rb, rcv );
+static void rb_debug_constraint_position( rigidbody *ra, v3f lca,
+                                          rigidbody *rb, v3f lcb )
+{
+   v3f wca, wcb;
+   m3x3_mulv( ra->to_world, lca, wca );
+   m3x3_mulv( rb->to_world, lcb, wcb );
+
+   v3f p0, p1;
+   v3_add( wca, ra->co, p0 );
+   v3_add( wcb, rb->co, p1 );
+   vg_line_pt3( p0, 0.005f, 0xffffff00 );
+   vg_line_pt3( p1, 0.005f, 0xffffff00 );
+   vg_line( p0, p1, 0xffffff00 );
 }
 
 static void rb_constraint_position( rigidbody *ra, v3f lca,
@@ -692,14 +2002,6 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    m3x3_mulv( ra->to_world, lca, wca );
    m3x3_mulv( rb->to_world, lcb, wcb );
 
-   v3f delta;
-   v3_add( wcb, rb->co, delta );
-   v3_sub( delta, wca, delta );
-   v3_sub( delta, ra->co, delta );
-
-   v3_muladds( ra->co, delta,  0.5f, ra->co );
-   v3_muladds( rb->co, delta, -0.5f, rb->co );
-
    v3f rcv;
    v3_sub( ra->v, rb->v, rcv );
 
@@ -709,41 +2011,47 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    v3_add( rcv_Ra, rcv, rcv );
    v3_sub( rcv, rcv_Rb, rcv );
 
-   float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
-
-   float mass_a = 1.0f/ra->inv_mass,
-         mass_b = 1.0f/rb->inv_mass,
-         total_mass = mass_a+mass_b;
-   
-   v3f impulse;
-   v3_muls( rcv, 1.0f, impulse );
-   v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
-   v3_cross( wcb, impulse, impulse );
-   v3_add( impulse, rb->w, rb->w );
+   v3f delta;
+   v3f p0, p1;
+   v3_add( wca, ra->co, p0 );
+   v3_add( wcb, rb->co, p1 );
+   v3_sub( p1, p0, delta );
 
-   v3_muls( rcv, -1.0f, impulse );
-   v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
-   v3_cross( wca, impulse, impulse );
-   v3_add( impulse, ra->w, ra->w );
+   float dist2 = v3_length2( delta );
 
-#if 0
-   /*
-    * this could be used for spring joints
-    * its not good for position constraint
-    */
-   v3f impulse;
-   v3_muls( delta, 0.5f*spring, impulse );
+   if( dist2 > 0.00001f )
+   {
+      float dist = sqrtf(dist2);
+      v3_muls( delta, 1.0f/dist, delta );
 
-   v3_add( impulse, ra->v, ra->v );
-   v3_cross( wca, impulse, impulse );
-   v3_add( impulse, ra->w, ra->w );
+      float joint_mass = rb->inv_mass + ra->inv_mass;
 
-   v3_muls( delta, -0.5f*spring, impulse );
+      v3f raCn, rbCn, raCt, rbCt;
+      v3_cross( wca, delta, raCn );
+      v3_cross( wcb, delta, rbCn );
+      
+      /* orient inverse inertia tensors */
+      v3f raCnI, rbCnI;
+      m3x3_mulv( ra->iIw, raCn, raCnI );
+      m3x3_mulv( rb->iIw, rbCn, rbCnI );
+      joint_mass += v3_dot( raCn, raCnI );
+      joint_mass += v3_dot( rbCn, rbCnI );
+      joint_mass = 1.0f/joint_mass;
+
+      float vd = v3_dot( rcv, delta ),
+            bias = -(0.08f * k_rb_rate) * dist,
+            lambda = -(vd + bias) * joint_mass;
 
-   v3_add( impulse, rb->v, rb->v );
-   v3_cross( wcb, impulse, impulse );
-   v3_add( impulse, rb->w, rb->w );
-#endif
+      v3f impulse;
+      v3_muls( delta,  lambda, impulse );
+      rb_linear_impulse( ra, wca, impulse );
+      v3_muls( delta, -lambda, impulse );
+      rb_linear_impulse( rb, wcb, impulse );
+
+      /* 'fake' snap */
+      v3_muladds( ra->co, delta,  dist * 0.01f, ra->co );
+      v3_muladds( rb->co, delta, -dist * 0.01f, rb->co );
+   }
 }
 
 static void debug_sphere( m4x3f m, float radius, u32 colour )
@@ -780,6 +2088,88 @@ static void debug_sphere( m4x3f m, float radius, u32 colour )
    }
 }
 
+static void debug_capsule( m4x3f m, float radius, float h, u32 colour )
+{
+   v3f ly = { 0.0f, 0.0f, radius },
+       lx = { 0.0f, radius, 0.0f },
+       lz = { 0.0f, 0.0f, radius };
+
+   float s0 = sinf(0.0f)*radius,
+         c0 = cosf(0.0f)*radius;
+
+   v3f p0, p1, up, right, forward;
+   m3x3_mulv( m, (v3f){0.0f,1.0f,0.0f}, up );
+   m3x3_mulv( m, (v3f){1.0f,0.0f,0.0f}, right );
+   m3x3_mulv( m, (v3f){0.0f,0.0f,-1.0f}, forward );
+   v3_muladds( m[3], up, -h*0.5f+radius, p0 );
+   v3_muladds( m[3], up,  h*0.5f-radius, p1 );
+
+   v3f a0, a1, b0, b1;
+   v3_muladds( p0, right, radius, a0 );
+   v3_muladds( p1, right, radius, a1 );
+   v3_muladds( p0, forward, radius, b0 );
+   v3_muladds( p1, forward, radius, b1 );
+   vg_line( a0, a1, colour );
+   vg_line( b0, b1, colour );
+
+   v3_muladds( p0, right, -radius, a0 );
+   v3_muladds( p1, right, -radius, a1 );
+   v3_muladds( p0, forward, -radius, b0 );
+   v3_muladds( p1, forward, -radius, b1 );
+   vg_line( a0, a1, colour );
+   vg_line( b0, b1, colour );
+   
+   for( int i=0; i<16; i++ )
+   {
+      float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
+            s1 = sinf(t)*radius,
+            c1 = cosf(t)*radius;
+
+      v3f e0 = { s0, 0.0f, c0 },
+          e1 = { s1, 0.0f, c1 },
+          e2 = { s0, c0, 0.0f },
+          e3 = { s1, c1, 0.0f },
+          e4 = { 0.0f, c0, s0 },
+          e5 = { 0.0f, c1, s1 };
+
+      m3x3_mulv( m, e0, e0 );
+      m3x3_mulv( m, e1, e1 );
+      m3x3_mulv( m, e2, e2 );
+      m3x3_mulv( m, e3, e3 );
+      m3x3_mulv( m, e4, e4 );
+      m3x3_mulv( m, e5, e5 );
+
+      v3_add( p0, e0, a0 );
+      v3_add( p0, e1, a1 );
+      v3_add( p1, e0, b0 );
+      v3_add( p1, e1, b1 );
+
+      vg_line( a0, a1, colour );
+      vg_line( b0, b1, colour );
+
+      if( c0 < 0.0f )
+      {
+         v3_add( p0, e2, a0 );
+         v3_add( p0, e3, a1 );
+         v3_add( p0, e4, b0 );
+         v3_add( p0, e5, b1 );
+      }
+      else
+      {
+         v3_add( p1, e2, a0 );
+         v3_add( p1, e3, a1 );
+         v3_add( p1, e4, b0 );
+         v3_add( p1, e5, b1 );
+      }
+
+      vg_line( a0, a1, colour );
+      vg_line( b0, b1, colour );
+
+      s0 = s1;
+      c0 = c1;
+   }
+}
+
 static void rb_debug( rigidbody *rb, u32 colour )
 {
    if( rb->type == k_rb_shape_box )
@@ -791,8 +2181,21 @@ static void rb_debug( rigidbody *rb, u32 colour )
    {
       debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
    }
+   else if( rb->type == k_rb_shape_capsule )
+   {
+      m4x3f m0, m1;
+      float h = rb->inf.capsule.height,
+            r = rb->inf.capsule.radius;
+
+      debug_capsule( rb->to_world, r, h, colour );
+   }
+   else if( rb->type == k_rb_shape_scene )
+   {
+      vg_line_boxf( rb->bbx, colour );
+   }
 }
 
+#ifdef RB_DEPR
 /*
  * out penetration distance, normal
  */
@@ -842,114 +2245,6 @@ static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
    return 0;
 }
 
-#if 0
-static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
-{
-   v3f verts[8];
-
-   v3f a, b;
-   v3_copy( ra->bbx[0], a );
-   v3_copy( ra->bbx[1], b );
-   
-	m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
-	m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
-	m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
-	m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
-	m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
-	m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
-	m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
-	m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
-
-   vg_line_boxf_transformed( rb_static->to_world, rb_static->bbx, 0xff0000ff );
-
-   int count = 0;
-
-   for( int i=0; i<8; i++ )
-   {
-      if( ra->manifold_count == vg_list_size(ra->manifold) )
-         return;
-
-      struct contact *ct = &ra->manifold[ ra->manifold_count ];
-      
-      float p;
-      v3f normal;
-
-      if( rb_point_in_body( rb_static, verts[i], &p, normal ))
-      {
-         v3_copy( normal, ct->n );
-         v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
-         v3_sub( ct->co, ra->co, ct->delta );
-
-         vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
-         
-         ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
-         rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
-
-         ct->norm_impulse = 0.0f;
-         ct->tangent_impulse[0] = 0.0f;
-         ct->tangent_impulse[1] = 0.0f;
-
-         ra->manifold_count ++;
-         count ++;
-         if( count == 4 )
-            return;
-      }
-   }
-}
-#endif
-
-/* 
- * Capsule phyics
- */
-
-static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
-{
-   v3f last = { 0.0f, 0.0f, radius };
-   m4x3f lower, upper;
-   m3x3_copy( m, lower );
-   m3x3_copy( m, upper );
-   m4x3_mulv( m, (v3f){0.0f,-height*0.5f+radius,0.0f}, lower[3] );
-   m4x3_mulv( m, (v3f){0.0f, height*0.5f-radius,0.0f}, upper[3] );
-   
-   for( int i=0; i<16; i++ )
-   {
-      float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
-            s = sinf(t),
-            c = cosf(t);
-
-      v3f p = { s*radius, 0.0f, c*radius };
-
-      v3f p0, p1;
-      m4x3_mulv( lower, p, p0 );
-      m4x3_mulv( lower, last, p1 );
-      vg_line( p0, p1, colour );
-
-      m4x3_mulv( upper, p, p0 );
-      m4x3_mulv( upper, last, p1 );
-      vg_line( p0, p1, colour );
-
-      v3_copy( p, last );
-   }
-
-   for( int i=0; i<4; i++ )
-   {
-      float t = ((float)(i) * (1.0f/4.0f)) * VG_PIf * 2.0f,
-            s = sinf(t),
-            c = cosf(t);
-
-      v3f p = { s*radius, 0.0f, c*radius };
-
-      v3f p0, p1;
-      m4x3_mulv( lower, p, p0 );
-      m4x3_mulv( upper, p, p1 );
-      vg_line( p0, p1, colour );
-
-      m4x3_mulv( lower, (v3f){0.0f,-radius,0.0f}, p0 );
-      m4x3_mulv( upper, (v3f){0.0f, radius,0.0f}, p1 );
-      vg_line( p0, p1, colour );
-   }
-}
-
 /*
  * BVH implementation, this is ONLY for static rigidbodies, its to slow for
  * realtime use.
@@ -993,4 +2288,6 @@ static bh_system bh_system_rigidbodies =
    .cast_ray = NULL
 };
 
+#endif
+
 #endif /* RIGIDBODY_H */