From: hgn <hgodden00@gmail.com>
Date: Tue, 19 Jul 2022 13:51:16 +0000 (+0100)
Subject: its ot'
X-Git-Url: https://skaterift.com/git/?a=commitdiff_plain;h=98ecdd9f0377f1563566fe4d3d3b8185a055ec4d;p=carveJwlIkooP6JGAAIwe30JlM.git

its ot'
---

diff --git a/character.h b/character.h
index 39ee472..94b181e 100644
--- a/character.h
+++ b/character.h
@@ -556,8 +556,7 @@ static void character_testpose( struct character *ch, float t )
    m4x3_identity( ch->matrices[k_chpart_wf] );
 }
 
-static float *player_cam_pos(void);
-static void character_draw( struct character *ch, float temp )
+static void character_draw( struct character *ch, float temp, m4x3f camera )
 {
    shader_character_use();
    shader_character_uPv( vg_pv );
@@ -565,7 +564,7 @@ static void character_draw( struct character *ch, float temp )
    vg_tex2d_bind( &tex_pallet, 0 );
    shader_character_uTexMain( 0 );
    shader_character_uOpacity( temp );
-   shader_character_uCamera( player_cam_pos() );
+   shader_character_uCamera( camera[3] );
    shader_link_standard_ub( _shader_character.id, 2 );
    
    glEnable( GL_CULL_FACE );
@@ -825,8 +824,7 @@ static void character_ragdoll_copypose( struct character *ch, v3f v )
       m4x3_mulv( ch->matrices[i], rb->delta, rb->co );
       m3x3_q( ch->matrices[i], rb->q );
       v3_copy( v, rb->v );
-      v3_zero( rb->I );
-      rb->manifold_count = 0; /* ? */
+      v3_zero( rb->w );
 
       rb_update_transform( rb );
    }
@@ -884,11 +882,14 @@ static void character_debug_ragdoll( struct character *ch )
 
 static void character_ragdoll_iter( struct character *ch )
 {
+   rb_solver_reset();
+
    for( int i=0; i<PART_COUNT; i++ )
    {
-      rb_manifold_reset( &ch->ragdoll[i] );
       rb_build_manifold_terrain( &ch->ragdoll[i] );
 
+      /* TODO: Cars */
+#if 0
       u32 colliders[16];
       int len = bh_select( &world.bhcubes, ch->ragdoll[i].bbx_world, 
             colliders, 16 );
@@ -896,6 +897,7 @@ static void character_ragdoll_iter( struct character *ch )
       for( int j=0; j<len; j++ )
          rb_build_manifold_rb_static( &ch->ragdoll[i], 
                &world.temp_rbs[colliders[j]] );
+#endif
    }
 
    v3f rv;
@@ -904,14 +906,14 @@ static void character_ragdoll_iter( struct character *ch )
    for( int i=0; i<2; i++ )
       if( ch->shoes[i] )
          shoe_vel[i] = v3_length( ch->ragdoll[i].v );
-
-   for( int i=0; i<20; i++ )
+   
+   /* This used to be 20 iterations */
+   for( int i=0; i<5; i++ )
    {
       float const k_springfactor = 1.0f/20.0f;
-
-      for( int j=0; j<PART_COUNT; j++ )
-         rb_constraint_manifold( &ch->ragdoll[j] );
-
+      
+      rb_solve_contacts();
+      
       for( int j=0; j<vg_list_size(rd_joints); j++ )
       {
          struct rd_joint *joint = &rd_joints[j];
@@ -959,7 +961,7 @@ static void character_ragdoll_iter( struct character *ch )
             v3_copy( src->co, dst->co );
             v3_copy( src->v, dst->v );
             v3_copy( src->q, dst->q );
-            v3_copy( src->I, dst->I );
+            v3_copy( src->w, dst->w );
          }
       }
    }
diff --git a/main.c b/main.c
index ca2b997..0219a66 100644
--- a/main.c
+++ b/main.c
@@ -121,6 +121,14 @@ void vg_start(void)
       .persistent = 1
    });
 
+   vg_convar_push( (struct vg_convar){
+      .name = "dt",
+      .data = &ktimestep,
+      .data_type = k_convar_dtype_f32,
+      .opt_f32 = { .clamp = 0 },
+      .persistent = 0
+   });
+
    vg_convar_push( (struct vg_convar){
       .name = "debugcam",
       .data = &sv_debugcam,
@@ -146,13 +154,13 @@ void vg_start(void)
    
    init_other();
 
-   character_load( &player.mdl, "ch_mike" );
+   character_load( &player.mdl, "ch_default" );
    character_init_ragdoll( &player.mdl );
 
    world_load();
 
    reset_player( 1, (const char *[]){ "start" } );
-   player_transform_update();
+   rb_init( &player.rb );
 }
 
 void vg_free(void)
@@ -165,6 +173,12 @@ void vg_update(void)
    player_update();
    world_update();
    //traffic_visualize( world.traffic, world.traffic_count );
+   //
+   /* TEMP */
+   if( glfwGetKey( vg_window, GLFW_KEY_J ))
+   {
+      v3_copy( player.camera_pos, world.mr_ball.co );
+   }
 }
 
 static void vg_framebuffer_resize( int w, int h )
@@ -183,7 +197,7 @@ void vg_render(void)
    glClearColor( 0.11f, 0.35f, 0.37f, 1.0f );
    glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT );
 
-   float speed = freecam? 0.0f: v3_length( player.v );
+   float speed = freecam? 0.0f: v3_length( player.rb.v );
    v3f shake = { vg_randf()-0.5f, vg_randf()-0.5f, vg_randf()-0.5f };
    v3_muls( shake, speed*0.01f, shake );
 
diff --git a/models/mp_dev.mdl b/models/mp_dev.mdl
index 84d6cc3..d2a152d 100644
Binary files a/models/mp_dev.mdl and b/models/mp_dev.mdl differ
diff --git a/models/rs_chicken.mdl b/models/rs_chicken.mdl
new file mode 100644
index 0000000..fc0e60c
Binary files /dev/null and b/models/rs_chicken.mdl differ
diff --git a/player.h b/player.h
index 0e38d85..9540d9e 100644
--- a/player.h
+++ b/player.h
@@ -12,8 +12,8 @@
 
 static float 
    k_walkspeed             = 2.0f,
-   k_board_radius          = 0.25f,
-   k_board_length          = 0.65f,
+   k_board_radius          = 0.3f,
+   k_board_length          = 0.55f,
    k_board_allowance       = 0.04f,
    k_friction_lat          = 8.68f,
    k_friction_resistance   = 0.02f,
@@ -31,18 +31,14 @@ static int walk_grid_iterations = 1;
 static struct gplayer
 {
    /* Physics */
-   v3f co, v, w, a, v_last, m, bob, vl;
+   rigidbody rb;
 
-   /* Utility */
-   v3f up, right, forward;
+   v3f a, v_last, m, bob, vl;
 
-   v4f rot;
+   /* Utility */
    float vswitch, slip, slip_last,
          reverse;
 
-   int rf, rb;
-   v3f ctf, ctb;
-
    float iY;   /* Yaw inertia */
    int in_air, is_dead, on_board;
 
@@ -55,8 +51,6 @@ static struct gplayer
    u32 land_target_colours[22];
    int land_log_count;
    m3x3f vr,vr_pstep;
-
-   m4x3f to_world, to_local;
    
    struct character mdl;
 
@@ -72,25 +66,15 @@ static struct gplayer
 }
 player = 
 {
-   .on_board = 1
+   .on_board = 1,
+
+   .rb = { .type = k_rb_shape_capsule }
 };
 
 /* 
  * Player API
  */
 
