+ 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 );
+
+ /* Do XY translation */
+ v3f walk_apply, walk_measured;
+ v3_zero( walk_apply );
+ v3_muladds( walk_apply, right_dir, walk[0], walk_apply );
+ v3_muladds( walk_apply, forward_dir, walk[1], walk_apply );
+ v3_add( walk_apply, phys->rb.co, phys->rb.co );
+
+ /* Directly resolve collisions */
+ player_walk_collider_configuration();
+ len = player_update_collision_manifold( manifold );
+
+ v3f dt;
+ v3_zero( dt );
+ for( int j=0; j<8; j++ )
+ {
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &manifold[i];
+
+ float resolved_amt = v3_dot( ct->n, dt ),
+ remaining = (ct->p-k_penetration_slop) - resolved_amt,
+ apply = vg_maxf( remaining, 0.0f ) * 0.3f;
+
+ v3_muladds( dt, ct->n, apply, dt );
+ }
+ }
+ v3_add( dt, phys->rb.co, phys->rb.co );
+
+ v3_add( dt, walk_apply, walk_measured );
+ v3_divs( walk_measured, VG_TIMESTEP_FIXED, phys->rb.v );
+
+ if( len )
+ {
+ struct world_material *surface_mat = world_contact_material(manifold);
+ player.surface_prop = surface_mat->info.surface_prop;
+ }
+
+ /* jump */
+ if( player.input_jump->button.value )
+ {
+ phys->rb.v[1] = 5.0f;
+ phys->in_air = 1;
+ return;
+ }
+
+ /* Check if grounded by current manifold */
+ phys->in_air = 1;
+ for( int i=0; i<len; i++ )
+ {
+ struct contact *ct = &manifold[i];
+ if( player_walk_surface_standable( ct->n ) )
+ phys->in_air = 0;
+ }
+
+ /* otherwise... */
+ if( phys->in_air )
+ player_walk_stepdown();
+ }
+}
+
+VG_STATIC void player_grind(void)
+{
+ struct player_phys *phys = &player.phys;
+
+ v3f closest;
+ int idx = bh_closest_point( world.grind_bh, phys->rb.co, closest, INFINITY );
+ if( idx == -1 )
+ return;
+
+ struct grind_edge *edge = &world.grind_edges[ idx ];
+
+ vg_line( phys->rb.co, closest, 0xff000000 );
+ vg_line_cross( closest, 0xff000000, 0.3f );
+ vg_line( edge->p0, edge->p1, 0xff000000 );
+
+ v3f grind_delta;
+ v3_sub( closest, phys->rb.co, grind_delta );
+
+ float p = v3_dot( phys->rb.forward, grind_delta );
+ v3_muladds( grind_delta, phys->rb.forward, -p, grind_delta );
+
+ float a = vg_maxf( 0.0f, 4.0f-v3_dist2( closest, phys->rb.co ) );
+ v3_muladds( phys->rb.v, grind_delta, a*0.2f, phys->rb.v );
+}
+
+VG_STATIC int player_update_grind_collision( rb_ct *contact )
+{
+ struct player_phys *phys = &player.phys;
+
+ v3f p0, p1, c0, c1;
+ v3_muladds( phys->rb.co, phys->rb.forward, 0.5f, p0 );
+ v3_muladds( phys->rb.co, phys->rb.forward, -0.5f, p1 );
+ v3_muladds( p0, phys->rb.up, 0.125f-0.15f, p0 );
+ v3_muladds( p1, phys->rb.up, 0.125f-0.15f, p1 );
+
+ float const k_r = 0.25f;
+ struct grind_edge *closest_edge = player_grind_collect_edge( p0, p1,
+ c0, c1, k_r );
+
+
+#if 0
+ vg_line( p0, p1, 0xff0000ff );
+#endif
+
+ if( closest_edge )
+ {
+#if 0
+ vg_line_cross( c0, 0xff000000, 0.1f );
+ vg_line_cross( c1, 0xff000000, 0.1f );
+ vg_line( c0, c1, 0xff000000 );
+#endif
+
+ v3f delta;
+ v3_sub( c1, c0, delta );
+
+ if( v3_dot( delta, phys->rb.up ) > 0.0001f )
+ {
+ contact->p = v3_length( delta );
+ contact->type = k_contact_type_edge;
+ contact->element_id = 0;
+ v3_copy( c1, contact->co );
+ contact->rba = &player.phys.rb;
+ contact->rbb = &world.rb_geo;
+
+ v3f edge_dir, axis_dir;
+ v3_sub( closest_edge->p1, closest_edge->p0, edge_dir );
+ v3_normalize( edge_dir );
+ v3_cross( (v3f){0.0f,1.0f,0.0f}, edge_dir, axis_dir );
+ v3_cross( edge_dir, axis_dir, contact->n );
+
+#if 0
+ vg_info( "%f %f\n", v3_length( contact->n ), contact->p );
+#endif
+
+ return 1;
+ }
+ else
+ return -1;
+ }
+
+ return 0;
+}
+
+/* Manifold must be able to hold at least 64 elements */
+VG_STATIC int player_update_collision_manifold( rb_ct *manifold )
+{
+ struct player_phys *phys = &player.phys;
+
+ rigidbody *rbf = &player.collide_front,
+ *rbb = &player.collide_back;
+
+ rb_debug( rbf, 0xff00ffff );
+ rb_debug( rbb, 0xffffff00 );
+
+
+#if 0
+ phys->rise = vg_lerpf( phys->rise, phys->in_air? -0.25f: 0.0f,
+ VG_TIMESTEP_FIXED );
+#endif
+
+ int len_f = 0,
+ len_b = 0;
+
+ len_f = rb_sphere_scene( rbf, &world.rb_geo, manifold );
+ rb_manifold_filter_coplanar( manifold, len_f, 0.05f );
+ if( len_f > 1 )
+ {
+ rb_manifold_filter_backface( manifold, len_f );
+ rb_manifold_filter_joint_edges( manifold, len_f, 0.05f );
+ rb_manifold_filter_pairs( manifold, len_f, 0.05f );
+ }
+ int new_len_f = rb_manifold_apply_filtered( manifold, len_f );
+ if( len_f && !new_len_f )
+ len_f = 1;
+ else
+ len_f = new_len_f;
+
+ rb_ct *man_b = &manifold[len_f];
+ len_b = rb_sphere_scene( rbb, &world.rb_geo, man_b );
+ rb_manifold_filter_coplanar( man_b, len_b, 0.05f );
+ if( len_b > 1 )
+ {
+ rb_manifold_filter_backface( man_b, len_b );
+ rb_manifold_filter_joint_edges( man_b, len_b, 0.05f );
+ rb_manifold_filter_pairs( man_b, len_b, 0.05f );
+ }
+ int new_len_b = rb_manifold_apply_filtered( man_b, len_b );
+ if( len_b && !new_len_b )
+ len_b = 1;
+ else
+ len_b = new_len_b;
+
+ return len_f + len_b;
+}
+
+VG_STATIC void player_adhere_ground( rb_ct *manifold, int len )
+{
+ struct player_phys *phys = &player.phys;
+ int was_in_air = phys->in_air;
+
+ v3f surface_avg;
+ v3_zero( surface_avg );
+
+ /*
+ *
+ * EXPERIMENTAL
+ * ================================================================
+ */
+ if( phys->in_air )
+ player.normal_pressure = 0.0f;
+ else
+ player.normal_pressure = v3_dot( phys->rb.up, phys->rb.v );
+
+ v3f p0_0, p0_1,
+ p1_0, p1_1,
+ n0, n1;
+ float t0, t1;
+
+ float mod = 0.7f * player.input_grab->axis.value + 0.3f,
+ spring_k = mod * k_spring_force,
+ damp_K = mod * k_spring_dampener,
+ disp_k = 0.4f;
+
+ v3_copy( player.collide_front.co, p0_0 );
+ v3_copy( player.collide_back.co, p1_0 );
+
+ v3_muladds( p0_0, phys->rb.up, -disp_k, p0_1 );
+ v3_muladds( p1_0, phys->rb.up, -disp_k, p1_1 );
+
+ int cast0 = spherecast_world( p0_0, p0_1, 0.2f, &t0, n0 ),
+ cast1 = spherecast_world( p1_0, p1_1, 0.2f, &t1, n1 );
+
+ v3f animp0, animp1;
+
+ m4x3f temp;
+ m3x3_copy( phys->rb.to_world, temp );
+ if( cast0 != -1 )
+ {
+ v3_lerp( p0_0, p0_1, t0, temp[3] );
+ v3_copy( temp[3], animp0 );
+ debug_sphere( temp, 0.2f, VG__PINK );
+
+ v3f F, delta;
+ v3_sub( p0_0, phys->rb.co, delta );
+
+ float displacement = vg_clampf( 1.0f-t0, 0.0f, 1.0f ),
+ damp = vg_maxf( 0.0f, v3_dot( phys->rb.up, phys->rb.v ) );
+ v3_muls( phys->rb.up, displacement*spring_k*k_rb_delta -
+ damp*damp_K*k_rb_delta, F );
+
+ v3_muladds( phys->rb.v, F, 1.0f, phys->rb.v );
+
+ /* Angular velocity */
+ v3f wa;
+ v3_cross( delta, F, wa );
+ v3_muladds( phys->rb.w, wa, k_spring_angular, phys->rb.w );