+ v3_muladds( player.rb.v, player.rb.up,
+ -k_downforce*ktimestep, player.rb.v );
+ player_physics_control();
+ }
+ else
+ {
+ player_physics_control_air();
+ }
+}
+
+static void player_do_motion(void)
+{
+ float horizontal = vg_get_axis("horizontal"),
+ vertical = vg_get_axis("vertical");
+
+ player_physics();
+
+ /* Integrate velocity */
+ v3f prevco;
+ v3_copy( player.rb.co, prevco );
+
+ 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.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.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.rb.q, player.rb.q );
+ }
+
+ /* Faux angular velocity */
+ v4f rotate;
+
+ static float siY = 0.0f;
+ float lerpq = player.in_air? 0.04f: 0.3f;
+ siY = vg_lerpf( siY, player.iY, lerpq );
+
+ q_axis_angle( rotate, player.rb.up, siY );
+ q_mul( rotate, player.rb.q, player.rb.q );
+ player.iY = 0.0f;
+
+ /*
+ * Gate intersection, by tracing a line over the gate planes
+ */
+ for( int i=0; i<world.gate_count; i++ )
+ {
+ teleport_gate *gate = &world.gates[i];
+
+ if( gate_intersect( gate, player.rb.co, prevco ) )
+ {
+ 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 );
+ m3x3_mulv( gate->transport, player.bob, player.bob );
+
+ v4f transport_rotation;
+ m3x3_q( gate->transport, transport_rotation );
+ q_mul( transport_rotation, player.rb.q, player.rb.q );
+
+ break;
+ }
+ }
+
+ rb_update_transform( &player.rb );
+}
+
+/*
+ * Walkgrid implementation,
+ * loosely based of cmuratoris youtube video 'Killing the Walkmonster'
+ */
+
+#define WALKGRID_SIZE 16
+struct walkgrid
+{
+ struct grid_sample
+ {
+ enum sample_type
+ {
+ k_sample_type_air, /* Nothing was hit. */
+ k_sample_type_invalid, /* The point is invalid, but there is a sample
+ underneath that can be used */
+ k_sample_type_valid, /* This point is good */
+ }
+ type;
+
+ v3f clip[2];
+ v3f pos;
+
+ enum traverse_state
+ {
+ k_traverse_none = 0x00,
+ k_traverse_h = 0x01,
+ k_traverse_v = 0x02
+ }
+ state;
+ }
+ samples[WALKGRID_SIZE][WALKGRID_SIZE];
+
+ boxf region;
+
+ float move; /* Current amount of movement we have left to apply */
+ v2f dir; /* The movement delta */
+ v2i cell_id;/* Current cell */
+ v2f pos; /* Local position (in cell) */
+ float h;
+};
+
+static int player_walkgrid_tri_walkable( u32 tri[3] )
+{
+ return tri[0] > world.sm_geo_std_oob.vertex_count;
+}
+
+/*
+ * Get a sample at this pole location, will return 1 if the sample is valid,
+ * and pos will be updated to be the intersection location.
+ */
+static void player_walkgrid_samplepole( struct grid_sample *s )
+{
+ boxf region = {{ s->pos[0] -0.01f, s->pos[1] - 4.0f, s->pos[2] -0.01f},
+ { s->pos[0] +0.01f, s->pos[1] + 4.0f, s->pos[2] +0.01f}};
+
+ u32 geo[256];
+ v3f tri[3];
+ int len = bh_select( &world.geo.bhtris, region, geo, 256 );
+
+ const float k_minworld_y = -2000.0f;
+
+ float walk_height = k_minworld_y,
+ block_height = k_minworld_y;
+
+ s->type = k_sample_type_air;
+
+ 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] );
+
+ v3f vdown = {0.0f,-1.0f,0.0f};
+ v3f sample_from;
+ v3_copy( s->pos, sample_from );
+ sample_from[1] = region[1][1];
+
+ float dist;
+ if( ray_tri( tri, sample_from, vdown, &dist ))
+ {
+ v3f p0;
+ v3_muladds( sample_from, vdown, dist, p0 );
+
+ if( player_walkgrid_tri_walkable(ptri) )
+ {
+ if( p0[1] > walk_height )
+ {
+ walk_height = p0[1];
+ }
+ }
+ else
+ {
+ if( p0[1] > block_height )
+ block_height = p0[1];
+ }
+ }
+ }
+
+ s->pos[1] = walk_height;
+
+ if( walk_height > k_minworld_y )
+ if( block_height > walk_height )
+ s->type = k_sample_type_invalid;
+ else
+ s->type = k_sample_type_valid;
+ else
+ s->type = k_sample_type_air;
+}
+
+float const k_gridscale = 0.5f;
+
+enum eclipdir
+{
+ k_eclipdir_h = 0,
+ k_eclipdir_v = 1
+};
+
+static void player_walkgrid_clip_blocker( struct grid_sample *sa,
+ struct grid_sample *sb,
+ struct grid_sample *st,
+ enum eclipdir dir )
+{
+ v3f clipdir, pos;
+ int valid_a = sa->type == k_sample_type_valid,
+ valid_b = sb->type == k_sample_type_valid;
+ struct grid_sample *target = valid_a? sa: sb,
+ *other = valid_a? sb: sa;
+ v3_copy( target->pos, pos );
+ v3_sub( other->pos, target->pos, clipdir );
+
+ boxf cell_region;
+ v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, -k_gridscale*2.1f, cell_region[0]);
+ v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, k_gridscale*2.1f, cell_region[1]);
+
+ u32 geo[256];
+ v3f tri[3];
+ int len = bh_select( &world.geo.bhtris, cell_region, geo, 256 );
+
+ float start_time = v3_length( clipdir ),
+ min_time = start_time;
+ v3_normalize( clipdir );
+ v3_muls( clipdir, 0.0001f, st->clip[dir] );
+
+ 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] );
+
+ if( player_walkgrid_tri_walkable(ptri) )
+ continue;
+
+ float dist;
+ if(ray_tri( tri, pos, clipdir, &dist ))
+ {
+ if( dist > 0.0f && dist < min_time )
+ {
+ min_time = dist;
+ sb->type = k_sample_type_air;
+ }
+ }
+ }
+
+ if( !(min_time < start_time) )
+ min_time = 0.5f * k_gridscale;
+
+ min_time = vg_clampf( min_time/k_gridscale, 0.01f, 0.99f );
+
+ v3_muls( clipdir, min_time, st->clip[dir] );
+
+ v3f p0;
+ v3_muladds( target->pos, st->clip[dir], k_gridscale, p0 );
+}
+
+static void player_walkgrid_clip_edge( struct grid_sample *sa,
+ struct grid_sample *sb,
+ struct grid_sample *st, /* data store */
+ enum eclipdir dir )
+{
+ v3f clipdir = { 0.0f, 0.0f, 0.0f }, pos;
+ int valid_a = sa->type == k_sample_type_valid,
+ valid_b = sb->type == k_sample_type_valid;
+
+ struct grid_sample *target = valid_a? sa: sb,
+ *other = valid_a? sb: sa;
+
+ v3_sub( other->pos, target->pos, clipdir );
+ clipdir[1] = 0.0f;
+
+ v3_copy( target->pos, pos );
+
+ boxf cell_region;
+ v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, -k_gridscale*1.1f, cell_region[0]);
+ v3_muladds( pos, (v3f){1.0f,1.0f,1.0f}, k_gridscale*1.1f, cell_region[1]);
+
+ u32 geo[256];
+ int len = bh_select( &world.geo.bhtris, cell_region, geo, 256 );
+
+ float max_dist = 0.0f;
+ v3f tri[3];
+ v3f perp;
+ v3_cross( clipdir,(v3f){0.0f,1.0f,0.0f},perp );
+ v3_muls( clipdir, 0.001f, st->clip[dir] );
+
+ 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] );
+
+ if( !player_walkgrid_tri_walkable(ptri) )
+ continue;
+
+ for( int k=0; k<3; k++ )
+ {
+ int ia = k,
+ ib = (k+1)%3;
+
+ v3f v0, v1;
+ v3_sub( tri[ia], pos, v0 );
+ v3_sub( tri[ib], pos, v1 );
+
+ if( (clipdir[2]*v0[0] - clipdir[0]*v0[2]) *
+ (clipdir[2]*v1[0] - clipdir[0]*v1[2]) < 0.0f )
+ {
+ float da = v3_dot(v0,perp),
+ db = v3_dot(v1,perp),
+ d = da-db,
+ qa = da/d;
+
+ v3f p0;
+ v3_muls( v1, qa, p0 );
+ v3_muladds( p0, v0, 1.0f-qa, p0 );
+
+ float h = v3_dot(p0,clipdir)/v3_dot(clipdir,clipdir);
+
+ if( h >= max_dist && h <= 1.0f )
+ {
+ max_dist = h;
+ float l = 1.0f/v3_length(clipdir);
+ v3_muls( p0, l, st->clip[dir] );
+ }
+ }
+ }
+ }
+}
+
+static const struct conf