X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;ds=sidebyside;f=traffic.h;fp=traffic.h;h=8850306b450d9aa534c3e1c024c058343190c3b3;hb=6d66c67945f84476d6ac75a0497007cc30bcf58c;hp=0000000000000000000000000000000000000000;hpb=d8b8c566831e15ef061a66409e1219f44a82097a;p=carveJwlIkooP6JGAAIwe30JlM.git diff --git a/traffic.h b/traffic.h new file mode 100644 index 0000000..8850306 --- /dev/null +++ b/traffic.h @@ -0,0 +1,232 @@ +#ifndef TRAFFIC_H +#define TRAFFIC_H + +#include "common.h" +#include "model.h" +#include "rigidbody.h" + +typedef struct traffic_node traffic_node; +typedef struct traffic_driver traffic_driver; + +struct traffic_node +{ + v3f co, h; + + union + { + struct{ traffic_node *next, *next1; }; + struct{ mdl_node *mn_next, *mn_next1; }; + }; +}; + +struct traffic_driver +{ + m4x3f transform; + + traffic_node *current; + int option; + float t, speed; +}; + +static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p ) +{ + float tt = t*t, + ttt = tt*t; + + v3_muls( p1, ttt, p ); + v3_muladds( p, h1, 3.0f*tt -3.0f*ttt, p ); + v3_muladds( p, h0, 3.0f*ttt -6.0f*tt +3.0f*t, p ); + v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p ); +} + +static float eval_bezier_length( v3f p0, v3f p1, v3f h0, v3f h1, int res ) +{ + float length = 0.0f, m = 1.0f/(float)res; + v3f l, p; + v3_copy( p0, l ); + + for( int i=0; imn_next ) + tn->next = &system[ tn->mn_next->sub_uid ]; + if( tn->mn_next1 ) + tn->next1 = &system[ tn->mn_next1->sub_uid ]; + } +} + +static void traffic_visualize_link( traffic_node *ta, traffic_node *tb ) +{ + v3f p0, p1, h0, h1, p, l; + + if( !tb ) return; + + v3_copy( ta->co, p0 ); + v3_muladds( ta->co, ta->h, 1.0f, h0 ); + v3_copy( tb->co, p1 ); + v3_muladds( tb->co, tb->h, -1.0f, h1 ); + v3_copy( p0, l ); + + vg_line_pt3( h0, 0.2f, 0xff00ff00 ); + vg_line_pt3( h1, 0.2f, 0xffff00ff ); + vg_line( p0, h0, 0xff000000 ); + vg_line( p1, h1, 0xff000000 ); + + for( int i=0; i<5; i++ ) + { + float t = (float)(i+1)/5.0f; + eval_bezier_time( p0, p1, h0, h1, t, p ); + + vg_line( p, l, 0xffffffff ); + v3_copy( p, l ); + } +} + +static void sample_wheel_floor( v3f pos ) +{ + v3f ground; + v3_copy( pos, ground ); + ground[1] += 4.0f; + + ray_hit hit; + hit.dist = 8.0f; + + if( ray_world( ground, (v3f){0.0f,-1.0f,0.0f}, &hit )) + { + v3_copy( hit.pos, pos ); + } +} + +static void traffic_drive( traffic_driver *driver ) +{ + traffic_node *next, *current = driver->current; + + if( !current ) return; + next = driver->option==0? current->next: current->next1; + + if( driver->t > 1.0f ) + { + driver->t = driver->t - floorf( driver->t ); + driver->current = driver->option==0? current->next: current->next1; + driver->option = 0; + + current = driver->current; + if( !current ) + return; + + if( current->next && current->next1 ) + if( vg_randf() > 0.5f ) + driver->option = 1; + } + + traffic_visualize_link( current, next ); + + /* + * Calculate the speed of the curve at the current point. On the reference + * curve the rate should come out to be exactly 1 ktimestep traveled. + * Dividing this distance by ktimestep gives us the modifier to use. + */ + v3f p0,p1,h0,h1,pc,pn; + + v3_copy( current->co, p0 ); + v3_muladds( current->co, current->h, 1.0f, h0 ); + v3_copy( next->co, p1 ); + v3_muladds( next->co, next->h, -1.0f, h1 ); + + eval_bezier_time( p0,p1,h0,h1, driver->t, pc ); + eval_bezier_time( p0,p1,h0,h1, driver->t + ktimestep, pn ); + + float mod = ktimestep / v3_dist( pc, pn ); + v3f dir,side,up; + v3_sub( pn, pc, dir ); + v3_normalize(dir); + + /* + * Stick the car on the ground by casting rays where the wheels are + */ + side[0] = -dir[2]; + side[1] = 0.0f; + side[2] = dir[0]; + v3_normalize(side); + + v3f fl, fr, bc; + v3_muladds( pc, dir, 2.0f, fr ); + v3_muladds( pc, dir, 2.0f, fl ); + v3_muladds( pc, dir, -2.0f, bc ); + v3_muladds( fr, side, 1.0f, fr ); + v3_muladds( fl, side, -1.0f, fl ); + + sample_wheel_floor( fl ); + sample_wheel_floor( fr ); + sample_wheel_floor( bc ); + + vg_line( fl, fr, 0xff00ffff ); + vg_line( fr, bc, 0xff00ffff ); + vg_line( bc, fl, 0xff00ffff ); + + v3f norm; + v3f v0, v1; + v3_sub( fr, fl, v0 ); + v3_sub( bc, fl, v1 ); + v3_cross( v1, v0, norm ); + v3_normalize( norm ); + + /* + * Jesus take the wheel + */ + float steer_penalty = 1.0f-v3_dot( dir, driver->transform[0] ); + steer_penalty /= ktimestep; + steer_penalty *= 30.0f; + + float target_speed = vg_maxf( 16.0f * (1.0f-steer_penalty), 0.1f ), + accel = target_speed - driver->speed; + driver->speed = stable_force( driver->speed, accel*ktimestep*2.0f ); + driver->t += driver->speed*mod*ktimestep; + + /* + * Update transform + */ + v3_cross( dir, norm, side ); + v3_copy( dir, driver->transform[0] ); + v3_copy( norm, driver->transform[1] ); + v3_copy( side, driver->transform[2] ); + + v3_add( fl, fr, pc ); + v3_add( bc, pc, pc ); + v3_muls( pc, 1.0f/3.0f, pc ); + v3_copy( pc, driver->transform[3] ); +} + +static void traffic_visualize( traffic_node *system, int count ) +{ + for( int i=0; inext ); + traffic_visualize_link( tn, tn->next1 ); + } +} + +static void traffic_visualize_car( traffic_driver *driver ) +{ + vg_line_boxf_transformed( driver->transform, + (boxf){{-1.0f,0.0f,-0.5f}, + { 1.0f,0.0f, 0.5f}}, 0xff00ff00 ); +} + +#endif /* TRAFFIC_H */