-static void player_transform_update(void)
-{
-   q_normalize( player.rot );
-   q_m3x3( player.rot, player.to_world );
-   v3_copy( player.co, player.to_world[3] );
-
-   m4x3_invert_affine( player.to_world, player.to_local );
-
-   m3x3_mulv( player.to_world, (v3f){1.0f,0.0f, 0.0f}, player.right );
-   m3x3_mulv( player.to_world, (v3f){0.0f,1.0f, 0.0f}, player.up );
-   m3x3_mulv( player.to_world, (v3f){0.0f,0.0f,-1.0f}, player.forward );
-}
 
 /*
  * Free camera movement
@@ -182,7 +166,7 @@ static void player_start_air(void)
    float k_bias = 0.97f;
 
    v3f axis;
-   v3_cross( player.up, player.v, axis );
+   v3_cross( player.rb.up, player.rb.v, axis );
    v3_normalize( axis );
    player.land_log_count = 0;
    
@@ -193,8 +177,8 @@ static void player_start_air(void)
       float vmod = ((float)m / 15.0f)*0.09f;
 
       v3f pco, pco1, pv;
-      v3_copy( player.co, pco );
-      v3_muls( player.v, k_bias, pv );
+      v3_copy( player.rb.co, pco );
+      v3_muls( player.rb.v, k_bias, pv );
 
       /* 
        * Try different 'rotations' of the velocity to find the best possible
@@ -285,7 +269,7 @@ static void player_physics_control(void)
     */
 
    v3f vel;
-   m3x3_mulv( player.to_local, player.v, vel );
+   m3x3_mulv( player.rb.to_local, player.rb.v, vel );
    float slip = 0.0f;
    
    if( fabsf(vel[2]) > 0.01f )
@@ -319,7 +303,7 @@ static void player_physics_control(void)
       vel[2] -= new_vel * player.reverse;
    }
    
-   m3x3_mulv( player.to_world, vel, player.v );
+   m3x3_mulv( player.rb.to_world, vel, player.rb.v );
    
    float steer = vg_get_axis( "horizontal" );
    player.iY -= vg_signf(steer)*powf(steer,2.0f) * k_steer_ground * ktimestep;
