+VG_STATIC void player_walk_update_collision(void)
+{
+ struct player_phys *phys = &player.phys;
+ float h0 = 0.3f,
+ h1 = 0.9f;
+
+ rigidbody *rbf = &player.collide_front,
+ *rbb = &player.collide_back;
+
+ v3_add( phys->rb.co, (v3f){0.0f,h0,0.0f}, rbf->co );
+ v3_add( phys->rb.co, (v3f){0.0f,h1,0.0f}, rbb->co );
+ v3_copy( rbf->co, rbf->to_world[3] );
+ v3_copy( rbb->co, rbb->to_world[3] );
+ m4x3_invert_affine( rbf->to_world, rbf->to_local );
+ m4x3_invert_affine( rbb->to_world, rbb->to_local );
+
+ rb_update_bounds( rbf );
+ rb_update_bounds( rbb );
+}
+
+VG_STATIC void player_integrate(void);
+
+VG_STATIC int player_walk_surface_standable( v3f n )
+{
+ return v3_dot( n, (v3f){0.0f,1.0f,0.0f} ) > 0.5f;
+}
+
+VG_STATIC void player_walk_stepdown(void)
+{
+ struct player_phys *phys = &player.phys;
+ float max_dist = 0.4f;
+
+ v3f pa, pb;
+ v3_copy( phys->rb.co, pa );
+ pa[1] += 0.3f;
+
+ v3_muladds( pa, (v3f){0.01f,1.0f,0.01f}, -max_dist, pb );
+ vg_line( pa, pb, 0xff000000 );
+
+ /* TODO: Make #define */
+ float r = 0.3f,
+ t;
+
+ v3f n;
+ if( spherecast_world( pa, pb, r, &t, n ) != -1 )
+ {
+ if( player_walk_surface_standable( n ) )
+ {
+ phys->in_air = 0;
+ v3_lerp( pa, pb, t+0.001f, phys->rb.co );
+ phys->rb.co[1] -= 0.3f;
+ }
+ }
+}
+
+/*
+ * Entire Walking physics model
+ * TODO: sleep when under certain velotiy
+ */
+VG_STATIC void player_walk_physics(void)
+{
+ struct player_phys *phys = &player.phys;
+ rigidbody *rbf = &player.collide_front,
+ *rbb = &player.collide_back;
+
+ m3x3_identity( player.collide_front.to_world );
+ m3x3_identity( player.collide_back.to_world );
+
+ v3_zero( phys->rb.w );
+ q_axis_angle( phys->rb.q, (v3f){0.0f,1.0f,0.0f}, -player.angles[0] );
+
+ rb_ct manifold[64];
+ int len;
+
+ v3f forward_dir = { sinf(player.angles[0]),0.0f,-cosf(player.angles[0]) };
+ v3f right_dir = { -forward_dir[2], 0.0f, forward_dir[0] };
+
+ v2f walk = { player.input_walkh->axis.value,
+ player.input_walkv->axis.value };
+
+ if( freecam )
+ v2_zero( walk );
+
+ if( v2_length2(walk) > 0.001f )
+ v2_normalize_clamp( walk );
+
+ if( phys->in_air )
+ {
+ player_walk_update_collision();
+ rb_debug( rbf, 0xff0000ff );
+ rb_debug( rbb, 0xff0000ff );
+
+ /* allow player to accelerate a bit */
+ v3f walk_3d;
+ v3_muls( forward_dir, walk[1], walk_3d );
+ v3_muladds( walk_3d, right_dir, walk[0], walk_3d );
+
+ float current_vel = fabsf(v3_dot( walk_3d, phys->rb.v )),
+ new_vel = current_vel + VG_TIMESTEP_FIXED*k_air_accelerate,
+ clamped_new = vg_clampf( new_vel, 0.0f, k_walkspeed ),
+ vel_diff = vg_maxf( 0.0f, clamped_new - current_vel );
+
+ v3_muladds( phys->rb.v, right_dir, walk[0] * vel_diff, phys->rb.v );
+ v3_muladds( phys->rb.v, forward_dir, walk[1] * vel_diff, phys->rb.v );
+
+
+ len = 0;
+ len += rb_sphere_scene( rbf, &world.rb_geo, manifold+len );
+ len += rb_sphere_scene( rbb, &world.rb_geo, manifold+len );
+ rb_presolve_contacts( manifold, len );
+
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &manifold[i];
+ if( v3_dot( ct->n, (v3f){0.0f,1.0f,0.0f} ) > 0.5f )
+ phys->in_air = 0;
+ }
+
+ for( int j=0; j<5; j++ )
+ {
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &manifold[i];
+
+ /*normal */
+ float vn = -v3_dot( phys->rb.v, ct->n );
+ vn += ct->bias;
+
+ float temp = ct->norm_impulse;
+ ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
+ vn = ct->norm_impulse - temp;
+
+ v3f impulse;
+ v3_muls( ct->n, vn, impulse );
+
+ v3_add( impulse, phys->rb.v, phys->rb.v );
+
+ /* friction */
+ for( int j=0; j<2; j++ )
+ {
+ float f = k_friction * ct->norm_impulse,
+ vt = v3_dot( phys->rb.v, ct->t[j] ),
+ lambda = -vt;
+
+ float temp = ct->tangent_impulse[j];
+ ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
+ lambda = ct->tangent_impulse[j] - temp;
+
+ v3_muladds( phys->rb.v, ct->t[j], lambda, phys->rb.v );
+ }
+ }
+ }
+
+ player_integrate();
+ }
+ else
+ {
+ player.walk = v2_length( walk );
+
+ if( player.input_walk->button.value )
+ v2_muls( walk, 0.5f, walk );
+
+ v2_muls( walk, k_walkspeed * VG_TIMESTEP_FIXED, walk );