From fce86711735b15bff37de0f70716808410fcf269 Mon Sep 17 00:00:00 2001
From: hgn <hgodden00@gmail.com>
Date: Sun, 18 Feb 2024 01:24:26 +0000
Subject: [PATCH] labs work

---
 labs/physics.c           | 516 +++++++++++++++++++++++++++++++++++++--
 vg.h                     |  27 +-
 vg_audio.h               |   2 +-
 vg_bvh.h                 | 415 +++++++++++++++++++++++++++++++
 vg_imgui.h               |   4 +-
 vg_profiler.h            |  60 ++---
 vg_rigidbody.h           |  54 +++-
 vg_rigidbody_collision.h | 232 ++++++++++++------
 vg_rigidbody_view.h      |   4 +-
 vg_shader.h              |   2 +-
 10 files changed, 1161 insertions(+), 155 deletions(-)
 create mode 100644 vg_bvh.h

diff --git a/labs/physics.c b/labs/physics.c
index e741f12..7373660 100644
--- a/labs/physics.c
+++ b/labs/physics.c
@@ -10,8 +10,93 @@
 
 #define SDL_MAIN_HANDLED
 
+#define VG_MAX_CONTACTS 2048
+
 #include "vg/vg.h"
 #include "vg/vg_camera.h"
