From 185ff7755bb44b98fe502b1b1c54d3870c45dfa1 Mon Sep 17 00:00:00 2001 From: hgn Date: Tue, 7 Feb 2023 19:38:16 +0000 Subject: [PATCH] basic 5050 --- common.h | 8 +- player_skate.c | 226 ++++++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 230 insertions(+), 4 deletions(-) diff --git a/common.h b/common.h index 79ea526..6bb6951 100644 --- a/common.h +++ b/common.h @@ -131,7 +131,10 @@ VG_STATIC float k_spring_angular = 1.0f, k_spring_force = 15.0f, - k_spring_dampener = 5.0f; + k_spring_dampener = 5.0f, + + k_grind_spring = 100.0f, + k_grind_dampener = 1.0f; VG_STATIC float @@ -169,6 +172,9 @@ VG_STATIC void common_var_temp(void) VG_VAR_F32( k_cam_damp ); VG_VAR_F32( k_cam_spring ); + VG_VAR_F32( k_grind_dampener ); + VG_VAR_F32( k_grind_spring ); + VG_VAR_F32( k_walkspeed ); VG_VAR_F32( k_stopspeed ); VG_VAR_F32( k_airspeed ); diff --git a/player_skate.c b/player_skate.c index b2d951e..8777763 100644 --- a/player_skate.c +++ b/player_skate.c @@ -35,8 +35,6 @@ VG_STATIC int skate_collide_smooth( player_instance *player, m4x3f mtx, rb_sphere *sphere, rb_ct *man ) { - debug_sphere( mtx, sphere->radius, VG__BLACK ); - int len = 0; len = rb_sphere__scene( mtx, sphere, NULL, &world.rb_geo.inf.scene, man ); @@ -149,6 +147,196 @@ VG_STATIC int skate_grind_collide( player_instance *player, rb_ct *contact ) return 0; } +VG_STATIC int skate_grind_scansq( player_instance *player, v3f ra ) +{ + v3f pos; + m4x3_mulv( player->rb.to_world, ra, pos ); + + v4f plane; + v3_copy( player->rb.to_world[2], plane ); + v3_normalize( plane ); + plane[3] = v3_dot( plane, pos ); + + boxf box; + float r = 0.3f; + v3_add( pos, (v3f){ r, r, r }, box[1] ); + v3_sub( pos, (v3f){ r, r, r }, box[0] ); + +#if 0 + vg_line_boxf( box, VG__BLUE ); +#endif + + m4x3f mtx; + m3x3_copy( player->rb.to_world, mtx ); + v3_copy( pos, mtx[3] ); + + debug_sphere( mtx, r, VG__CYAN ); + + bh_iter it; + bh_iter_init( 0, &it ); + int idx; + + struct grind_sample + { + v2f co; + v2f normal; + v3f normal3; + } + samples[48]; + + int sample_count = 0; + + v2f support_min, + support_max; + + v3f support_axis; + v3_cross( plane, (v3f){0.0f,1.0f,0.0f}, support_axis ); + v3_normalize( support_axis ); + + while( bh_next( world.geo_bh, &it, box, &idx ) ) + { + u32 *ptri = &world.scene_geo->arrindices[ idx*3 ]; + v3f tri[3]; + + for( int j=0; j<3; j++ ) + v3_copy( world.scene_geo->arrvertices[ptri[j]].co, tri[j] ); + + for( int j=0; j<3; j++ ) + { + int i0 = j, + i1 = (j+1) % 3; + + struct grind_sample *sample = &samples[ sample_count ]; + v3f co; + + if( plane_segment( plane, tri[i0], tri[i1], co ) ) + { + v3f d; + v3_sub( co, pos, d ); + if( v3_length2( d ) > r*r ) + continue; + + v3f va, vb, normal; + v3_sub( tri[1], tri[0], va ); + v3_sub( tri[2], tri[0], vb ); + v3_cross( va, vb, normal ); + + sample->normal[0] = v3_dot( support_axis, normal ); + sample->normal[1] = normal[1]; + sample->co[0] = v3_dot( support_axis, d ); + sample->co[1] = d[1]; + + v3_copy( normal, sample->normal3 ); /* normalize later + if we want to us it */ + + v2_normalize( sample->normal ); + sample_count ++; + + if( sample_count == vg_list_size( samples ) ) + { + break; + } + } + } + } + + if( sample_count < 2 ) + return 0; + + v3f average_position, + average_direction; + + v3_zero( average_position ); + v3_zero( average_direction ); + + int passed_samples = 0; + + for( int i=0; ico, sj->co ) <= (0.01f*0.01f) && + v2_dot( si->normal, sj->normal ) < 0.7f ) + { + /* TODO: Filter concave */ + + v3f p0; + v3_muls( support_axis, sj->co[0], p0 ); + p0[1] += sj->co[1]; + + v3_add( average_position, p0, average_position ); + + v3f n0, n1, dir; + v3_copy( si->normal3, n0 ); + v3_copy( sj->normal3, n1 ); + v3_cross( n0, n1, dir ); + v3_normalize( dir ); + + /* make sure the directions all face a common hemisphere */ + v3_muls( dir, vg_signf(v3_dot(dir,plane)), dir ); + + v3_add( average_direction, dir, average_direction ); + passed_samples ++; + } + } + } + + if( !passed_samples ) + return 0; + + float div = 1.0f/(float)passed_samples; + v3_muls( average_position, div, average_position ); + v3_muls( average_direction, div, average_direction ); /* !! not normed */ + + v3_add( pos, average_position, average_position ); + vg_line_pt3( average_position, 0.02f, VG__GREEN ); + + v3f p0, p1; + v3_muladds( average_position, average_direction, 0.35f, p0 ); + v3_muladds( average_position, average_direction, -0.35f, p1 ); + vg_line( p0, p1, VG__PINK ); + + if( passed_samples ) + { + v3f displacement, dir; + v3_sub( pos, average_position, displacement ); + v3_copy( displacement, dir ); + v3_normalize( dir ); + + v3f rv, raW; + q_mulv( player->rb.q, ra, raW ); + + v3_cross( player->rb.w, raW, rv ); + v3_add( player->rb.v, rv, rv ); + + v3_muladds( rv, player->rb.to_world[2], + -v3_dot( rv, player->rb.to_world[2] ), rv ); + + v3f Fd, Fs, F; + v3_muls( displacement, -k_grind_spring, Fs ); + v3_muls( rv, -k_grind_dampener, Fd ); + + v3_add( Fd, Fs, F ); + v3_muls( F, k_rb_delta, F ); + + v3_add( player->rb.v, F, player->rb.v ); + v3f wa; + v3_cross( raW, F, wa ); + v3_add( player->rb.w, wa, player->rb.w ); + + /* Constraint based */ + } +} + /* * * Prediction system @@ -1224,11 +1412,38 @@ VG_STATIC void player__skate_update( player_instance *player ) { w, 0.0f, l }, }; + int wheel_states[] = + { + 1, 1, 1, 1 + }; + + if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f, -l } ) ) + { + wheel_states[0] = 0; + wheel_states[1] = 0; + } + + if( skate_grind_scansq( player, (v3f){ 0.0f, 0.0f, l } ) ) + { + wheel_states[2] = 0; + wheel_states[3] = 0; + } + rb_sphere collider; collider.radius = 0.07f; s->substep = k_rb_delta; + for( int i=0; i<4; i++ ) + { + m4x3f mtx; + m3x3_copy( player->rb.to_world, mtx ); + m4x3_mulv( player->rb.to_world, wheel_positions[i], mtx[3] ); + debug_sphere( mtx, collider.radius, wheel_states[i]? VG__WHITE: + VG__BLACK ); + } + + begin_collision:; #ifdef SKATE_CCD @@ -1257,6 +1472,9 @@ begin_collision:; for( int i=0; i<4; i++ ) { + if( !wheel_states[i] ) + continue; + v3f current, future; q_mulv( future_q, wheel_positions[i], future ); v3_add( future, future_co, future ); @@ -1316,6 +1534,9 @@ begin_collision:; for( int i=0; i<4; i++ ) { + if( !wheel_states[i] ) + continue; + m4x3f mtx; m3x3_identity( mtx ); @@ -1380,7 +1601,6 @@ begin_collision:; #else rb_prepare_contact( &manifold[i] ); #endif - rb_debug_contact( &manifold[i] ); } skate_collision_response( player, manifold, manifold_len ); -- 2.25.1