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

diff --git a/rigidbody.h b/rigidbody.h
index b140cbb..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,7 +13,7 @@ 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)
 
@@ -26,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;
    
@@ -45,6 +47,12 @@ struct rigidbody
          float height, radius;
       } 
       capsule;
+
+      struct rb_scene
+      {
+         scene *pscene;
+      }
+      scene;
    }
    inf;
 
@@ -55,10 +63,17 @@ struct rigidbody
    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},
@@ -68,6 +83,7 @@ static rigidbody rb_static_null =
    .is_world = 1,
    .inv_mass = 0.0f
 };
+#endif
 
 static void rb_debug( rigidbody *rb, u32 colour );
 
@@ -76,11 +92,20 @@ static struct contact
    rigidbody *rba, *rbb;
    v3f co, n;
    v3f t[2];
-   float mass_total, p, 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 );
@@ -89,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 )
@@ -112,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 )
    {
@@ -130,14 +162,42 @@ 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 );
+   }
 
    if( rb->is_world )
    {
       rb->inv_mass = 0.0f;
+      v3_zero( rb->I );
+      m3x3_zero(rb->iI);
    }
    else
    {
-      rb->inv_mass = 1.0f/(8.0f*volume);
+      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 );
@@ -148,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 */
@@ -195,8 +255,10 @@ 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 );
+#endif
 static void rb_solve_contacts( rb_ct *buf, int len );
 static void rb_presolve_contacts( rb_ct *buffer, int len );
 
@@ -396,6 +458,136 @@ static void closest_on_triangle( v3f p, v3f tri[3], v3f 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
  *
@@ -409,8 +601,8 @@ static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
 static void rb_debug_contact( rb_ct *ct )
 {
    v3f p1;
-   v3_muladds( ct->co, ct->n, 0.2f, p1 );
-   vg_line_pt3( ct->co, 0.1f, 0xff0000ff );
+   v3_muladds( ct->co, ct->n, 0.1f, p1 );
+   vg_line_pt3( ct->co, 0.025f, 0xff0000ff );
    vg_line( ct->co, p1, 0xffffffff );
 }
 
@@ -814,6 +1006,320 @@ static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    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",
@@ -837,20 +1343,53 @@ static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    return rb_sphere_vs_box( rbb, rba, buf );
 }
 
-static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
-= {
-              /* box */         /* Sphere */         /* Capsule */
-/*box    */ { RB_MATRIX_ERROR,  rb_box_vs_sphere,    rb_box_vs_capsule,    RB_MATRIX_ERROR },
-/*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, RB_MATRIX_ERROR },
+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_MATRIX_ERROR,  RB_MATRIX_ERROR,     RB_MATRIX_ERROR,      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) )
+   {
+      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;
+   }
+
+   /*
+    * TODO: Replace this with a more dedicated broad phase pass
+    */
+   if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
+   {
+      int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count); 
+      rb_contact_count += count;
+      return count;
+   }
+   else
+      return 0;
+}
 
 /*
  * Generic functions
  */
 
+#ifdef RB_DEPR
 /* 
  * This function does not accept triangle as a dynamic object, it is assumed
  * to always be static.
@@ -891,8 +1430,6 @@ static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
    return 0;
 }
 
-
-RB_DEPR
 static int sphere_vs_triangle( v3f c, float r, v3f tri[3], 
       v3f co, v3f norm, float *p )
 {
@@ -926,6 +1463,7 @@ static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
 }
 
 #include "world.h"
+#endif
 
 static void rb_solver_reset(void)
 {
@@ -937,6 +1475,7 @@ 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) )
@@ -1004,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;
@@ -1069,6 +1607,7 @@ static void rb_build_manifold_terrain( rigidbody *rb )
       }
    }
 }
+#endif
 
 /*
  * Initializing things like tangent vectors
@@ -1079,13 +1618,42 @@ 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.04f);
+      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;
-      ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass);
+
+      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 );
    }
@@ -1114,35 +1682,49 @@ static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
 /*
  * 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;
 
-   v3f ia, ib;
-   v3_muls( impulse,  ct->mass_total*rba->inv_mass, ia );
-   v3_muls( impulse, -ct->mass_total*rbb->inv_mass, ib );
-
-   /* response */
-   v3_add( rba->v, ia, rba->v );
-   v3_add( rbb->v, ib, rbb->v );
+   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;
-   v3_cross( da, ia, wa );
-   v3_cross( db, ib, wb );
+   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 );
+}
+
 /*
  * 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<len; i++ )
    {
       struct contact *ct = &buf[i];
@@ -1151,39 +1733,33 @@ static void rb_solve_contacts( rb_ct *buf, int len )
       v3f rv, da, db;
       rb_rcv( ct, rv, da, db );
       
+      /* Friction */
       for( int j=0; j<2; j++ )
       {
-         float f = k_friction * ct->norm_impulse,
-              vt = -v3_dot( rv, 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, -f, f );
-         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_muls( ct->t[j], lambda, impulse );
          rb_standard_impulse( ct, da, db, impulse );
       }
-   }
-
-   /* Normal Impulse */
-   for( int i=0; i<len; i++ )
-   {
-      struct contact *ct = &buf[i];
-      rigidbody *rba = ct->rba,
-                *rbb = ct->rbb;
 
-      v3f rv, da, db;
+      /* Normal */
       rb_rcv( ct, rv, da, db );
-
-      float vn = -v3_dot( rv, ct->n ) + ct->bias;
+      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_muls( ct->n, lambda, impulse );
       rb_standard_impulse( ct, da, db, impulse );
    }
 }
@@ -1232,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 )
@@ -1270,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,
@@ -1294,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 );
 
@@ -1311,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 )
@@ -1483,8 +2189,13 @@ static void rb_debug( rigidbody *rb, u32 colour )
 
       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
  */
@@ -1577,4 +2288,6 @@ static bh_system bh_system_rigidbodies =
    .cast_ray = NULL
 };
 
+#endif
+
 #endif /* RIGIDBODY_H */