- rb_init( &player.collide_front );
- rb_init( &player.collide_back );
-}
-
-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;