@@ -330,7 +314,7 @@ static void player_physics_control(void)
 
 static void player_physics_control_air(void)
 {
-   m3x3_mulv( player.vr, player.v, player.v );
+   m3x3_mulv( player.vr, player.rb.v, player.rb.v );
    draw_cross( player.land_target, 0xff0000ff, 0.25f );
 
    ray_hit hit;
@@ -341,8 +325,8 @@ static void player_physics_control_air(void)
    float pstep = ktimestep*10.0f;
 
    v3f pco, pco1, pv;
-   v3_copy( player.co, pco );
-   v3_copy( player.v, pv );
+   v3_copy( player.rb.co, pco );
+   v3_copy( player.rb.v, pv );
    
    float time_to_impact = 0.0f;
    float limiter = 1.0f;
@@ -366,12 +350,9 @@ static void player_physics_control_air(void)
       float orig_dist = contact.dist;
       if( ray_world( pco1, vdir, &contact ))
       {
-         v3f localup;
-         m3x3_mulv( player.to_world, (v3f){0.0f,1.0f,0.0f}, localup );
-
-         float angle = v3_dot( localup, contact.normal );
+         float angle = v3_dot( player.rb.up, contact.normal );
          v3f axis; 
-         v3_cross( localup, contact.normal, axis );
+         v3_cross( player.rb.up, contact.normal, axis );
 
          time_to_impact += (contact.dist/orig_dist)*pstep;
          limiter = vg_minf( 5.0f, time_to_impact )/5.0f;
@@ -383,7 +364,7 @@ static void player_physics_control_air(void)
          {
             v4f correction;
             q_axis_angle( correction, axis, acosf(angle)*0.05f*(1.0f-limiter) );
-            q_mul( correction, player.rot, player.rot );
+            q_mul( correction, player.rb.q, player.rb.q );
          }
 
          draw_cross( contact.pos, 0xffff0000, 0.25f );
@@ -401,12 +382,8 @@ static void player_physics_control_air(void)
       siX = vg_lerpf( siX, iX, k_steer_air_lerp );
       
       v4f rotate;
-      v3f vside;
-      
-      m3x3_mulv( player.to_world, (v3f){1.0f,0.0f,0.0f}, vside );
-
-      q_axis_angle( rotate, vside, siX );
-      q_mul( rotate, player.rot, player.rot );
+      q_axis_angle( rotate, player.rb.right, siX );
+      q_mul( rotate, player.rb.q, player.rb.q );
    }
    
    v2f target = {0.0f,0.0f};
@@ -417,11 +394,20 @@ static void player_physics_control_air(void)
 
 static void player_physics(void)
 {
+   /*
+    * Player physics uses a customized routine seperate from the main
+    * rigidbody implementation. It requires some non-standard impulse
+    * responses being applied for example limiting the effect on certain axises
+    * ( especially for angular velocity )
+    *
+    * The capsule collider is also at a different angle to the players roation.
+    */
+
    m4x3f mboard;
-   v3_copy( player.to_world[0], mboard[0] );
-   v3_copy( player.to_world[2], mboard[1] );
-   v3_copy( player.to_world[1], mboard[2] );
-   m4x3_mulv( player.to_world, (v3f){ 0.0f, 0.3f, 0.0f }, mboard[3] );
+   v3_copy( player.rb.to_world[0], mboard[0] );
+   v3_copy( player.rb.to_world[2], mboard[1] );
+   v3_copy( player.rb.to_world[1], mboard[2] );
+   m4x3_mulv( player.rb.to_world, (v3f){ 0.0f, 0.3f, 0.0f }, mboard[3] );
 
    debug_capsule( mboard, k_board_length*2.0f, k_board_radius, 0xff0000ff );
 
@@ -471,9 +457,10 @@ static void player_physics(void)
             vg_line( poles[j], p1, 0xffffffff );
 
             struct contact *ct = &manifold[manifold_count ++];
-            v3_sub( co, player.co, ct->delta );
+            v3_sub( co, player.rb.co, ct->delta );
             v3_copy( co, ct->co );
             v3_copy( norm, ct->n );
+
             ct->bias = -0.2f*k_rb_rate*vg_minf(0.0f,-p+k_board_allowance);
             ct->norm_impulse = 0.0f;
 
@@ -490,7 +477,7 @@ static void player_physics(void)
    {
       v3_normalize( surface_avg );
 
-      if( v3_dot( player.v, surface_avg ) > 0.5f )
+      if( v3_dot( player.rb.v, surface_avg ) > 0.5f )
       {
          player_start_air();
       }
@@ -498,15 +485,15 @@ static void player_physics(void)
          player.in_air = 0;
    }
 
-   for( int j=0; j<10; j++ )
+   for( int j=0; j<5; j++ )
    {
       for( int i=0; i<manifold_count; i++ )
       {
          struct contact *ct = &manifold[i];
          
          v3f dv;
-         v3_cross( player.w, ct->delta, dv );
-         v3_add( player.v, dv, dv );
+         v3_cross( player.rb.w, ct->delta, dv );
+         v3_add( player.rb.v, dv, dv );
 
          float vn = -v3_dot( dv, ct->n );
          vn += ct->bias;
@@ -518,8 +505,16 @@ static void player_physics(void)
          v3f impulse;
 
          v3_muls( ct->n, vn, impulse );
-         v3_add( impulse, player.v, player.v );
 
+         if( fabsf(v3_dot( impulse, player.rb.forward )) > 10.0f ||
+             fabsf(v3_dot( impulse, player.rb.up )) > 50.0f )
+         {
+            player.is_dead = 1;
+            character_ragdoll_copypose( &player.mdl, player.rb.v );
+            return;
+         }
+
+         v3_add( impulse, player.rb.v, player.rb.v );
          v3_cross( ct->delta, impulse, impulse );
 
          /*
@@ -530,31 +525,32 @@ static void player_physics(void)
           * components.
           */
          
-         float wy = v3_dot( player.up, impulse ),
-               wx = v3_dot( player.right, impulse );
+         float wy = v3_dot( player.rb.up, impulse ),
+               wx = v3_dot( player.rb.right, impulse )*1.5f;
 
-         v3_muladds( player.w, player.up, wy, player.w );
-         v3_muladds( player.w, player.right, wx, player.w );
+         v3_muladds( player.rb.w, player.rb.up, wy, player.rb.w );
+         v3_muladds( player.rb.w, player.rb.right, wx, player.rb.w );
       }
    }
 
    if( !player.in_air )
    {
       v3f axis;
-      float angle = v3_dot( player.up, surface_avg );
-      v3_cross( player.up, surface_avg, axis );
+      float angle = v3_dot( player.rb.up, surface_avg );
+      v3_cross( player.rb.up, surface_avg, axis );
 
-      float cz = v3_dot( player.forward, axis );
-      v3_muls( player.forward, cz, axis );
+      float cz = v3_dot( player.rb.forward, axis );
+      v3_muls( player.rb.forward, cz, axis );
 
       if( angle < 0.999f )
       {
          v4f correction;
          q_axis_angle( correction, axis, acosf(angle)*0.3f );
-         q_mul( correction, player.rot, player.rot );
+         q_mul( correction, player.rb.q, player.rb.q );
       }
 
-      v3_muladds( player.v, player.up, -k_downforce*ktimestep, player.v );
+      v3_muladds( player.rb.v, player.rb.up, 
+            -k_downforce*ktimestep, player.rb.v );
       player_physics_control();
    }
    else
@@ -572,23 +568,23 @@ static void player_do_motion(void)
    
    /* Integrate velocity */
    v3f prevco;
-   v3_copy( player.co, prevco );
+   v3_copy( player.rb.co, prevco );
    
-   apply_gravity( player.v, ktimestep );
-   v3_muladds( player.co, player.v, ktimestep, player.co );
+   apply_gravity( player.rb.v, ktimestep );
+   v3_muladds( player.rb.co, player.rb.v, ktimestep, player.rb.co );
 
    /* Real angular velocity integration */
-   v3_lerp( player.w, (v3f){0.0f,0.0f,0.0f}, 0.125f, player.w );
-   if( v3_length2( player.w ) > 0.0f )
+   v3_lerp( player.rb.w, (v3f){0.0f,0.0f,0.0f}, 0.125f, player.rb.w );
+   if( v3_length2( player.rb.w ) > 0.0f )
    {
       v4f rotation;
       v3f axis;
-      v3_copy( player.w, axis );
+      v3_copy( player.rb.w, axis );
       
       float mag = v3_length( axis );
       v3_divs( axis, mag, axis );
       q_axis_angle( rotation, axis, mag*k_rb_delta );
-      q_mul( rotation, player.rot, player.rot );
+      q_mul( rotation, player.rb.q, player.rb.q );
    }
 
    /* Faux angular velocity */
@@ -598,8 +594,8 @@ static void player_do_motion(void)
    float lerpq = player.in_air? 0.04f: 0.3f;
    siY = vg_lerpf( siY, player.iY, lerpq );
 
-   q_axis_angle( rotate, player.up, siY );
-   q_mul( rotate, player.rot, player.rot );
+   q_axis_angle( rotate, player.rb.up, siY );
+   q_mul( rotate, player.rb.q, player.rb.q );
    player.iY = 0.0f;
 
    /* 
@@ -609,10 +605,10 @@ static void player_do_motion(void)
    {
       teleport_gate *gate = &world.gates[i];
 
-      if( gate_intersect( gate, player.co, prevco ) )
+      if( gate_intersect( gate, player.rb.co, prevco ) )
       {
-         m4x3_mulv( gate->transport, player.co, player.co );
-         m3x3_mulv( gate->transport, player.v, player.v );
+         m4x3_mulv( gate->transport, player.rb.co, player.rb.co );
+         m3x3_mulv( gate->transport, player.rb.v, player.rb.v );
          m3x3_mulv( gate->transport, player.vl, player.vl );
          m3x3_mulv( gate->transport, player.v_last, player.v_last );
          m3x3_mulv( gate->transport, player.m, player.m );
@@ -620,13 +616,13 @@ static void player_do_motion(void)
 
          v4f transport_rotation;
          m3x3_q( gate->transport, transport_rotation );
-         q_mul( transport_rotation, player.rot, player.rot );
+         q_mul( transport_rotation, player.rb.q, player.rb.q );
 
          break;
       }
    }
-
-   player_transform_update();
+   
+   rb_update_transform( &player.rb );
 }
 
 /*
@@ -672,7 +668,7 @@ struct walkgrid
 
 static int player_walkgrid_tri_walkable( u32 tri[3] )
 {
-   return tri[0] < world.sm_surface.vertex_count;
+   return tri[0] > world.sm_geo_std_oob.vertex_count;
 }
 
 /*
@@ -1210,7 +1206,7 @@ static void player_walkgrid_stand_cell(struct walkgrid *wg)
 
    v3f world;
    world[0] = wg->region[0][0]+((float)wg->cell_id[0]+wg->pos[0])*k_gridscale;
-   world[1] = player.co[1];
+   world[1] = player.rb.co[1];
    world[2] = wg->region[0][2]+((float)wg->cell_id[1]+wg->pos[1])*k_gridscale;
 
    struct grid_sample *corners[4];
@@ -1346,7 +1342,7 @@ static void player_walkgrid_stand_cell(struct walkgrid *wg)
       }
    }
 
-   v3_copy( world, player.co );
+   v3_copy( world, player.rb.co );
 }
 
 static void player_walkgrid_getsurface(void)
@@ -1359,7 +1355,7 @@ static void player_walkgrid_getsurface(void)
    static struct walkgrid wg;
 
    v3f cell;
-   v3_copy( player.co, cell );
+   v3_copy( player.rb.co, cell );
    player_walkgrid_floor( cell );
 
    v3_muladds( cell, (v3f){-1.0f,-1.0f,-1.0f}, k_region_size, wg.region[0] );
@@ -1401,8 +1397,8 @@ static void player_walkgrid_getsurface(void)
 
    v2f region_pos = 
    {
-      (player.co[0] - wg.region[0][0]) * (1.0f/k_gridscale),
-      (player.co[2] - wg.region[0][2]) * (1.0f/k_gridscale)
+      (player.rb.co[0] - wg.region[0][0]) * (1.0f/k_gridscale),
+      (player.rb.co[2] - wg.region[0][2]) * (1.0f/k_gridscale)
    };
    v2f region_cell_pos;
    v2_floor( region_pos, region_cell_pos );
@@ -1580,9 +1576,9 @@ static void player_walkgrid(void)
 {
    player_walkgrid_getsurface();
    
-   m4x3_mulv( player.to_world, (v3f){0.0f,1.8f,0.0f}, player.camera_pos );
+   m4x3_mulv( player.rb.to_world, (v3f){0.0f,1.8f,0.0f}, player.camera_pos );
    player_mouseview();
-   player_transform_update();
+   rb_update_transform( &player.rb );
 }
 
 /*
@@ -1592,8 +1588,8 @@ static void player_walkgrid(void)
 static void player_animate(void)
 {
    /* Camera position */
-   v3_sub( player.v, player.v_last, player.a );
-   v3_copy( player.v, player.v_last );
+   v3_sub( player.rb.v, player.v_last, player.a );
+   v3_copy( player.rb.v, player.v_last );
 
    v3_add( player.m, player.a, player.m );
    v3_lerp( player.m, (v3f){0.0f,0.0f,0.0f}, 0.1f, player.m );
@@ -1613,11 +1609,11 @@ static void player_animate(void)
          kleg = 0.6f;
 
    v3f offset;
-   m3x3_mulv( player.to_local, player.bob, offset );
+   m3x3_mulv( player.rb.to_local, player.bob, offset );
 
    static float speed_wobble = 0.0f, speed_wobble_2 = 0.0f;
    
-   float kickspeed = vg_clampf(v3_length(player.v)*(1.0f/40.0f), 0.0f, 1.0f);
+   float kickspeed = vg_clampf(v3_length(player.rb.v)*(1.0f/40.0f), 0.0f, 1.0f);
    float kicks = (vg_randf()-0.5f)*2.0f*kickspeed;
    float sign = vg_signf( kicks );
    speed_wobble = vg_lerpf( speed_wobble, kicks*kicks*sign, 0.1f );
@@ -1644,7 +1640,7 @@ static void player_animate(void)
    static float fstand = 0.0f;
    static float ffly = 0.0f;
 
-   float speed = v3_length( player.v );
+   float speed = v3_length( player.rb.v );
    
    fstand = vg_lerpf(fstand, 1.0f-vg_clampf(speed*0.03f,0.0f,1.0f),0.1f);
    fslide = vg_lerpf(fslide, vg_clampf(lslip,0.0f,1.0f), 0.04f);
@@ -1688,7 +1684,7 @@ static void player_animate(void)
                    *arm_r = &player.mdl.ik_arm_r;
 
    v3f localv;
-   m3x3_mulv( player.to_local, player.v, localv );
+   m3x3_mulv( player.rb.to_local, player.rb.v, localv );
 
    /* New board transformation */
    v4f board_rotation; v3f board_location;
@@ -1824,12 +1820,12 @@ static void player_animate_death_cam(void)
 static void player_animate_camera(void)
 {
    v3f offs = { -0.29f, 0.08f, 0.0f };
-   m3x3_mulv( player.to_world, offs, offs );
-   m4x3_mulv( player.to_world, player.mdl.ik_body.end, player.camera_pos );
+   m3x3_mulv( player.rb.to_world, offs, offs );
+   m4x3_mulv( player.rb.to_world, player.mdl.ik_body.end, player.camera_pos );
    v3_add( offs, player.camera_pos, player.camera_pos );
    
    /* Look angles */
-   v3_lerp( player.vl, player.v, 0.05f, player.vl );
+   v3_lerp( player.vl, player.rb.v, 0.05f, player.vl );
 
    float yaw = atan2f(  player.vl[0], -player.vl[2] ),
        pitch = atan2f( -player.vl[1], 
@@ -1843,7 +1839,9 @@ static void player_animate_camera(void)
    /* Camera shake */
    static v2f shake_damp = {0.0f,0.0f};
    v2f shake = { vg_randf()-0.5f, vg_randf()-0.5f };
-   v2_muls( shake, v3_length(player.v)*0.3f *(1.0f+fabsf(player.slip)), shake);
+   v2_muls( shake, v3_length(player.rb.v)*0.3f 
+                        * (1.0f+fabsf(player.slip)), shake);
+
    v2_lerp( shake_damp, shake, 0.01f, shake_damp );
    shake_damp[0] *= 0.2f;
 
@@ -1855,8 +1853,8 @@ static void player_animate_camera(void)
  */
 static void player_audio(void)
 {
-   float speed = vg_minf(v3_length( player.v )*0.1f,1.0f),
-         attn = v3_dist( player.co, player.camera[3] )+1.0f;
+   float speed = vg_minf(v3_length( player.rb.v )*0.1f,1.0f),
+         attn = v3_dist( player.rb.co, player.camera[3] )+1.0f;
    attn = (1.0f/(attn*attn)) * speed;
 
    static float air = 0.0f;
@@ -1865,7 +1863,7 @@ static void player_audio(void)
    v3f ears = { 1.0f,0.0f,0.0f };
    v3f delta;
 
-   v3_sub( player.co, player.camera[3], delta );
+   v3_sub( player.rb.co, player.camera[3], delta );
    v3_normalize( delta );
    m3x3_mulv( player.camera, ears, ears );
 
@@ -1930,11 +1928,10 @@ static int reset_player( int argc, char const *argv[] )
    {
       float min_dist = INFINITY;
 
-         vg_info( "%f %f %f\n", player.co[0], player.co[1], player.co[2] );
       for( int i=0; i<world.spawn_count; i++ )
       {
          r = &world.spawns[i];
-         float d = v3_dist2( r->co, player.co );
+         float d = v3_dist2( r->co, player.rb.co );
          
          vg_info( "Dist %s : %f\n", r->name, d );
          if( d < min_dist )
@@ -1954,8 +1951,8 @@ static int reset_player( int argc, char const *argv[] )
       rp = &world.spawns[0];
    }
 
-   v4_copy( rp->q, player.rot );
-   v3_copy( rp->co, player.co );
+   v4_copy( rp->q, player.rb.q );
+   v3_copy( rp->co, player.rb.co );
 
    player.vswitch = 1.0f;
    player.slip_last = 0.0f;
@@ -1965,9 +1962,9 @@ static int reset_player( int argc, char const *argv[] )
 
    player.mdl.shoes[0] = 1;
    player.mdl.shoes[1] = 1;
-
-   player_transform_update();
-   m3x3_mulv( player.to_world, (v3f){ 0.0f, 0.0f, -1.2f }, player.v );
+   
+   rb_update_transform( &player.rb );
+   m3x3_mulv( player.rb.to_world, (v3f){ 0.0f, 0.0f, -1.2f }, player.rb.v );
    return 1;
 }
 
@@ -2019,7 +2016,7 @@ static void player_update(void)
 static void draw_player(void)
 {
    /* Draw */
-   m4x3_copy( player.to_world, player.mdl.mroot );
+   m4x3_copy( player.rb.to_world, player.mdl.mroot );
 
    if( player.is_dead )
       character_mimic_ragdoll( &player.mdl );
@@ -2030,7 +2027,7 @@ static void draw_player(void)
    if( player.is_dead )
       opacity = 0.0f;
 
-   character_draw( &player.mdl, opacity );
+   character_draw( &player.mdl, opacity, player.camera );
 }
 
 #endif /* PLAYER_H */
diff --git a/rigidbody.h b/rigidbody.h
index eeed745..d911688 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -19,34 +19,54 @@ static bh_system bh_system_rigidbodies;
 typedef struct rigidbody rigidbody;
 struct rigidbody
 {
-   v3f co, v, I;
+   v3f co, v, w;
    v4f q;
 
    enum rb_shape
    {
       k_rb_shape_box,
+      k_rb_shape_sphere,
       k_rb_shape_capsule
    } 
    type;
-   v3f top, bottom;
-   float radius;
+   
+   union
+   {
+      struct rb_sphere
+      {
+         float radius;
+      }
+      sphere;
 
+      struct rb_capsule
+      {
+         float height, radius;
+      } 
+      capsule;
+   }
+   inf;
+
+   v3f right, up, forward;
+   
    boxf bbx, bbx_world;
    float inv_mass;
 
-   struct contact
-   {
-      v3f co, n, delta;
-      v3f t[2];
-      float bias, norm_impulse, tangent_impulse[2];
-   }
-   manifold[12];
-   int manifold_count;
-
    v3f delta;  /* where is the origin of this in relation to a parent body */
    m4x3f to_world, to_local;
 };
 
+static void rb_debug( rigidbody *rb, u32 colour );
+
+static struct contact
+{
+   rigidbody *rba;
+   v3f co, n, delta;
+   v3f t[2];
+   float bias, norm_impulse, tangent_impulse[2];
+}
+rb_contact_buffer[256];
+static int rb_contact_count = 0;
+
 static void rb_update_transform( rigidbody *rb )
 {
    q_normalize( rb->q );
@@ -57,18 +77,50 @@ static void rb_update_transform( rigidbody *rb )
 
    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 );
+}
+
+static float sphere_volume( float radius )
+{
+   float r3 = radius*radius*radius;
+   return (4.0f/3.0f) * VG_PIf * r3;
 }
 
 static void rb_init( rigidbody *rb )
 {
-   q_identity( rb->q );
-   v3_zero( rb->v );
-   v3_zero( rb->I );
+   float volume = 1.0f;
+
+   if( rb->type == k_rb_shape_box )
+   {
+      v3f dims;
+      v3_sub( rb->bbx[1], rb->bbx[0], dims );
+      volume = dims[0]*dims[1]*dims[2];
+   }
+   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 );
+   }
+   else if( rb->type == k_rb_shape_capsule )
+   {
+      float r = rb->inf.capsule.radius,
+            h = rb->inf.capsule.height;
+      volume = sphere_volume( r ) + VG_PIf * r*r * (h - r*2.0f);
+
+      v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
+      v3_fill( rb->bbx[1],  rb->inf.sphere.radius );
+      rb->bbx[0][1] = -h;
+      rb->bbx[1][1] =  h;
+   }
 
-   v3f dims;
-   v3_sub( rb->bbx[1], rb->bbx[0], dims );
+   rb->inv_mass = 1.0f/(8.0f*volume);
 
-   rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]);
+   v3_zero( rb->v );
+   v3_zero( rb->w );
 
    rb_update_transform( rb );
 }