+#include "vg/vg_rigidbody.h"
+#include "vg/vg_rigidbody_collision.h"
+#include "vg/vg_profiler.h"
+#include "vg/vg_bvh.h"
+
+#define SHAPE_MAX 256
+static rigidbody shapes[SHAPE_MAX];
+static rb_capsule shapes_inf[SHAPE_MAX];
+static v4f  shapes_colour[SHAPE_MAX];
+static boxf shapes_bbx[SHAPE_MAX];
+
+static boxf floor_box = {{-6.999,-2.001,-6.999},{6.999,-0.999,6.999}};
+
+static rigidbody racket;
+
+static f32 k_iterations = 8.0f,
+           k_view_x = 0.0f,
+           k_view_y = 0.2f,
+           k_view_z = 10.0f,
+           k_shapes = 32.0f,
+           k_racket_d  = 8.0f;
+
+static rb_capsule racket_ca = { .h = 4.3f, .r = 0.6f }, 
+                  racket_cb = { .h = 5.5f, .r = 0.9f };
+static m4x3f racket_a_mdl, racket_b_mdl;
+static m3x3f racket_I;
+
+static v3f k_racket_init_w;
+static i32 k_demo = 0;
+static i32 k_gyro = 0;
+static i32 k_prof_normalize = 0;
+static i32 k_bbx = 1;
+static i32 k_spacial = 0;
+
+static struct vg_profile prof_refit   = { .name = "Refit" },
+                         prof_broad = { .name = "Broad phase", 
+                                        .mode = k_profile_mode_accum }, 
+                         prof_narrow = { .name = "Narrow phase", 
+                                         .mode = k_profile_mode_accum },
+                         prof_solve   = { .name = "Solver" };
+
+static void shape_bvh_expand_bound( void *user, boxf bound, u32 item_index ){
+   box_concat( bound, shapes_bbx[item_index] );
+}
+
+static f32 shape_bvh_centroid( void *user, u32 item_index, int axis ){
+   f32 x = shapes_bbx[item_index][0][axis] + shapes_bbx[item_index][1][axis];
+   return x*0.5f;
+}
+
+static void shape_bvh_closest( void *user, u32 item_index, 
+                               v3f point, v3f closest ){
+   closest_point_aabb( point, shapes_bbx[item_index], closest );
+}
+
+static void shape_bvh_swap( void *user, u32 ia, u32 ib ){
+   rigidbody temp = shapes[ib];
+   shapes[ib] = shapes[ia];
+   shapes[ia] = temp;
+
+   rb_capsule cb = shapes_inf[ib];
+   shapes_inf[ib] = shapes_inf[ia];
+   shapes_inf[ia] = cb;
+
+   v4f colourb;
+   v4_copy( shapes_colour[ib], colourb );
+   v4_copy( shapes_colour[ia], shapes_colour[ib] );
+   v4_copy( colourb, shapes_colour[ia] );
+
+   boxf boxb;
+   box_copy( shapes_bbx[ib], boxb );
+   box_copy( shapes_bbx[ia], shapes_bbx[ib] );
+   box_copy( boxb, shapes_bbx[ia] );
+}
+
+static bh_system shape_bvh = {
+   .expand_bound = shape_bvh_expand_bound,
+   .item_centroid = shape_bvh_centroid,
+   .item_closest = shape_bvh_closest,
+   .item_swap = shape_bvh_swap
+};
+
+static bh_tree *shape_bvh_tree = NULL;
 
 int main( int argc, char *argv[] ){
    vg_mem.use_libc_malloc = 0;
@@ -28,17 +113,278 @@ static void vg_preload(void){
    vg_audio.dsp_enabled = 0;
 }
 
+static void init_random(void){
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      f32 h  = vg_randf64( &vg.rand ) * 2.0f + 1.3f,
+          r  = vg_randf64( &vg.rand ) * 0.5f + 0.125f,
+          pv = vg_capsule_volume( r, h ),
+          k_density = 8.0f,
+          pm = pv * k_density;
+
+      shapes_inf[i].r = r;
+      shapes_inf[i].h = h;
+
+      m3x3f pI;
+      vg_capsule_inertia( r, h, pm, pI );
+      m3x3_inv( pI, shapes[i].iI );
+      shapes[i].inv_mass = 1.0f / pm;
+
+      v3f dir;
+      vg_rand_dir( &vg.rand, dir );
+      q_axis_angle( shapes[i].q, dir, vg_randf64(&vg.rand)*VG_TAUf );
+      vg_rand_sphere( &vg.rand, shapes[i].co );
+      v3_muladds( (v3f){0,4,0}, shapes[i].co, 4.0f, shapes[i].co );
+      v3_zero( shapes[i].v );
+      v3_zero( shapes[i].w );
+      shapes_colour[i][0] = vg_randf64(&vg.rand);
+      shapes_colour[i][1] = vg_randf64(&vg.rand);
+      shapes_colour[i][2] = vg_randf64(&vg.rand);
+      shapes_colour[i][3] = 1.0f;
+      rb_update_matrices( &shapes[i] );
+   }
+}
+
+static void init_racket(void){
+   f32 ma = vg_capsule_volume( racket_ca.r, racket_ca.h ) * k_racket_d,
+       mb = vg_capsule_volume( racket_cb.r, racket_cb.h ) * k_racket_d,
+       mt = ma+mb;
+   m3x3f aI, bI;
+
+   /* tensor for A */
+   vg_capsule_inertia( racket_ca.r, racket_ca.h, ma, aI );
+   m4x3_identity( racket_a_mdl );
+   racket_a_mdl[3][1] = -racket_ca.h*0.5f*(mb/mt);
+   vg_translate_inertia( aI, ma, racket_a_mdl[3] );
+
+   /* tensor for B */
+   vg_capsule_inertia( racket_cb.r, racket_cb.h, mb, bI );
+
+   v4f q;
+   q_axis_angle( q, (v4f){1,0,0}, VG_TAUf*0.25f );
+
+   m4x3_identity( racket_b_mdl );
+   q_m3x3( q, racket_b_mdl );
+   vg_rotate_inertia( bI, racket_b_mdl );
+   racket_b_mdl[3][1] = racket_ca.h*0.5f*(ma/mt);
+   vg_translate_inertia( bI, mb, racket_b_mdl[3] );
+
+   m3x3_add( aI, bI, racket_I );
+   m3x3_inv( racket_I, racket.iI );
+   racket.inv_mass = 1.0f/(mb+ma);
+}
+
+static void reset_racket(void){
+   q_identity( racket.q );
+   v3_zero( racket.co );
+   v3_copy( k_racket_init_w, racket.w );
+   v3_zero( racket.v );
+   rb_update_matrices( &racket );
+}
+
 static void vg_load(void){
    vg_bake_shaders();
+   init_random();
+   shape_bvh_tree = bh_create( NULL, &shape_bvh, NULL, SHAPE_MAX, 1 );
+   init_racket();
+   reset_racket();
 }
 
 static void vg_pre_update(void){
+   vg_console.cheats =  1;
+   vg_lines.render = 1;
+}
+
+static void demo0_refit(void){
+   if( k_spacial == 0 ) return;
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      f32 h = shapes_inf[i].h,
+          r = shapes_inf[i].r;
+
+      rigidbody *rb = &shapes[i];
+
+      v3f p0, p1;
+      v3_muladds( rb->to_world[3], rb->to_world[1], -h*0.5f+r, p0 );
+      v3_muladds( rb->to_world[3], rb->to_world[1],  h*0.5f-r, p1 );
+      
+      v3f *bbx = shapes_bbx[i];
+      v3_minv( p0, p1, bbx[0] );
+      v3_maxv( p0, p1, bbx[1] );
+      v3_muladds( bbx[0], (v3f){-1,-1,-1}, r, bbx[0] );
+      v3_muladds( bbx[1], (v3f){ 1, 1, 1}, r, bbx[1] );
+   }
+
+   if( k_spacial == 1 ) return;
+
+   if( k_spacial == 2 )
+      bh_rebuild( shape_bvh_tree, (u32)k_shapes );
+}
+
+static void demo0(void){
+   vg_profile_begin( &prof_refit );
+   demo0_refit();
+   vg_profile_end( &prof_refit );
+
+   static rigidbody _null,
+                    _mover;
+   rb_solver_reset();
+
+   f32 t = vg.time * 0.1f * VG_TAUf;
+   v3f sphere_pos = { sinf(t)*2.0f, -1, cosf(t)*2.0f };
+   _mover.v[0] = (sinf(t+vg.time_fixed_delta)-sinf(t))*2.0f;
+   _mover.v[2] = (cosf(t+vg.time_fixed_delta)-cosf(t))*2.0f;
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      rigidbody *rbi = &shapes[i];
+      rb_capsule *infi = &shapes_inf[i];
+      v3f *bbxi = shapes_bbx[i];
+
+      if( rb_global_has_space() ){
+         rb_ct *buf = rb_global_buffer();
+         m4x3f mtx;
+         m4x3_identity( mtx );
+         u32 l = rb_capsule__box( rbi->to_world, infi,
+                                  mtx, mtx, floor_box, buf );
+
+         for( u32 k=0; k<l; k ++ ){
+            buf[k].rba = rbi;
+            buf[k].rbb = &_null;
+         }
+
+         rb_contact_count += l;
+      }
+      else break;
+
+      if( rb_global_has_space() ){
+         rb_ct *buf = rb_global_buffer();
+         u32 l = rb_capsule__sphere( rbi->to_world, infi, sphere_pos, 1, buf );
+
+         for( u32 k=0; k<l; k ++ ){
+            buf[k].rba = rbi;
+            buf[k].rbb = &_mover;
+         }
+
+         rb_contact_count += l;
+      }
+      
+      if( k_spacial == 2 ){
+         bh_iter it;
+         bh_iter_init_box( 0, &it, bbxi );
+         i32 idx;
+
+         while(1){
+            vg_profile_begin( &prof_broad );
+            if( !bh_next( shape_bvh_tree, &it, &idx ) ){
+               vg_profile_end( &prof_broad );
+               break;
+            }
+            vg_profile_end( &prof_broad );
+
+            if( idx <= i ) continue;
+
+            vg_profile_begin( &prof_narrow );
+
+            rigidbody *rbj = &shapes[idx];
+            v3f *bbxj = shapes_bbx[idx];
+            rb_capsule *infj = &shapes_inf[idx];
+
+            if( rb_global_has_space() ){
+               rb_ct *buf = rb_global_buffer();
+               u32 l = rb_capsule__capsule( rbi->to_world, infi,
+                                            rbj->to_world, infj, buf );
+
+               for( u32 k=0; k<l; k ++ ){
+                  buf[k].rba = rbi;
+                  buf[k].rbb = rbj;
+               }
+
+               rb_contact_count += l;
+            }
+
+            vg_profile_end( &prof_narrow );
+         }
+      }
+      else {
+         if( i == ((u32)k_shapes)-1 ){
+            break;
+         }
+         for( u32 j=i+1; j<(u32)k_shapes; j ++ ){
+            rigidbody *rbj = &shapes[j];
+            v3f *bbxj = shapes_bbx[j];
+            rb_capsule *infj = &shapes_inf[j];
+
+            if( k_spacial == 1 ){
+               vg_profile_begin( &prof_broad );
+               if( !box_overlap( bbxi, bbxj ) ){
+                  vg_profile_end( &prof_broad );
+                  continue;
+               }
+            }
+
+            vg_profile_begin( &prof_narrow );
+            if( rb_global_has_space() ){
+               rb_ct *buf = rb_global_buffer();
+               u32 l = rb_capsule__capsule( rbi->to_world, infi,
+                                            rbj->to_world, infj, buf );
+
+               for( u32 k=0; k<l; k ++ ){
+                  buf[k].rba = rbi;
+                  buf[k].rbb = rbj;
+               }
+
+               rb_contact_count += l;
+            }
+            vg_profile_end( &prof_narrow );
+         }
+      }
+   }
+
+   vg_profile_increment( &prof_broad );
+   vg_profile_increment( &prof_narrow );
+
+   vg_profile_begin( &prof_solve );
+   rb_presolve_contacts( rb_contact_buffer, rb_contact_count );
+   for( u32 i=0; i<(u32)k_iterations; i ++ )
+      rb_solve_contacts( rb_contact_buffer, rb_contact_count );
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      rigidbody *rbi = &shapes[i];
+      if( k_gyro ){
+         m3x3f I;
+         m3x3_inv( rbi->iI, I );
+         rb_solve_gyroscopic( rbi, I, vg.time_fixed_delta );
+      }
+      rb_iter( rbi );
+      rb_update_matrices( rbi );
+   }
+   vg_profile_end( &prof_solve );
+}
+
+static void demo1(void){
+   vg_profile_begin( &prof_refit );
+   vg_profile_end( &prof_refit );
+
+   vg_profile_increment( &prof_broad );
+   vg_profile_increment( &prof_narrow );
+
+   vg_profile_begin( &prof_solve );
+
+   v3_muladds( racket.v, (v3f){0,9.8f,0}, vg.time_fixed_delta, racket.v );
+   if( k_gyro ) rb_solve_gyroscopic( &racket, racket_I, vg.time_fixed_delta );
+   rb_iter( &racket );
+   rb_update_matrices( &racket );
+
+   vg_profile_end( &prof_solve );
 }
 
 static void vg_fixed_update(void){
+   if( k_demo == 0 ) demo0();
+   else if( k_demo == 1 ) demo1();
 }
 
 static void vg_post_update(void){
+   if( vg_getkey( SDLK_8 ) )
+      init_random();
 }
 
 static void vg_framebuffer_resize( int w, int h ){
@@ -50,6 +396,54 @@ static void draw_origin_axis(void){
    vg_line( (v3f){ 0.0f, 0.0f, 0.0f }, (v3f){ 0.0f, 0.0f, 1.0f }, 0xff0000ff );
 }
 
+static void render0(void){
+   f32 t = vg.time * 0.1f * VG_TAUf;
+   m4x3f mdl;
+   m4x3_identity( mdl );
+   vg_rb_view_box( mdl, floor_box, (v4f){0.8f,0.8f,0.8f,1} );
+
+   mdl[3][0] = sinf(t)*2.0f;
+   mdl[3][1] = -1;
+   mdl[3][2] = cosf(t)*2.0f;
+   vg_rb_view_sphere( mdl, 1, (v4f){0,1,0,1} );
+
+   for( u32 i=0; i<(u32)k_shapes; i ++ ){
+      rigidbody *rbi = &shapes[i];
+      rb_capsule *infi = &shapes_inf[i];
+      f32 *coli = shapes_colour[i];
+
+      v4f q;
+      v3f co;
+      rb_extrapolate( rbi, co, q );
+      q_m3x3( q, mdl );
+      v3_copy( co, mdl[3] );
+      vg_rb_view_capsule( mdl, infi->r, infi->h, coli );
+   }
+
+   if( k_spacial && k_bbx ){
+      for( u32 i=0; i<(u32)k_shapes; i ++ ){
+         vg_line_boxf( shapes_bbx[i], VG__RED );
+      }
+
+      if( k_spacial == 2 ){
+         bh_debug_trace( shape_bvh_tree, 0, (v3f){0,0,0}, VG__GREEN );
+      }
+   }
+}
+
+static void render1(void){
+   m4x3f mdl, mmdl;
+   v4f q;
+   rb_extrapolate( &racket, mdl[3], q );
+   q_m3x3( q, mdl );
+
+   m4x3_mul( mdl, racket_a_mdl, mmdl );
+   vg_rb_view_capsule( mmdl, racket_ca.r, racket_ca.h, (v4f){1,0,0,1} );
+
+   m4x3_mul( mdl, racket_b_mdl, mmdl );
+   vg_rb_view_capsule( mmdl, racket_cb.r, racket_cb.h, (v4f){0,1,0,1} );
+}
+
 static void vg_render(void){
    glBindFramebuffer( GL_FRAMEBUFFER, 0 );
    glViewport( 0,0, vg.window_x, vg.window_y );
@@ -59,19 +453,14 @@ static void vg_render(void){
    glClearColor( 0.05f, 0.05f, 0.05f, 1.0f );
    glClear( GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT );
 
-   f32 x = -((f32)vg.mouse_pos[0] / (f32)vg.window_x) * VG_TAUf,
-       y = (((f32)vg.mouse_pos[1] / (f32)vg.window_y) - 0.5f)*VG_PIf;
-
-   f32 t = vg.time * 0.1f * VG_TAUf;
-
    vg_camera cam = {
-      .angles = { -x, y, 0 },
+      .angles = { -k_view_x, k_view_y, 0 },
       .nearz = 0.01f,
       .farz = 500.0f,
       .fov = 90.0f,
-      .pos = { sinf(x)*10.0f*cosf(y), 
-               sinf(y)*10.0f, 
-               cosf(x)*10.0f*cosf(y) },
+      .pos = { sinf(k_view_x)*k_view_z*cosf(k_view_y), 
+               sinf(k_view_y)*k_view_z, 
+               cosf(k_view_x)*k_view_z*cosf(k_view_y) },
    };
 
    vg_camera_update_transform( &cam );
@@ -80,19 +469,9 @@ static void vg_render(void){
    vg_camera_finalize( &cam );
    m4x4_copy( cam.mtx.pv, vg.pv );
 
-   m4x3f mdl;
-   m4x3_identity( mdl );
-
    vg_rb_view_bind();
-   vg_rb_view_capsule( mdl, (sinf(t*0.2f)*0.5f+0.55f)*2.0f, 
-                            (sinf(t*0.33f)*0.5f+0.55f)*1.0f,
-                            (v4f){1,0,0,1} );
-   vg_rb_view_box( mdl, (boxf){{-4.999,-2.001,-4.999},{4.999,-0.999,4.999}}, 
-                  (v4f){0.8f,0.8f,0.8f,1} );
-
-   mdl[3][0] = sinf(t)*2.0f;
-   mdl[3][2] = cosf(t)*2.0f;
-   vg_rb_view_sphere( mdl, 1, (v4f){0,1,0,1} );
+   if( k_demo == 0 ) render0();
+   else if( k_demo == 1 ) render1();
 
    draw_origin_axis();
    vg_lines_drawall();
@@ -100,6 +479,101 @@ static void vg_render(void){
    glDisable(GL_DEPTH_TEST);
 }
 
+struct ui_enum_opt spacial_mode_ui_enum[] = { 
+   { 0, "None" },
+   { 1, "BBX" },
+   { 2, "BVH - Full rebuild" },
+   { 3, "BVH - Temporal fitting" }
+};
+
+static void gui0( ui_rect panel ){
+   ui_slider( panel, "Shapes", 2, vg_list_size(shapes), &k_shapes, "%.0f" );
+
+   if( ui_button( panel, "randomize" ) == k_ui_button_click ){
+      init_random();
+   }
+}
+
+static void gui1( ui_rect panel ){
+   ui_rect l, r;
+   ui_standard_widget( panel, l, 1 );
+   ui_split_ratio( l, k_ui_axis_v, 0.5f, 4, l, r );
+   ui_slider( l, "aH", 1.0f, 10.0f, &racket_ca.h, "%.1f" );
+   ui_slider( r, "aR", 0.1f, 10.0f, &racket_ca.r, "%.1f" );
+
+   ui_standard_widget( panel, l, 1 );
+   ui_split_ratio( l, k_ui_axis_v, 0.5f, 4, l, r );
+   ui_slider( l, "bH", 1.0f, 10.0f, &racket_cb.h, "%.1f" );
+   ui_slider( r, "bR", 0.1f, 10.0f, &racket_cb.r, "%.1f" );
+
+   for( u32 i=0; i<3; i ++ ){
+      ui_rect v0,v1,v2;
+      ui_standard_widget( panel, v0, 1 );
+      ui_split_ratio( v0, k_ui_axis_v, 2.0f/3.0f, 4, v0, v2 );
+      ui_split_ratio( v0, k_ui_axis_v, 1.0f/2.0f, 4, v0, v1 );
+
+      char buf[16];
+      snprintf( buf, sizeof(buf), "%.3f", racket_I[i][0] );
+      ui_text( v0, buf, 1, k_ui_align_middle_center, 0 );
+      snprintf( buf, sizeof(buf), "%.3f", racket_I[i][1] );
+      ui_text( v1, buf, 1, k_ui_align_middle_center, 0 );
+      snprintf( buf, sizeof(buf), "%.3f", racket_I[i][2] );
+      ui_text( v2, buf, 1, k_ui_align_middle_center, 0 );
+   }
+
+   init_racket();
+
+   ui_info( panel, "init conditions" );
+   ui_rect v0,v1,v2;
+   ui_standard_widget( panel, v0, 1 );
+   ui_split_ratio( v0, k_ui_axis_v, 2.0f/3.0f, 4, v0, v2 );
+   ui_split_ratio( v0, k_ui_axis_v, 1.0f/2.0f, 4, v0, v1 );
+   ui_slider( v0, "X", 0.01f, 30.0f, k_racket_init_w+0, "%.1f" );
+   ui_slider( v1, "Y", 0.01f, 30.0f, k_racket_init_w+1, "%.1f" );
+   ui_slider( v2, "Z", 0.01f, 30.0f, k_racket_init_w+2, "%.1f" );
+
+   if( ui_button( panel, "init" ) == k_ui_button_click ){
+      reset_racket();
+   }
+}
+
 static void vg_gui(void){
    vg_ui.wants_mouse = 1;
+   ui_rect panel = { vg.window_x-300, 0, 300, vg.window_y };
+   ui_rect_pad( panel, (ui_px[2]){ 8, 8 } );
+
+   ui_rect box;
+   ui_split( panel, k_ui_axis_h, VG_PROFILE_SAMPLE_COUNT*2 +4, 8, box, panel );
+   vg_profile_drawn( (struct vg_profile *[]){ &prof_refit, 
+                                              &prof_broad,
+                                              &prof_narrow,
+                                              &prof_solve }, 4,
+                     vg.time_fixed_delta*1000.0, box, 0, k_prof_normalize );
+
+   ui_split( panel, k_ui_axis_h, 14*2+8, 4, box, panel );
+   ui_checkbox( panel, "Normalize", &k_prof_normalize );
+
+   ui_slider( panel, "Iterations", 1.0f, 20.0f, &k_iterations, "%.0f" );
+   ui_enum( panel, "Spacial Type", spacial_mode_ui_enum,
+            vg_list_size(spacial_mode_ui_enum), &k_spacial );
+
+   static f32 rate = 60.0f;
+   ui_slider( panel, "Fixed timestep", 10, 200, &rate, "%.1f" );
+   vg.time_fixed_delta = 1.0f/rate;
+
+   ui_checkbox( panel, "Show BBX", &k_bbx );
+   ui_checkbox( panel, "Gyroscopic Term", &k_gyro );
+
+   ui_tabs( panel, panel, 
+            (const char *[]){ "collision", "racket" }, 2, &k_demo );
+
+   if( k_demo == 0 ) gui0( panel );
+   else if( k_demo == 1 ) gui1( panel );
+
+   ui_rect viewport = { 0,0, vg.window_x-300, vg.window_y };
+
+   if( ui_inside_rect( viewport, vg_ui.mouse ) && ui_clicking(UI_MOUSE_LEFT) ){
+      k_view_x += -((f32)vg.mouse_delta[0] / (f32)vg.window_x) * VG_TAUf,
+      k_view_y +=  ((f32)vg.mouse_delta[1] / (f32)vg.window_y) * VG_PIf;
+   }
 }
diff --git a/vg.h b/vg.h
index 5237792..119e9b1 100644
--- a/vg.h
+++ b/vg.h
@@ -167,6 +167,8 @@ struct vg{
           time_fixed_extrapolate,
           time_frame_delta;
 
+   f32 time_fixed_delta;
+
    u64 time_hp, time_hp_last, time_spinning;
 
    int fixed_iterations;
@@ -194,7 +196,7 @@ struct vg{
 
    vg_rand rand;
 }
-static vg = { .time_rate = 1.0 };
+static vg = { .time_rate = 1.0, .time_fixed_delta = VG_TIMESTEP_FIXED };
 const char *vg_get_basepath(void){
    return vg.base_path;
 }
@@ -265,8 +267,8 @@ static void vg_checkgl( const char *src_info );
 
 /* Diagnostic */
 static struct vg_profile vg_prof_update = {.name="update()"},
-                            vg_prof_render = {.name="render()"},
-                            vg_prof_swap   = {.name="swap"};
+                         vg_prof_render = {.name="render()"},
+                         vg_prof_swap   = {.name="swap"};
 
 static void vg_checkgl( const char *src_info )
 {
@@ -323,11 +325,12 @@ static void _vg_load_full( void *data ){
 #endif
    vg_loader_step( vg_profiler_init, NULL );
 
-   vg_async_call( async_internal_complete, NULL, 0 );
-
    /* client */
    vg_load();
 
+   vg_async_call( async_internal_complete, NULL, 0 );
+
+
    vg_success( "Client loaded in %fs\n", vg.time_real );
 }
 
@@ -432,10 +435,10 @@ static void _vg_gameloop_update(void)
    vg_lines.enabled = vg_lines.render;
    vg.time_fixed_accumulator += vg.time_delta;
 
-   while( vg.time_fixed_accumulator >= VG_TIMESTEP_FIXED ){
+   while( vg.time_fixed_accumulator >= vg.time_fixed_delta ){
       vg_fixed_update();
       vg_lines.enabled = 0;
-      vg.time_fixed_accumulator -= VG_TIMESTEP_FIXED;
+      vg.time_fixed_accumulator -= vg.time_fixed_delta;
 
       vg.fixed_iterations ++;
       if( vg.fixed_iterations == 8 ){
@@ -443,7 +446,7 @@ static void _vg_gameloop_update(void)
       }
    }
    vg_lines.enabled = vg_lines.render;
-   vg.time_fixed_extrapolate = vg.time_fixed_accumulator / VG_TIMESTEP_FIXED;
+   vg.time_fixed_extrapolate = vg.time_fixed_accumulator / vg.time_fixed_delta;
 
    vg.engine_stage = k_engine_stage_update;
    vg_post_update();
@@ -458,6 +461,8 @@ static void _vg_gameloop_render(void)
    vg.engine_stage = k_engine_stage_rendering;
    vg_render();
 
+   vg_profile_end( &vg_prof_render );
+
    /* ui */
    vg.engine_stage = k_engine_stage_ui;
    {
@@ -507,12 +512,10 @@ static void _vg_gameloop_render(void)
                vg.time_fixed_extrapolate, vg.time_frame_delta,
                vg.time_spinning );
 
-         ui_text( (ui_rect){258, 4+24+12+12,900,900},perf,1,0,k_ui_align_left);
+         ui_text( (ui_rect){258,4,900,900},perf,1,0,k_ui_align_left);
       }
       ui_postrender();
    }
-
-   vg_profile_end( &vg_prof_render );
 }
 
 static void aaaaaaaaaaaaaaaaa( ui_rect r ){
@@ -523,7 +526,7 @@ static void aaaaaaaaaaaaaaaaa( ui_rect r ){
          (struct vg_profile *[]){
             &vg_prof_update,&vg_prof_render,&vg_prof_swap}, 3,
          (1.0f/(f32)frame_target)*1500.0f, 
-         r, 0, 1
+         r,  1, 0
    );
 
    ui_fill( (ui_rect){ r[0], r[1] + (r[3]*2)/3, r[2], 1 }, ui_colour(k_ui_fg) );
diff --git a/vg_audio.h b/vg_audio.h
index 59ad057..0290e85 100644
--- a/vg_audio.h
+++ b/vg_audio.h
@@ -1356,7 +1356,7 @@ static void audio_debug_ui(
                                               &vg_prof_audio_mix,
                                               &vg_prof_audio_dsp}, 3, 
                      budget, (ui_rect){ 4, VG_PROFILE_SAMPLE_COUNT*2 + 8,
-                                        512, 0 }, 3, 0 );
+                                        512, 0 }, 0, 0 );
 
 
    char perf[128];
diff --git a/vg_bvh.h b/vg_bvh.h
new file mode 100644
index 0000000..85790c5
--- /dev/null
+++ b/vg_bvh.h
@@ -0,0 +1,415 @@
+#pragma once
+#include "vg_mem.h"
+#include "vg_m.h"
+#include "vg_lines.h"
+
+/*
+ * Usage:
+ *
+ * create a bh_system with functions filled out for expand, centroid, and swap.
+ * optionally include item_debug and cast_ray functions if needed, otherwise,
+ *   set them to null
+ *
+ * create a bh_tree struct with:
+ *   user: a pointer back the base of the data you are ordering
+ *   system: the system we created above which will deal with the data
+ *
+ * call bh_create( bh_tree *bh, u32 item_count )
+ * static int bh_ray( bh_tree *bh, u32 inode, v3f co, v3f dir, ray_hit *hit )
+ * static int bh_select( bh_tree *bh, boxf box, u32 *buffer, int len )
+ */
+
+typedef struct bh_node bh_node;
+typedef struct bh_tree bh_tree;
+typedef struct bh_system bh_system;
+
+typedef struct ray_hit ray_hit;
+struct ray_hit{
+   float dist;
+   u32 *tri;
+   v3f pos, normal;
+};
+
+struct bh_tree{
+   u32 node_count;
+
+   bh_system *system;
+   void *user;
+   u32 max_per_leaf;
+
+   struct bh_node{
+      boxf bbx;
+
+      /* if il is 0, this is a leaf */
+      int il, count;
+      union{ int ir, start; };
+   } 
+   nodes[];
+};
+
+struct bh_system{
+   void  (*expand_bound)( void *user, boxf bound, u32 item_index );
+   float (*item_centroid)( void *user, u32 item_index, int axis );
+   void  (*item_closest)( void *user, u32 item_index, v3f point, v3f closest );
+   void  (*item_swap)( void *user, u32 ia, u32 ib );
+
+   /*
+    * Optional:
+    *   item_debug   - draw this item quickly usually with lines
+    *   cast_ray     - shoot a ray against the object, if this is not set,
+    *                  raycasts will simply return the hit on the bvh node
+    */
+
+   void  (*item_debug)( void *user, u32 item_index );
+   int   (*cast_ray)( void *user, u32 index, v3f co, v3f dir, ray_hit *hit );
+};
+
+#define BVH_FIXED_MODE
+#ifdef BVH_FIXED_MODE
+static f32 shape_bvh_centroid( void *user, u32 item_index, int axis );
+static void shape_bvh_swap( void *user, u32 ia, u32 ib );
+static void shape_bvh_expand_bound( void *user, boxf bound, u32 item_index );
+#endif
+
+static void bh_update_bounds( bh_tree *bh, u32 inode ){
+   bh_node *node = &bh->nodes[ inode ];
+
+   box_init_inf( node->bbx );
+   for( u32 i=0; i<node->count; i++ ){
+      u32 idx = node->start+i;
+#ifdef BVH_FIXED_MODE
+      shape_bvh_expand_bound( bh->user, node->bbx, idx );
+#else
+      bh->system->expand_bound( bh->user, node->bbx, idx );
+#endif
+   }
+}
+
+static void bh_subdivide( bh_tree *bh, u32 inode ){
+   bh_node *node = &bh->nodes[ inode ];
+
+   if( node->count <= bh->max_per_leaf )
+      return;
+
+   v3f extent;
+   v3_sub( node->bbx[1], node->bbx[0], extent );
+
+   int axis = 0;
+   if( extent[1] > extent[0] ) axis = 1;
+   if( extent[2] > extent[axis] ) axis = 2;
+
+   float split = node->bbx[0][axis] + extent[axis]*0.5f;
+   float avg = 0.0;
+   for( u32 t=0; t<node->count; t++ ){
+      u32 idx = node->start+t;
+#ifdef BVH_FIXED_MODE
+      avg += shape_bvh_centroid( bh->user, idx, axis );
+#else
+      avg += bh->system->item_centroid( bh->user, idx, axis );
+#endif
+   }
+   avg /= (float)node->count;
+   split = avg;
+
+
+   i32 i = node->start,
+       j = i + node->count-1;
+   
+   while( i <= j ){
+#ifdef BVH_FIXED_MODE
+      f32 centroid = shape_bvh_centroid( bh->user, i, axis );
+#else
+      f32 centroid = bh->system->item_centroid( bh->user, i, axis );
+#endif
+
+      if( centroid < split )
+         i ++;
+      else{
+#ifdef BVH_FIXED_MODE
+         shape_bvh_swap( bh->user, i, j );
+#else
+         bh->system->item_swap( bh->user, i, j );
+#endif
+         j --;
+      }
+   }
+
+   u32 left_count = i - node->start;
+   if( left_count == 0 || left_count == node->count ) return;
+
+   u32 il = bh->node_count ++,
+       ir = bh->node_count ++;
+
+   bh_node *lnode = &bh->nodes[il],
+           *rnode = &bh->nodes[ir];
+
+   lnode->start = node->start;
+   lnode->count = left_count;
+   rnode->start = i;
+   rnode->count = node->count - left_count;
+
+   node->il = il;
+   node->ir = ir;
+   node->count = 0;
+
+   bh_update_bounds( bh, il );
+   bh_update_bounds( bh, ir );
+   bh_subdivide( bh, il );
+   bh_subdivide( bh, ir );
+}
+
+static void bh_rebuild( bh_tree *bh, u32 item_count ){
+   bh_node *root = &bh->nodes[0];
+   bh->node_count = 1;
+   
+   root->il = 0;
+   root->ir = 0;
+   root->count = item_count;
+   root->start = 0;
+
+   bh_update_bounds( bh, 0 );
+
+   if( item_count > 2 )
+      bh_subdivide( bh, 0 );
+}
+
+static bh_tree *bh_create( void *lin_alloc, bh_system *system, 
+                              void *user, u32 item_count, u32 max_per_leaf ){
+   assert( max_per_leaf > 0 );
+
+   u32 alloc_count = VG_MAX( 1, item_count );
+
+   u32 totsize = sizeof(bh_tree) + sizeof(bh_node)*(alloc_count*2-1);
+   bh_tree *bh = lin_alloc? vg_linear_alloc( lin_alloc, vg_align8(totsize) ):
+                            malloc( totsize );
+   bh->system = system;
+   bh->user = user;
+   bh->max_per_leaf = max_per_leaf;
+   bh_rebuild( bh, item_count );
+
+   if( lin_alloc ){
+      totsize = vg_align8(sizeof(bh_tree) + sizeof(bh_node) * bh->node_count);
+      bh = vg_linear_resize( lin_alloc, bh, totsize );
+   }
+
+   vg_success( "BVH done, size: %u/%u\n", bh->node_count, (alloc_count*2-1) );
+   return bh;
+}
+
+/*
+ * Draw items in this leaf node.
+ * *item_debug() must be set!
+ */
+static void bh_debug_leaf( bh_tree *bh, bh_node *node ){
+   vg_line_boxf( node->bbx, 0xff00ff00 );
+
+   if( bh->system->item_debug ){
+      for( u32 i=0; i<node->count; i++ ){
+         u32 idx = node->start+i;
+         bh->system->item_debug( bh->user, idx );
+      }
+   }
+}
+
+/*
+ * Trace the bh tree all the way down to the leaf nodes where pos is inside
+ */
+static void bh_debug_trace( bh_tree *bh, u32 inode, v3f pos, u32 colour ){
+   bh_node *node = &bh->nodes[ inode ];
+
+   if( (pos[0] >= node->bbx[0][0] && pos[0] <= node->bbx[1][0]) &&
+       (pos[2] >= node->bbx[0][2] && pos[2] <= node->bbx[1][2]) )
+   {
+      if( !node->count ){
+         vg_line_boxf( node->bbx, colour );
+
+         bh_debug_trace( bh, node->il, pos, colour );
+         bh_debug_trace( bh, node->ir, pos, colour );
+      }
+      else{
+         if( bh->system->item_debug )
+            bh_debug_leaf( bh, node );
+      }
+   }
+}
+
+typedef struct bh_iter bh_iter;
+struct bh_iter{
+   struct {
+      i32 id, depth;
+   }
+   stack[64];
+
+   enum bh_query_type{
+      k_bh_query_box,
+      k_bh_query_ray,
+      k_bh_query_range
+   }
+   query;
+
+   union{
+      struct{
+         boxf box;
+      }
+      box;
+
+      struct{
+         v3f co, inv_dir;
+         f32 max_dist;
+      }
+      ray;
+
+      struct {
+         v3f co;
+         f32 dist_sqr;
+      }
+      range;
+   };
+
+   i32 depth, i;
+};
+
+static void bh_iter_init_generic( i32 root, bh_iter *it ){
+   it->stack[0].id = root;
+   it->stack[0].depth = 0;
+   it->depth = 0;
+   it->i = 0;
+}
+
+static void bh_iter_init_box( i32 root, bh_iter *it, boxf box ){
+   bh_iter_init_generic( root, it );
+   it->query = k_bh_query_box;
+
+   box_copy( box, it->box.box );
+}
+
+static void bh_iter_init_ray( i32 root, bh_iter *it, v3f co, 
+                                 v3f dir, f32 max_dist ){
+   bh_iter_init_generic( root, it );
+   it->query = k_bh_query_ray;
+   
+   v3_div( (v3f){1.0f,1.0f,1.0f}, dir, it->ray.inv_dir );
+   v3_copy( co, it->ray.co );
+   it->ray.max_dist = max_dist;
+}
+
+static void bh_iter_init_range( i32 root, bh_iter *it, v3f co, f32 range ){
+   bh_iter_init_generic( root, it );
+   it->query = k_bh_query_range;
+
+   v3_copy( co, it->range.co );
+   it->range.dist_sqr = range*range;
+}
+
+/* NOTE: does not compute anything beyond the leaf level. element level tests
+ *       should be implemented by the users code.
+ *
+ *       this is like a 'broad phase only' deal.
+ */
+static i32 bh_next( bh_tree *bh, bh_iter *it, i32 *em ){
+   while( it->depth >= 0 ){
+      bh_node *inode = &bh->nodes[ it->stack[it->depth].id ];
+      
+      /* Only process overlapping nodes */
+      i32 q = 0;
+
+      if( it->i ) /* already checked */
+         q = 1;
+      else{
+         if( it->query == k_bh_query_box )
+            q = box_overlap( inode->bbx, it->box.box );
+         else if( it->query == k_bh_query_ray )
+            q = ray_aabb1( inode->bbx, it->ray.co, 
+                           it->ray.inv_dir, it->ray.max_dist );
+         else {
+            v3f nearest;
+            closest_point_aabb( it->range.co, inode->bbx, nearest );
+
+            if( v3_dist2( nearest, it->range.co ) <= it->range.dist_sqr )
+               q = 1;
+         }
+      }
+
+      if( !q ){
+         it->depth --;
+         continue;
+      }
+
+      if( inode->count ){
+         if( it->i < inode->count ){
+            *em = inode->start+it->i;
+            it->i ++;
+            return 1;
+         }
+         else{
+            it->depth --;
+            it->i = 0;
+         }
+      }
+      else{
+         if( it->depth+1 >= vg_list_size(it->stack) ){
+            vg_error( "Maximum stack reached!\n" );
+            return 0;
+         }
+
+         it->stack[it->depth  ].id = inode->il;
+         it->stack[it->depth+1].id = inode->ir;
+         it->depth ++;
+         it->i = 0;
+      }
+   }
+
+   return 0;
+}
+
+static int bh_closest_point( bh_tree *bh, v3f pos, 
+                                v3f closest, float max_dist )
+{
+   if( bh->node_count < 2 )
+      return -1;
+
+   max_dist = max_dist*max_dist;
+
+   int queue[ 128 ],
+       depth = 0,
+       best_item = -1;
+
+   queue[0] = 0;
+
+   while( depth >= 0 ){
+      bh_node *inode = &bh->nodes[ queue[depth] ];
+
+      v3f p1;
+      closest_point_aabb( pos, inode->bbx, p1 );
+
+      /* branch into node if its closer than current best */
+      float node_dist = v3_dist2( pos, p1 );
+      if( node_dist < max_dist ){
+         if( inode->count ){
+            for( int i=0; i<inode->count; i++ ){
+               v3f p2;
+               bh->system->item_closest( bh->user, inode->start+i, pos, p2 );
+
+               float item_dist = v3_dist2( pos, p2 );
+               if( item_dist < max_dist ){
+                  max_dist = item_dist;
+                  v3_copy( p2, closest );
+                  best_item = inode->start+i;
+               }
+            }
+
+            depth --;
+         }
+         else{
+            queue[depth] = inode->il;
+            queue[depth+1] = inode->ir;
+
+            depth ++;
+         }
+      }
+      else
+         depth --;
+   }
+
+   return best_item;
+}
diff --git a/vg_imgui.h b/vg_imgui.h
index b3dfa6a..4604327 100644
--- a/vg_imgui.h
+++ b/vg_imgui.h
@@ -203,7 +203,7 @@ struct{
 	GLuint tex_glyphs, vao, vbo, ebo, tex_bg;
    v2f bg_inverse_ratio;
 
-	ui_px mouse[2], mouse_click[2];
+	ui_px mouse[2], mouse_delta[2], mouse_click[2];
    u32 mouse_state[2];
    u32 ignore_input_frames;
    int wants_mouse;
@@ -887,6 +887,8 @@ static void ui_prerender(void){
    int x, y;
    vg_ui.mouse_state[1] = vg_ui.mouse_state[0];
    vg_ui.mouse_state[0] = SDL_GetMouseState( &x, &y );
+   vg_ui.mouse_delta[0] = x-vg_ui.mouse[0];
+   vg_ui.mouse_delta[1] = y-vg_ui.mouse[1];
    vg_ui.mouse[0] = x;
    vg_ui.mouse[1] = y;
 
diff --git a/vg_profiler.h b/vg_profiler.h
index d0106af..699e2a7 100644
--- a/vg_profiler.h
+++ b/vg_profiler.h
@@ -59,8 +59,8 @@ static void vg_profile_end( struct vg_profile *profile )
 }
 
 static void vg_profile_drawn( struct vg_profile **profiles, u32 count,
-                              float budget, ui_rect panel, u32 colour_offset,
-                              int dir )
+                              f64 budget, ui_rect panel,
+                              int dir, i32 normalize )
 {
    if( panel[2] == 0 )
       panel[2] = 256;
@@ -68,54 +68,41 @@ static void vg_profile_drawn( struct vg_profile **profiles, u32 count,
    if( panel[3] == 0 )
       panel[3] = VG_PROFILE_SAMPLE_COUNT * 2;
 
-   f32 sh = (f32)panel[3^dir] / (f32)VG_PROFILE_SAMPLE_COUNT,
+   f64 sh = (f32)panel[3^dir] / (f32)VG_PROFILE_SAMPLE_COUNT,
        sw = (f32)panel[2^dir];
 
    ui_fill( panel, 0xa0000000 );
 
    assert( count <= 8 );
    f64 avgs[8];
-   int ptrs[8];
-
-   for( int i=0; i<count; i++ ){
-#if 0
-      ptrs[i] = profiles[i]->buffer_current;
-#else
-      ptrs[i] = 0;
-#endif
-      avgs[i] = 0.0f;
+   u32 colours[8];
+   for( u32 i=0; i<count; i ++ ){
+      avgs[i] = 0.0;
+      colours[i] = ui_colour( k_ui_red + ((i*3)&0xe) );
    }
 
-   u32 colours[] = { 0xff0000ff, 0xff00ff00, 0xff00ffff, 0xffff0000,
-                     0xffff00ff, 0xffffff00 };
-
    f64 rate_mul = 1000.0 / (f64)SDL_GetPerformanceFrequency();
 
-   for( int i=0; i<VG_PROFILE_SAMPLE_COUNT-1; i++ ){
+   for( i32 i=0; i<VG_PROFILE_SAMPLE_COUNT; i++ ){
       f64 total = 0.0;
 
-      for( int j=0; j<count; j++ ){
-#if 0
-         ptrs[j] --;
-
-         if( ptrs[j] < 0 )
-            ptrs[j] = VG_PROFILE_SAMPLE_COUNT-1;
-#else
-         ptrs[j] ++;
-#endif
+      if( normalize ){
+         budget = 0.0;
+         for( u32 j=0; j<count; j++ )
+            budget += (f64)profiles[j]->samples[i] * rate_mul;
+      }
 
-         f64 sample  = (f64)profiles[j]->samples[ptrs[j]] * rate_mul,
-                px   = (total  / (budget)) * sw,
-                wx   = (sample / (budget)) * sw;
+      for( int j=0; j<count; j++ ){
+         f64 sample  = (f64)profiles[j]->samples[i] * rate_mul,
+                px   = (total  / budget) * sw,
+                wx   = (sample / budget) * sw;
 
          ui_rect block;
          block[0^dir] = panel[0^dir] + px;
          block[1^dir] = panel[1^dir] + (f32)i*sh;
-         block[2^dir] = wx;
+         block[2^dir] = VG_MAX( 1, wx-1 );
          block[3^dir] = ceilf(sh)-1;
-
-         u32 colour = colours[ (j+colour_offset) % vg_list_size(colours) ];
-         ui_fill( block, colour );
+         ui_fill( block, colours[j] );
 
          total += sample;
          avgs[j] += sample;
@@ -136,11 +123,10 @@ static void vg_profile_drawn( struct vg_profile **profiles, u32 count,
                         avgs[i] * (1.0f/(VG_PROFILE_SAMPLE_COUNT-1)),
                         profiles[i]->name );
 
-      ui_text( (ui_rect){ panel[0] + panel[2] + 4,
-                          panel[1] + i * 14, 0, 0 }, 
-                          infbuf,
-                          1,
-                          k_ui_align_left, 0 );
+      ui_text( (ui_rect){ panel[0] + 4,
+                          panel[1] + panel[3] + 4 + i*14, 
+                          panel[2]-8, 14 }, 
+                          infbuf, 1, k_ui_align_left, 0 );
    }
 }
 
diff --git a/vg_rigidbody.h b/vg_rigidbody.h
index c44aa5d..f6b9417 100644
--- a/vg_rigidbody.h
+++ b/vg_rigidbody.h
@@ -18,8 +18,8 @@
  */
 
 static const float 
-   k_rb_rate          = (1.0/VG_TIMESTEP_FIXED),
-   k_rb_delta         = (1.0/k_rb_rate),
+   //k_rb_rate          = (1.0/VG_TIMESTEP_FIXED),
+   //k_rb_delta         = (1.0/k_rb_rate),
    k_friction         = 0.4f,
    k_damp_linear      = 0.1f,               /* scale velocity 1/(1+x) */
    k_damp_angular     = 0.1f,                /* scale angular  1/(1+x) */
@@ -130,7 +130,7 @@ static void rb_update_matrices( rigidbody *rb ){
  */
 static void rb_extrapolate( rigidbody *rb, v3f co, v4f q ){
    float substep = vg.time_fixed_extrapolate;
-   v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
+   v3_muladds( rb->co, rb->v, vg.time_fixed_delta*substep, co );
 
    if( v3_length2( rb->w ) > 0.0f ){
       v4f rotation;
@@ -139,7 +139,7 @@ static void rb_extrapolate( rigidbody *rb, v3f co, v4f q ){
       
       float mag = v3_length( axis );
       v3_divs( axis, mag, axis );
-      q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
+      q_axis_angle( rotation, axis, mag*vg.time_fixed_delta*substep );
       q_mul( rotation, rb->q, q );
       q_normalize( q );
    }
@@ -157,11 +157,13 @@ static void rb_iter( rigidbody *rb ){
    }
 
    v3f gravity = { 0.0f, -9.8f, 0.0f };
-   v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
+   v3_muladds( rb->v, gravity, vg.time_fixed_delta, rb->v );
 
    /* intergrate velocity */
-   v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
+   v3_muladds( rb->co, rb->v, vg.time_fixed_delta, rb->co );
+#if 0
    v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
+#endif
 
    /* inegrate inertia */
    if( v3_length2( rb->w ) > 0.0f ){
@@ -171,12 +173,46 @@ static void rb_iter( rigidbody *rb ){
       
       float mag = v3_length( axis );
       v3_divs( axis, mag, axis );
-      q_axis_angle( rotation, axis, mag*k_rb_delta );
+      q_axis_angle( rotation, axis, mag*vg.time_fixed_delta );
       q_mul( rotation, rb->q, rb->q );
       q_normalize( rb->q );
    }
 }
 
+/* 
+ * based on: https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf,
+ * page 76.
+ */
+static void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ){
+   /* convert to body coordinates */
+   v3f w_local;
+   m3x3_mulv( rb->to_local, rb->w, w_local );
+
+   /* Residual vector */
+   v3f f, v0;
+   m3x3_mulv( I, w_local, v0 );
+   v3_cross( w_local, v0, f );
+   v3_muls( f, h, f );
+
+   /* Jacobian */
+   m3x3f iJ, J, skew_w_local, skew_v0, m0;
+   m3x3_skew_symetric( skew_w_local, w_local );
+   m3x3_mul( skew_w_local, I, m0 );
+
+   m3x3_skew_symetric( skew_v0, v0 );
+   m3x3_sub( m0, skew_v0, J );
+   m3x3_scalef( J, h );
+   m3x3_add( I, J, J );
+
+   /* Single Newton-Raphson update */
+   v3f w1;
+   m3x3_inv( J, iJ );
+   m3x3_mulv( iJ, f, w1 );
+   v3_sub( w_local, w1, rb->w );
+
+   m3x3_mulv( rb->to_world, rb->w, rb->w );
+}
+
 /*
  * Creates relative contact velocity vector
  */
@@ -215,10 +251,10 @@ static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
    float depth  = v3_dot( plane, ra->co ) - plane[3],
          lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
 
-   v3_muladds( ra->v, plane, lambda * k_rb_delta, ra->v );
+   v3_muladds( ra->v, plane, lambda * vg.time_fixed_delta, ra->v );
 
    if( depth < 0.0f )
-      v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
+      v3_muls( ra->v, 1.0f-(drag*vg.time_fixed_delta), ra->v );
 }
 
 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to 
diff --git a/vg_rigidbody_collision.h b/vg_rigidbody_collision.h
index 31bd1bb..c08baa5 100644
--- a/vg_rigidbody_collision.h
+++ b/vg_rigidbody_collision.h
@@ -1,6 +1,10 @@
 #pragma once
 #include "vg_rigidbody.h"
 
+#ifndef VG_MAX_CONTACTS
+#define VG_MAX_CONTACTS 256
+#endif
+
 typedef struct rb_ct rb_ct;
 static struct rb_ct{
    rigidbody *rba, *rbb;
@@ -13,7 +17,7 @@ static struct rb_ct{
 
    enum contact_type type;
 }
-rb_contact_buffer[256];
+rb_contact_buffer[VG_MAX_CONTACTS];
 static int rb_contact_count = 0;
 
 /*
@@ -117,29 +121,28 @@ static int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c,
     * Debugging
     */
 
+#if 0
    if( count == 2 )
       vg_line( buf[0].co, buf[1].co, 0xff0000ff );
+#endif
 
    return count;
 }
 
-#if 0
-static int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
-   rigidbody *rba = &obja->rb, *rbb = &objb->rb;
-   f32 h = obja->inf.capsule.h,
-        ra = obja->inf.capsule.r,
-        rb = objb->inf.sphere.r;
+static int rb_capsule__sphere( m4x3f mtxA, rb_capsule *ca,
+                               v3f coB, f32 rb, rb_ct *buf ){
+   f32 ha = ca->h,
+       ra = ca->r,
+       r  = ra + rb;
 
    v3f p0, p1;
-   v3_muladds( rba->co, rba->to_world[1], -h*0.5f+ra, p0 );
-   v3_muladds( rba->co, rba->to_world[1],  h*0.5f-ra, p1 );
+   v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 );
+   v3_muladds( mtxA[3], mtxA[1],  ha*0.5f-ra, p1 );
 
    v3f c, delta;
-   closest_point_segment( p0, p1, rbb->co, c );
-   v3_sub( c, rbb->co, delta );
-
-   f32 d2 = v3_length2(delta),
-          r = ra + rb;
+   closest_point_segment( p0, p1, coB, c );
+   v3_sub( c, coB, delta );
+   f32 d2 = v3_length2(delta);
 
    if( d2 < r*r ){
       f32 d = sqrtf(d2);
@@ -150,20 +153,14 @@ static int rb_capsule_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
 
       v3f p0, p1;
       v3_muladds( c, ct->n, -ra, p0 );
-      v3_muladds( rbb->co, ct->n, rb,  p1 );
+      v3_muladds( coB, ct->n, rb,  p1 );
       v3_add( p0, p1, ct->co );
       v3_muls( ct->co, 0.5f, ct->co );
-
-      ct->rba = rba;
-      ct->rbb = rbb;
       ct->type = k_contact_type_default;
-
       return 1;
    }
-
-   return 0;
+   else return 0;
 }
-#endif
 
 static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
                                 m4x3f mtxB, rb_capsule *cb, rb_ct *buf ){
@@ -200,71 +197,170 @@ static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
    return rb_capsule__manifold_done( mtxA, ca, &manifold, buf );
 }
 
-#if 0
-static int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){
-   v3f co, delta;
-   rigidbody *rba = &obja->rb, *rbb = &objb->rb;
+/*
+ * Generates up to two contacts; optimised for the most stable manifold
+ */
+static int rb_capsule__box( m4x3f mtxA, rb_capsule *ca,
+                            m4x3f mtxB, m4x3f mtxB_inverse, boxf box, 
+                            rb_ct *buf ){
+   f32 h = ca->h, r = ca->r;
 
-   closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
-   v3_sub( rba->co, co, delta );
+   /*
+    * 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( box[0], box[1], centroid );
+   v3_muls( centroid, 0.5f, centroid );
 
-   f32 d2 = v3_length2(delta),
-          r = obja->inf.sphere.radius;
+   boxf bbx;
+   v3_sub( box[0], centroid, bbx[0] );
+   v3_sub( box[1], centroid, bbx[1] );
+   
+   v3f pc, p0w, p1w, p0, p1;
+   v3_muladds( mtxA[3], mtxA[1], -h*0.5f+r, p0w );
+   v3_muladds( mtxA[3], mtxA[1],  h*0.5f-r, p1w );
+
+   m4x3_mulv( mtxB_inverse, p0w, p0 );
+   m4x3_mulv( mtxB_inverse, 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])) )
+   {
+      f32 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]) )
+   {
+      f32 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
+   {
+      f32 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] );
+   }
 
-   if( d2 <= r*r ){
+   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( i32 i=0; i<4; i++ ){
+      i32 i0 = i,
+          i1 = (i+1)%4;
+
+      v3f ca, cb;
+      f32 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( mtxB, manifold.d0, manifold.d0 );
+   m3x3_mulv( mtxB, manifold.d1, manifold.d1 );
+   return rb_capsule__manifold_done( mtxA, ca, &manifold, buf );
+}
+
+static int rb_sphere__box( v3f coA, f32 ra, 
+                           m4x3f mtxB, m4x3f mtxB_inverse, boxf box,
+                           rb_ct *buf ){
+   v3f co, delta;
+   closest_point_obb( coA, box, mtxB, mtxB_inverse, co );
+   v3_sub( coA, co, delta );
+
+   f32 d2 = v3_length2(delta);
+
+   if( d2 <= ra*ra ){
       f32 d;
 
       rb_ct *ct = buf;
       if( d2 <= 0.0001f ){
-         v3_sub( rba->co, rbb->co, delta );
+         v3f e, coB;
+         v3_sub( box[1], box[0], e );
+         v3_muls( e, 0.5f, e );
+         v3_add( box[0], e, coB );
+         v3_sub( coA, coB, 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.
           */
-         f32 lx = v3_dot( rbb->to_world[0], delta ),
-               ly = v3_dot( rbb->to_world[1], delta ),
-               lz = v3_dot( rbb->to_world[2], 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->to_world[0], vg_signf(lx), ct->n );
-         else if( py < pz )
-            v3_muls( rbb->to_world[1], vg_signf(ly), ct->n );
-         else
-            v3_muls( rbb->to_world[2], vg_signf(lz), ct->n );
-
-         v3_muladds( rba->co, ct->n, -r, ct->co );
-         ct->p = r;
+         f32 lx = v3_dot( mtxB[0], delta ),
+             ly = v3_dot( mtxB[1], delta ),
+             lz = v3_dot( mtxB[2], delta ),
+             px = e[0] - fabsf(lx),
+             py = e[1] - fabsf(ly),
+             pz = e[2] - fabsf(lz);
+
+         if( px < py && px < pz ) v3_muls( mtxB[0], vg_signf(lx), ct->n );
+         else if( py < pz )       v3_muls( mtxB[1], vg_signf(ly), ct->n );
+         else                     v3_muls( mtxB[2], vg_signf(lz), ct->n );
+
+         v3_muladds( coA, ct->n, -ra, ct->co );
+         ct->p = ra;
       }
       else{
          d = sqrtf(d2);
          v3_muls( delta, 1.0f/d, ct->n );
-         ct->p = r-d;
+         ct->p = ra-d;
          v3_copy( co, ct->co );
       }
 
-      ct->rba = rba;
-      ct->rbb = rbb;
       ct->type = k_contact_type_default;
       return 1;
    }
-
-   return 0;
+   else return 0;
 }
-#endif
 
-#if 0
-static int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
-   rigidbody *rba = &obja->rb, *rbb = &objb->rb;
+static int rb_sphere__sphere( v3f coA, f32 ra, 
+                              v3f coB, f32 rb, rb_ct *buf ){
    v3f delta;
-   v3_sub( rba->co, rbb->co, delta );
+   v3_sub( coA, coB, delta );
 
    f32 d2 = v3_length2(delta),
-          r = obja->inf.sphere.radius + objb->inf.sphere.radius;
+        r = ra+rb;
 
    if( d2 < r*r ){
       f32 d = sqrtf(d2);
@@ -273,20 +369,16 @@ static int rb_sphere_sphere( rb_object *obja, rb_object *objb, rb_ct *buf ){
       v3_muls( delta, 1.0f/d, ct->n );
 
       v3f p0, p1;
-      v3_muladds( rba->co, ct->n,-obja->inf.sphere.radius, p0 );
-      v3_muladds( rbb->co, ct->n, objb->inf.sphere.radius, p1 );
+      v3_muladds( coA, ct->n,-ra, p0 );
+      v3_muladds( coB, ct->n, rb, p1 );
       v3_add( p0, p1, ct->co );
       v3_muls( ct->co, 0.5f, ct->co );
       ct->type = k_contact_type_default;
       ct->p = r-d;
-      ct->rba = rba;
-      ct->rbb = rbb;
       return 1;
    }
-
-   return 0;
+   else return 0;
 }
-#endif
 
 static int rb_sphere__triangle( m4x3f mtxA, f32 r,
                                 v3f tri[3], rb_ct *buf ){
@@ -669,8 +761,8 @@ static rb_ct *rb_global_ct(void){
    return rb_contact_buffer + rb_contact_count;
 }
 
-static void rb_prepare_contact( rb_ct *ct, float timestep ){
-   ct->bias = -k_phys_baumgarte * (timestep*3600.0f) 
+static void rb_prepare_contact( rb_ct *ct ){
+   ct->bias = -k_phys_baumgarte * (vg.time_fixed_delta*3600.0f) 
                * vg_minf( 0.0f, -ct->p+k_penetration_slop );
    
    v3_tangent_basis( ct->n, ct->t[0], ct->t[1] );
@@ -705,7 +797,7 @@ static void rb_depenetrate( rb_ct *manifold, int len, v3f dt ){
 static void rb_presolve_contacts( rb_ct *buffer, int len ){
    for( int i=0; i<len; i++ ){
       rb_ct *ct = &buffer[i];
-      rb_prepare_contact( ct, k_rb_delta );
+      rb_prepare_contact( ct );
 
       v3f ra, rb, raCn, rbCn, raCt, rbCt;
       v3_sub( ct->co, ct->rba->co, ra );
@@ -735,8 +827,6 @@ static void rb_presolve_contacts( rb_ct *buffer, int len ){
          ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
          ct->tangent_mass[j]  = 1.0f/ct->tangent_mass[j];
       }
-
-      rb_debug_contact( ct );
    }
 }
 
diff --git a/vg_rigidbody_view.h b/vg_rigidbody_view.h
index 578060e..1064fed 100644
--- a/vg_rigidbody_view.h
+++ b/vg_rigidbody_view.h
@@ -302,8 +302,8 @@ static void vg_rb_view_capsule( m4x3f mdl, f32 r, f32 h, v4f colour ){
    m4x3_identity( mmdl1 );
    m3x3_scalef( mmdl0, r );
    m3x3_scalef( mmdl1, r );
-   mmdl0[3][1] = -h-r;
-   mmdl1[3][1] =  h+r;
+   mmdl0[3][1] = -h*0.5f+r;
+   mmdl1[3][1] =  h*0.5f-r;
    m4x3_mul( mdl, mmdl0, mmdl0 );
    m4x3_mul( mdl, mmdl1, mmdl1 );
 
diff --git a/vg_shader.h b/vg_shader.h
index 6537ec9..211086a 100644
--- a/vg_shader.h
+++ b/vg_shader.h
@@ -35,7 +35,7 @@ struct
       void (*link)(void);
       int compiled;
    }
-   * shaders[32];
+   * shaders[48];
    u32 count;
 }
 static vg_shaders;
-- 
2.25.1