- 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);