@@ -80,14 +132,14 @@ static void rb_iter( rigidbody *rb )
 
    /* intergrate velocity */
    v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
-   v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
+   v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
 
    /* inegrate inertia */
-   if( v3_length2( rb->I ) > 0.0f )
+   if( v3_length2( rb->w ) > 0.0f )
    {
       v4f rotation;
       v3f axis;
-      v3_copy( rb->I, axis );
+      v3_copy( rb->w, axis );
       
       float mag = v3_length( axis );
       v3_divs( axis, mag, axis );
@@ -98,7 +150,7 @@ static void rb_iter( rigidbody *rb )
 
 static void rb_torque( rigidbody *rb, v3f axis, float mag )
 {
-   v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I );
+   v3_muladds( rb->w, axis, mag*k_rb_delta, rb->w );
 }
 
 static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
@@ -121,13 +173,297 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
    v3_cross( n, tx, ty );
 }
 
+static void rb_solver_reset(void);
+static void rb_build_manifold_terrain( rigidbody *rb );
+static void rb_build_manifold_terrain_sphere( rigidbody *rb );
+static void rb_solve_contacts(void);
+
+static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, 
+   float *s, float *t, v3f c1, v3f c2)
+{
+   v3f d1,d2,r;
+   v3_sub( q1, p1, d1 );
+   v3_sub( q2, p2, d2 );
+   v3_sub( p1, p2, r );
+
+   float a = v3_length2( d1 ),
+         e = v3_length2( d2 ),
+         f = v3_dot( d2, r );
+
+   const float kEpsilon = 0.0001f;
+
+   if( a <= kEpsilon && e <= kEpsilon )
+   {
+      *s = 0.0f;
+      *t = 0.0f;
+      v3_copy( p1, c1 );
+      v3_copy( p2, c2 );
+
+      v3f v0;
+      v3_sub( c1, c2, v0 );
+
+      return v3_length2( v0 );
+   }
+   
+   if( a<= kEpsilon )
+   {
+      *s = 0.0f;
+      *t = vg_clampf( f / e, 0.0f, 1.0f );
+   }
+   else
+   {
+      float c = v3_dot( d1, r );
+      if( e <= kEpsilon )
+      {
+         *t = 0.0f;
+         *s = vg_clampf( -c / a, 0.0f, 1.0f );
+      }
+      else
+      {
+         float b = v3_dot(d1,d2),
+               d = a*e-b*b;
+
+         if( d != 0.0f )
+         {
+            *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+         }
+         else
+         {
+            *s = 0.0f;
+         }
+
+         *t = (b*(*s)+f) / e;
+
+         if( *t < 0.0f )
+         {
+            *t = 0.0f;
+            *s = vg_clampf( -c / a, 0.0f, 1.0f );
+         }
+         else if( *t > 1.0f )
+         {
+            *t = 1.0f;
+            *s = vg_clampf((b-c)/a,0.0f,1.0f);
+         }
+      }
+   }
+
+   v3_muladds( p1, d1, *s, c1 );
+   v3_muladds( p2, d2, *t, c2 );
+
+   v3f v0;
+   v3_sub( c1, c2, v0 );
+   return v3_length2( v0 );
+}
+
+static void 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 );
+}
+
+/* Real-Time Collision Detection */
+static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+{
+   v3f ab, ac, ap;
+   float d1, d2;
+
+   /* Region outside A */
+   v3_sub( tri[1], tri[0], ab );
+   v3_sub( tri[2], tri[0], ac );
+   v3_sub( p, tri[0], ap );
+   
+   d1 = v3_dot(ab,ap);
+   d2 = v3_dot(ac,ap);
+   if( d1 <= 0.0f && d2 <= 0.0f ) 
+   {
+      v3_copy( tri[0], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region outside B */
+   v3f bp;
+   float d3, d4;
+
+   v3_sub( p, tri[1], bp );
+   d3 = v3_dot( ab, bp );
+   d4 = v3_dot( ac, bp );
+
+   if( d3 >= 0.0f && d4 <= d3 )
+   {
+      v3_copy( tri[1], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+   
+   /* Edge region of AB */
+   float vc = d1*d4 - d3*d2;
+   if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+   {
+      float v = d1 / (d1-d3);
+      v3_muladds( tri[0], ab, v, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region outside C */
+   v3f cp;
+   float d5, d6;
+   v3_sub( p, tri[2], cp );
+   d5 = v3_dot(ab, cp);
+   d6 = v3_dot(ac, cp);
+   
+   if( d6 >= 0.0f && d5 <= d6 )
+   {
+      v3_copy( tri[2], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region of AC */
+   float vb = d5*d2 - d1*d6;
+   if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+   {
+      float w = d2 / (d2-d6);
+      v3_muladds( tri[0], ac, w, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region of BC */
+   float va = d3*d6 - d5*d4;
+   if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+   {
+      float w = (d4-d3) / ((d4-d3) + (d5-d6));
+      v3f bc;
+      v3_sub( tri[2], tri[1], bc );
+      v3_muladds( tri[1], bc, w, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* P inside region, Q via barycentric coordinates uvw */
+   float d = 1.0f/(va+vb+vc),
+         v = vb*d,
+         w = vc*d;
+
+   v3_muladds( tri[0], ab, v, dest );
+   v3_muladds( dest, ac, w, dest );
+}
+
+static int sphere_vs_triangle( v3f c, float r, v3f tri[3], 
+      v3f co, v3f norm, float *p )
+{
+   v3f delta;
+   closest_on_triangle( c, tri, co );
+
+   v3_sub( c, co, delta );
+
+
+   float d = v3_length2( delta );
+   if( d < r*r )
+   {
+      v3f ab, ac, tn;
+      v3_sub( tri[1], tri[0], ab );
+      v3_sub( tri[2], tri[0], ac );
+      v3_cross( ac, ab, tn );
+
+      if( v3_dot( delta, tn ) > 0.0f )
+         v3_muls( delta, -1.0f, delta );
+
+      vg_line_pt3( co, 0.05f, 0xff00ff00 );
+
+      d = sqrtf(d);
+      v3_muls( delta, 1.0f/d, norm );
+
+      *p = r-d;
+      return 1;
+   }
+
+   return 0;
+}
+
 #include "world.h"
 
-static void rb_manifold_reset( rigidbody *rb )
+static void rb_solver_reset(void)
 {
-   rb->manifold_count = 0;
+   rb_contact_count = 0;
 }
 
+static struct contact *rb_start_contact(void)
+{
+   if( rb_contact_count == vg_list_size(rb_contact_buffer) )
+   {
+      vg_error( "rigidbody: too many contacts generated (%u)\n", 
+            rb_contact_count );
+      return NULL;
+   }
+
+   return &rb_contact_buffer[ rb_contact_count ];
+}
+
+static void rb_commit_contact( struct contact *ct, float p )
+{
+   ct->bias = -0.2f*k_rb_rate*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;
+
+   rb_contact_count ++;
+}
+
+static void rb_build_manifold_terrain_sphere( rigidbody *rb )
+{
+   u32 geo[256];
+   v3f tri[3];
+   int len = bh_select( &world.geo.bhtris, rb->bbx_world, geo, 256 );
+   
+   for( int i=0; i<len; i++ )
+   {
+      u32 *ptri = &world.geo.indices[ geo[i]*3 ];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( world.geo.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 );
+
+      v3f co, norm;
+      float p;
+
+      for( int j=0; j<2; j++ )
+      {
+         if( sphere_vs_triangle( rb->co, rb->inf.sphere.radius, tri,co,norm,&p))
+         {
+            struct contact *ct = rb_start_contact();
+
+            if( !ct )
+               return;
+
+            v3f p1;
+            v3_muladds( rb->co, norm, p, p1 );
+            vg_line( rb->co, p1, 0xffffffff );
+
+            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 );
+         }
+      }
+   }
+
+}
+
+RB_DEPR
 static void rb_build_manifold_terrain( rigidbody *rb )
 {
    v3f *box = rb->bbx;
@@ -159,7 +495,12 @@ static void rb_build_manifold_terrain( rigidbody *rb )
    for( int i=0; i<8; i++ )
    {
       float *point = pts[i];
-      struct contact *ct = &rb->manifold[rb->manifold_count];
+      struct contact *ct = rb_start_contact();
+
+      if( !ct )
+         return;
+
+      ct->rba = rb;
       
       v3f surface;
       v3_copy( point, surface );
@@ -170,27 +511,18 @@ static void rb_build_manifold_terrain( rigidbody *rb )
       if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
          continue;
 
-      v3_copy( hit.normal, ct->n );
       v3_copy( hit.pos, surface );
 
       float p = vg_minf( surface[1] - point[1], 1.0f );
 
       if( p > 0.0f )
       {
+         v3_copy( hit.normal, ct->n );
          v3_add( point, surface, ct->co );
          v3_muls( ct->co, 0.5f, ct->co );
-
-         //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
-
          v3_sub( ct->co, rb->co, ct->delta );
-         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;
 
-         rb->manifold_count ++;
+         rb_commit_contact( ct, p );
          count ++;
          if( count == 4 )
             break;
@@ -198,17 +530,18 @@ static void rb_build_manifold_terrain( rigidbody *rb )
    }
 }
 
-static void rb_constraint_manifold( rigidbody *rb )
+static void rb_solve_contacts(void)
 {
    float k_friction = 0.1f;
 
    /* Friction Impulse */
-   for( int i=0; i<rb->manifold_count; i++ )
+   for( int i=0; i<rb_contact_count; i++ )
    {
-      struct contact *ct = &rb->manifold[i];
+      struct contact *ct = &rb_contact_buffer[i];
+      rigidbody *rb = ct->rba;
 
       v3f dv;
-      v3_cross( rb->I, ct->delta, dv );
+      v3_cross( rb->w, ct->delta, dv );
       v3_add( rb->v, dv, dv );
       
       for( int j=0; j<2; j++ )
@@ -227,17 +560,18 @@ static void rb_constraint_manifold( rigidbody *rb )
          v3_muls( ct->t[j], vt, impulse );
          v3_add( impulse, rb->v, rb->v );
          v3_cross( ct->delta, impulse, impulse );
-         v3_add( impulse, rb->I, rb->I );
+         v3_add( impulse, rb->w, rb->w );
       }
    }
 
    /* Normal Impulse */
-   for( int i=0; i<rb->manifold_count; i++ )
+   for( int i=0; i<rb_contact_count; i++ )
    {
-      struct contact *ct = &rb->manifold[i];
+      struct contact *ct = &rb_contact_buffer[i];
+      rigidbody *rb = ct->rba;
 
       v3f dv;
-      v3_cross( rb->I, ct->delta, dv );
+      v3_cross( rb->w, ct->delta, dv );
       v3_add( rb->v, dv, dv );
 
       float vn = -v3_dot( dv, ct->n );
@@ -252,7 +586,7 @@ static void rb_constraint_manifold( rigidbody *rb )
       v3_muls( ct->n, vn, impulse );
       v3_add( impulse, rb->v, rb->v );
       v3_cross( ct->delta, impulse, impulse );
-      v3_add( impulse, rb->I, rb->I );
+      v3_add( impulse, rb->w, rb->w );
    }
 }
 
@@ -313,8 +647,8 @@ static void rb_constraint_angle( rigidbody *rba, v3f va,
 
    v3f axis;
    v3_cross( wva, wvb, axis );
-   v3_muladds( rba->I, axis,  ang*spring*0.5f, rba->I );
-   v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I );
+   v3_muladds( rba->w, axis,  ang*spring*0.5f, rba->w );
+   v3_muladds( rbb->w, axis, -ang*spring*0.5f, rbb->w );
 
    return;
    
@@ -344,8 +678,8 @@ static void rb_relative_velocity( rigidbody *ra, v3f lca,
    v3_sub( ra->v, rb->v, rcv );
 
    v3f rcv_Ra, rcv_Rb;
-   v3_cross( ra->I, wca, rcv_Ra );
-   v3_cross( rb->I, wcb, 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 );
 }
@@ -370,8 +704,8 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    v3_sub( ra->v, rb->v, rcv );
 
    v3f rcv_Ra, rcv_Rb;
-   v3_cross( ra->I, wca, rcv_Ra );
-   v3_cross( rb->I, wcb, 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 );
 
@@ -385,12 +719,12 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    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->I, rb->I );
+   v3_add( impulse, rb->w, rb->w );
 
    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->I, ra->I );
+   v3_add( impulse, ra->w, ra->w );
 
 #if 0
    /*
@@ -402,20 +736,61 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
 
    v3_add( impulse, ra->v, ra->v );
    v3_cross( wca, impulse, impulse );
-   v3_add( impulse, ra->I, ra->I );
+   v3_add( impulse, ra->w, ra->w );
 
    v3_muls( delta, -0.5f*spring, impulse );
 
    v3_add( impulse, rb->v, rb->v );
    v3_cross( wcb, impulse, impulse );
-   v3_add( impulse, rb->I, rb->I );
+   v3_add( impulse, rb->w, rb->w );
 #endif
 }
 
+static void debug_sphere( m4x3f m, float radius, u32 colour )
+{
+   v3f ly = { 0.0f, 0.0f, radius },
+       lx = { 0.0f, radius, 0.0f },
+       lz = { 0.0f, 0.0f, radius };
+   
+   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 py = { s*radius, 0.0f, c*radius },
+          px = { s*radius, c*radius, 0.0f },
+          pz = { 0.0f, s*radius, c*radius };
+
+      v3f p0, p1, p2, p3, p4, p5;
+      m4x3_mulv( m, py, p0 );
+      m4x3_mulv( m, ly, p1 );
+      m4x3_mulv( m, px, p2 );
+      m4x3_mulv( m, lx, p3 );
+      m4x3_mulv( m, pz, p4 );
+      m4x3_mulv( m, lz, p5 );
+
+      vg_line( p0, p1, colour == 0x00? 0xff00ff00: colour );
+      vg_line( p2, p3, colour == 0x00? 0xff0000ff: colour );
+      vg_line( p4, p5, colour == 0x00? 0xffff0000: colour );
+
+      v3_copy( py, ly );
+      v3_copy( px, lx );
+      v3_copy( pz, lz );
+   }
+}
+
 static void rb_debug( rigidbody *rb, u32 colour )
 {
-   v3f *box = rb->bbx;
-   vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
+   if( rb->type == k_rb_shape_box )
+   {
+      v3f *box = rb->bbx;
+      vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
+   }
+   else if( rb->type == k_rb_shape_sphere )
+   {
+      debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
+   }
 }
 
 /*
@@ -467,6 +842,7 @@ 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];
@@ -520,220 +896,12 @@ static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
       }
    }
 }
+#endif
 
 /* 
  * Capsule phyics
  */
 
-static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, 
-   float *s, float *t, v3f c1, v3f c2)
-{
-   v3f d1,d2,r;
-   v3_sub( q1, p1, d1 );
-   v3_sub( q2, p2, d2 );
-   v3_sub( p1, p2, r );
-
-   float a = v3_length2( d1 ),
-         e = v3_length2( d2 ),
-         f = v3_dot( d2, r );
-
-   const float kEpsilon = 0.0001f;
-
-   if( a <= kEpsilon && e <= kEpsilon )
-   {
-      *s = 0.0f;
-      *t = 0.0f;
-      v3_copy( p1, c1 );
-      v3_copy( p2, c2 );
-
-      v3f v0;
-      v3_sub( c1, c2, v0 );
-
-      return v3_length2( v0 );
-   }
-   
-   if( a<= kEpsilon )
-   {
-      *s = 0.0f;
-      *t = vg_clampf( f / e, 0.0f, 1.0f );
-   }
-   else
-   {
-      float c = v3_dot( d1, r );
-      if( e <= kEpsilon )
-      {
-         *t = 0.0f;
-         *s = vg_clampf( -c / a, 0.0f, 1.0f );
-      }
-      else
-      {
-         float b = v3_dot(d1,d2),
-               d = a*e-b*b;
-
-         if( d != 0.0f )
-         {
-            *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
-         }
-         else
-         {
-            *s = 0.0f;
-         }
-
-         *t = (b*(*s)+f) / e;
-
-         if( *t < 0.0f )
-         {
-            *t = 0.0f;
-            *s = vg_clampf( -c / a, 0.0f, 1.0f );
-         }
-         else if( *t > 1.0f )
-         {
-            *t = 1.0f;
-            *s = vg_clampf((b-c)/a,0.0f,1.0f);
-         }
-      }
-   }
-
-   v3_muladds( p1, d1, *s, c1 );
-   v3_muladds( p2, d2, *t, c2 );
-
-   v3f v0;
-   v3_sub( c1, c2, v0 );
-   return v3_length2( v0 );
-}
-
-static void 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 );
-}
-
-/* Real-Time Collision Detection */
-static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
-{
-   v3f ab, ac, ap;
-   float d1, d2;
-
-   /* Region outside A */
-   v3_sub( tri[1], tri[0], ab );
-   v3_sub( tri[2], tri[0], ac );
-   v3_sub( p, tri[0], ap );
-   
-   d1 = v3_dot(ab,ap);
-   d2 = v3_dot(ac,ap);
-   if( d1 <= 0.0f && d2 <= 0.0f ) 
-   {
-      v3_copy( tri[0], dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
-   }
-
-   /* Region outside B */
-   v3f bp;
-   float d3, d4;
-
-   v3_sub( p, tri[1], bp );
-   d3 = v3_dot( ab, bp );
-   d4 = v3_dot( ac, bp );
-
-   if( d3 >= 0.0f && d4 <= d3 )
-   {
-      v3_copy( tri[1], dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
-   }
-   
-   /* Edge region of AB */
-   float vc = d1*d4 - d3*d2;
-   if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
-   {
-      float v = d1 / (d1-d3);
-      v3_muladds( tri[0], ab, v, dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
-   }
-
-   /* Region outside C */
-   v3f cp;
-   float d5, d6;
-   v3_sub( p, tri[2], cp );
-   d5 = v3_dot(ab, cp);
-   d6 = v3_dot(ac, cp);
-   
-   if( d6 >= 0.0f && d5 <= d6 )
-   {
-      v3_copy( tri[2], dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
-   }
-
-   /* Region of AC */
-   float vb = d5*d2 - d1*d6;
-   if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
-   {
-      float w = d2 / (d2-d6);
-      v3_muladds( tri[0], ac, w, dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
-   }
-
-   /* Region of BC */
-   float va = d3*d6 - d5*d4;
-   if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
-   {
-      float w = (d4-d3) / ((d4-d3) + (d5-d6));
-      v3f bc;
-      v3_sub( tri[2], tri[1], bc );
-      v3_muladds( tri[1], bc, w, dest );
-      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
-      return;
-   }
-
-   /* P inside region, Q via barycentric coordinates uvw */
-   float d = 1.0f/(va+vb+vc),
-         v = vb*d,
-         w = vc*d;
-
-   v3_muladds( tri[0], ab, v, dest );
-   v3_muladds( dest, ac, w, dest );
-}
-
-static int sphere_vs_triangle( v3f c, float r, v3f tri[3], 
-      v3f co, v3f norm, float *p )
-{
-   v3f delta;
-   closest_on_triangle( c, tri, co );
-
-   v3_sub( c, co, delta );
-
-
-   float d = v3_length2( delta );
-   if( d < r*r )
-   {
-      v3f ab, ac, tn;
-      v3_sub( tri[1], tri[0], ab );
-      v3_sub( tri[2], tri[0], ac );
-      v3_cross( ac, ab, tn );
-
-      if( v3_dot( delta, tn ) > 0.0f )
-         v3_muls( delta, -1.0f, delta );
-
-      vg_line_pt3( co, 0.05f, 0xff00ff00 );
-
-      d = sqrtf(d);
-      v3_muls( delta, 1.0f/d, norm );
-
-      *p = r-d;
-      return 1;
-   }
-
-   return 0;
-}
-
 static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
 {
    v3f last = { 0.0f, 0.0f, radius };
diff --git a/shaders/vblend.fs b/shaders/vblend.fs
index 023721e..75eed17 100644
--- a/shaders/vblend.fs
+++ b/shaders/vblend.fs
@@ -18,7 +18,7 @@ void main()
    vec3 vfrag = vec3(0.5,0.5,0.5);
 
    // ws modulation
-   vec4 wgarbage = texture( uTexGarbage, aCo.xz * 0.160 );
+   vec4 wgarbage = vec4(0.5,0.5,0.5,1.0);//texture( uTexGarbage, aCo.xz * 0.160 );
    
    // Creating normal patches
    vec3 modnorm = (wgarbage.rgb-0.4) * 1.4;
diff --git a/shaders/vblend.h b/shaders/vblend.h
index 3f5bfdc..5738ef1 100644
--- a/shaders/vblend.h
+++ b/shaders/vblend.h
@@ -158,7 +158,7 @@ static struct vg_shader _shader_vblend = {
 "   vec3 vfrag = vec3(0.5,0.5,0.5);\n"
 "\n"
 "   // ws modulation\n"
-"   vec4 wgarbage = texture( uTexGarbage, aCo.xz * 0.160 );\n"
+"   vec4 wgarbage = vec4(0.5,0.5,0.5,1.0);//texture( uTexGarbage, aCo.xz * 0.160 );\n"
 "   \n"
 "   // Creating normal patches\n"
 "   vec3 modnorm = (wgarbage.rgb-0.4) * 1.4;\n"
diff --git a/textures/gradients.png b/textures/gradients.png
index 5118b9c..15bd113 100644
Binary files a/textures/gradients.png and b/textures/gradients.png differ
diff --git a/world.h b/world.h
index add08b7..079d6d3 100644
--- a/world.h
+++ b/world.h
@@ -48,17 +48,19 @@ static struct gworld
    /* Physics */
    rigidbody temp_rbs[128];
    u32 rb_count;
-   bh_tree bhcubes;
    
    /* Rendering & geometry */
-   scene geo, foliage, props;
-   mdl_submesh sm_surface, sm_other;
+   scene geo, foliage;
+
+   mdl_submesh sm_geo_std_oob, sm_geo_std, sm_geo_vb;
 
    glmesh skybox, skydome;
    mdl_submesh dome_upper, dome_lower;
 
    glmesh cars;
    mdl_submesh car_holden;
+
+   rigidbody mr_ball;
 }
 world;
 
@@ -81,7 +83,7 @@ static int ray_world( v3f pos, v3f dir, ray_hit *hit )
 
 static int ray_hit_is_ramp( ray_hit *hit )
 {
-   return hit->tri[0] < world.sm_surface.vertex_count;
+   return hit->tri[0] > world.sm_geo_std_oob.vertex_count;
 }
 
 static void world_register(void)
@@ -179,7 +181,6 @@ static void world_load(void)
    world.traffic_count = 0;
 
    scene_init( &world.geo );
-   scene_init( &world.props );
 
    /* 
     * Compile meshes into the world scenes
@@ -203,26 +204,22 @@ static void world_load(void)
          mat_vertex_blend = i;
    }
 
-   if( mat_surf )
-      add_all_if_material( &world.geo, mworld, mat_surf );
-   if( mat_vertex_blend )
-      add_all_if_material( &world.geo, mworld, mat_vertex_blend );
-
-
-   scene_copy_slice( &world.geo, &world.sm_surface );
-
    if( mat_surf_oob )
       add_all_if_material( &world.geo, mworld, mat_surf_oob );
    else
       vg_warn( "No OOB surface\n" );
+   scene_copy_slice( &world.geo, &world.sm_geo_std_oob );
 
-   scene_bh_create( &world.geo );
-   scene_upload( &world.geo );
+   if( mat_surf )
+      add_all_if_material( &world.geo, mworld, mat_surf );
+   scene_copy_slice( &world.geo, &world.sm_geo_std );
 
    if( mat_vertex_blend )
-      add_all_if_material( &world.props, mworld, mat_vertex_blend );
+      add_all_if_material( &world.geo, mworld, mat_vertex_blend );
+   scene_copy_slice( &world.geo, &world.sm_geo_vb );
 
-   /* TODO  bvh? */
+   scene_upload( &world.geo );
+   scene_bh_create( &world.geo );
    
    /*
     * Process entities
@@ -317,11 +314,6 @@ static void world_load(void)
    for( int i=0; i<vg_list_size(world.van_man); i++ )
       world.van_man[i].current =&world.traffic[vg_randint(world.traffic_count)];
 
-   scene_upload( &world.props );
-
-   bh_create( &world.bhcubes, 
-         &bh_system_rigidbodies, world.temp_rbs, world.rb_count );
-
    world_apply_foliage();
    free( mworld );
 
@@ -378,7 +370,14 @@ static void world_load(void)
 
    winfo->g_water_fog = 0.04f;
    render_update_lighting_ub();
+   
+
+   world.mr_ball.type = k_rb_shape_sphere;
+   world.mr_ball.inf.sphere.radius = 2.0f;
+   v3_copy( (v3f){ 0.0f, 110.0f, 0.0f }, world.mr_ball.co );
 
+   q_identity(world.mr_ball.q);
+   rb_init( &world.mr_ball );
 }
 
 static void world_init(void)
@@ -406,6 +405,16 @@ static void world_init(void)
 
 static void world_update(void)
 {
+   rb_solver_reset();
+   rb_build_manifold_terrain_sphere( &world.mr_ball );
+   
+   for( int i=0; i<5; i++ )
+      rb_solve_contacts();
+
+   rb_iter( &world.mr_ball );
+   rb_update_transform( &world.mr_ball );
+   rb_debug( &world.mr_ball, 0 );
+
    for( int i=0; i<vg_list_size(world.van_man); i++ )
    {
       traffic_drive( &world.van_man[i] );
@@ -423,7 +432,7 @@ static void bind_terrain_textures(void)
    vg_tex2d_bind( &tex_terrain_colours, 1 );
 }
 
-static void render_props( m4x4f projection, v3f camera )
+static void render_world_vb( m4x4f projection, v3f camera )
 {
    m4x3f identity_matrix;
    m4x3_identity( identity_matrix );
@@ -438,8 +447,8 @@ static void render_props( m4x4f projection, v3f camera )
    shader_vblend_uMdl( identity_matrix );
    shader_vblend_uCamera( camera );
 
-   scene_bind( &world.props );
-   scene_draw( &world.props );
+   scene_bind( &world.geo );
+   mdl_draw_submesh( &world.sm_geo_vb );
 
    mesh_bind( &world.cars );
 
@@ -466,7 +475,8 @@ static void render_terrain( m4x4f projection, v3f camera )
    shader_terrain_uCamera( camera );
 
    scene_bind( &world.geo );
-   scene_draw( &world.geo );
+   mdl_draw_submesh( &world.sm_geo_std_oob );
+   mdl_draw_submesh( &world.sm_geo_std );
 
    glDisable(GL_CULL_FACE);
    scene_bind( &world.foliage );
@@ -532,7 +542,7 @@ static void render_sky(m4x3f camera)
 static void render_world( m4x4f projection, m4x3f camera )
 {
    render_sky( camera );
-   render_props( projection, camera[3] );
+   render_world_vb( projection, camera[3] );
    render_terrain( projection, camera[3] );
 }
 
@@ -555,9 +565,6 @@ static void render_world_depth( m4x4f projection, m4x3f camera )
    scene_draw( &world.foliage );
    glEnable(GL_CULL_FACE);
 #endif
-
-   scene_bind( &world.props );
-   scene_draw( &world.props );
 }
 
 #endif /* WORLD_H */