+static void player_walk_physics(void)
+{
+ rigidbody *rbf = &player.collide_front,
+ *rbb = &player.collide_back;
+
+ m3x3_copy( player.rb.to_world, player.collide_front.to_world );
+ m3x3_copy( player.rb.to_world, player.collide_back.to_world );
+
+ float h0 = 0.3f,
+ h1 = 0.9f;
+
+ m4x3_mulv( player.rb.to_world, (v3f){0.0f,h0,0.0f}, rbf->co );
+ v3_copy( rbf->co, rbf->to_world[3] );
+ m4x3_mulv( player.rb.to_world, (v3f){0.0f,h1,0.0f}, rbb->co );
+ 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 );
+
+ rb_debug( rbf, 0xff0000ff );
+ rb_debug( rbb, 0xff0000ff );
+
+ rb_ct manifold[64];
+ int 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 j=0; j<5; j++ )
+ {
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &manifold[i];
+
+ /*normal */
+ float vn = -v3_dot( player.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, player.rb.v, player.rb.v );
+
+ /* friction */
+ for( int j=0; j<2; j++ )
+ {
+ float f = k_friction * ct->norm_impulse,
+ vt = v3_dot( player.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( player.rb.v, ct->t[j], lambda, player.rb.v );
+ }
+ }
+ }
+
+ v3_zero( player.rb.w );
+ q_axis_angle( player.rb.q, (v3f){0.0f,1.0f,0.0f}, -player.angles[0] );
+
+ v3f forward_dir = { sinf(player.angles[0]),0.0f,-cosf(player.angles[0]) };
+
+ v3f p1;
+ v3_muladds( player.rb.co, forward_dir, 2.0f, p1 );
+ vg_line( player.rb.co, p1, 0xff0000ff );
+
+ float move_dead = 0.1f,
+ move = vg_get_axis("grabr")*0.5f + 0.5f - move_dead;
+
+ if( move > 0.0f )
+ {
+ float move_norm = move * (1.0f/(1.0f-move_dead)),
+ speed = vg_lerpf( 0.1f*k_runspeed, k_runspeed, move_norm ),
+ amt = k_walk_accel * ktimestep,
+ zvel = v3_dot( player.rb.v, forward_dir ),
+ new_vel = vg_minf( zvel + amt, speed ),
+ diff = new_vel - vg_minf( zvel, speed );
+
+ v3_muladds( player.rb.v, forward_dir, diff, player.rb.v );
+
+ /* TODO move */
+ float walk_norm = (float)player.mdl.anim_walk->length / 30.0f,
+ run_norm = (float)player.mdl.anim_run->length / 30.0f;
+
+ player.walk_timer += ktimestep * vg_lerpf( walk_norm,run_norm,move_norm );
+ }
+ else
+ {
+ player.walk_timer = 0.0f;
+ }
+
+ player.rb.v[0] *= 1.0f - (ktimestep*k_walk_friction);
+ player.rb.v[2] *= 1.0f - (ktimestep*k_walk_friction);
+}
+