traffic.h

   1 #ifndef TRAFFIC_H
   2 #define TRAFFIC_H
   3
   4 #include "common.h"
   5 #include "model.h"
   6 #include "rigidbody.h"
   7
   8 typedef struct traffic_node traffic_node;
   9 typedef struct traffic_driver traffic_driver;
  10
  11 struct traffic_node
  12 {
  13    v3f co, h;
  14
  15    union
  16    {
  17       struct{ traffic_node *next, *next1; };
  18       struct{ mdl_node *mn_next, *mn_next1; };
  19    };
  20 };
  21
  22 struct traffic_driver
  23 {
  24    m4x3f transform;
  25
  26    traffic_node *current;
  27    int option;
  28    float t, speed;
  29 };
  30
  31 static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p )
  32 {
  33    float tt = t*t,
  34          ttt = tt*t;
  35
  36    v3_muls( p1, ttt, p );
  37    v3_muladds( p, h1, 3.0f*tt  -3.0f*ttt, p );
  38    v3_muladds( p, h0, 3.0f*ttt -6.0f*tt  +3.0f*t, p );
  39    v3_muladds( p, p0, 3.0f*tt  -ttt -3.0f*t +1.0f, p );
  40 }
  41
  42 static float eval_bezier_length( v3f p0, v3f p1, v3f h0, v3f h1, int res )
  43 {
  44    float length = 0.0f, m = 1.0f/(float)res;
  45    v3f l, p;
  46    v3_copy( p0, l );
  47
  48    for( int i=0; i<res; i++ )
  49    {
  50       float t = (float)(i+1)*m;
  51       eval_bezier_time(p0,p1,h0,h1,t,p);
  52       length += v3_dist( p,l );
  53       v3_copy( p, l );
  54    }
  55
  56    return length;
  57 }
  58
  59 static void traffic_finalize( traffic_node *system, int count )
  60 {
  61    for( int i=0; i<count; i++ )
  62    {
  63       traffic_node *tn = &system[i];
  64
  65       if( tn->mn_next )
  66          tn->next = &system[ tn->mn_next->sub_uid ];
  67       if( tn->mn_next1 )
  68          tn->next1 = &system[ tn->mn_next1->sub_uid ];
  69    }
  70 }
  71
  72 static void traffic_visualize_link( traffic_node *ta, traffic_node *tb )
  73 {
  74    v3f p0, p1, h0, h1, p, l;
  75
  76    if( !tb ) return;
  77
  78    v3_copy( ta->co, p0 );
  79    v3_muladds( ta->co, ta->h,  1.0f, h0 );
  80    v3_copy( tb->co, p1 );
  81    v3_muladds( tb->co, tb->h, -1.0f, h1 );
  82    v3_copy( p0, l );
  83
  84    vg_line_pt3( h0, 0.2f, 0xff00ff00 );
  85    vg_line_pt3( h1, 0.2f, 0xffff00ff );
  86    vg_line( p0, h0, 0xff000000 );
  87    vg_line( p1, h1, 0xff000000 );
  88
  89    for( int i=0; i<5; i++ )
  90    {
  91       float t = (float)(i+1)/5.0f;
  92       eval_bezier_time( p0, p1, h0, h1, t, p );
  93
  94       vg_line( p, l, 0xffffffff );
  95       v3_copy( p, l );
  96    }
  97 }
  98
  99 static void sample_wheel_floor( v3f pos )
 100 {
 101    v3f ground;
 102    v3_copy( pos, ground );
 103    ground[1] += 4.0f;
 104
 105    ray_hit hit;
 106    hit.dist = 8.0f;
 107
 108    if( ray_world( ground, (v3f){0.0f,-1.0f,0.0f}, &hit ))
 109    {
 110       v3_copy( hit.pos, pos );
 111    }
 112 }
 113
 114 static void traffic_drive( traffic_driver *driver )
 115 {
 116    traffic_node *next, *current = driver->current;
 117
 118    if( !current ) return;
 119    next = driver->option==0? current->next: current->next1;
 120
 121    if( driver->t > 1.0f )
 122    {
 123       driver->t = driver->t - floorf( driver->t );
 124       driver->current = driver->option==0? current->next: current->next1;
 125       driver->option = 0;
 126
 127       current = driver->current;
 128       if( !current )
 129          return;
 130
 131       if( current->next && current->next1 )
 132          if( vg_randf() > 0.5f )
 133             driver->option = 1;
 134    }
 135
 136    traffic_visualize_link( current, next );
 137
 138    /*
 139     * Calculate the speed of the curve at the current point. On the reference
 140     * curve the rate should come out to be exactly 1 ktimestep traveled.
 141     * Dividing this distance by ktimestep gives us the modifier to use.
 142     */
 143    v3f p0,p1,h0,h1,pc,pn;
 144
 145    v3_copy( current->co, p0 );
 146    v3_muladds( current->co, current->h, 1.0f, h0 );
 147    v3_copy( next->co, p1 );
 148    v3_muladds( next->co, next->h, -1.0f, h1 );
 149
 150    eval_bezier_time( p0,p1,h0,h1, driver->t, pc );
 151    eval_bezier_time( p0,p1,h0,h1, driver->t + ktimestep, pn );
 152
 153    float mod = ktimestep / v3_dist( pc, pn );
 154    v3f dir,side,up;
 155    v3_sub( pn, pc, dir );
 156    v3_normalize(dir);
 157
 158    /*
 159     * Stick the car on the ground by casting rays where the wheels are
 160     */
 161    side[0] = -dir[2];
 162    side[1] =  0.0f;
 163    side[2] =  dir[0];
 164    v3_normalize(side);
 165
 166    v3f fl, fr, bc;
 167    v3_muladds( pc, dir, 2.0f, fr );
 168    v3_muladds( pc, dir, 2.0f, fl );
 169    v3_muladds( pc, dir, -2.0f, bc );
 170    v3_muladds( fr, side, 1.0f, fr );
 171    v3_muladds( fl, side, -1.0f, fl );
 172
 173    sample_wheel_floor( fl );
 174    sample_wheel_floor( fr );
 175    sample_wheel_floor( bc );
 176
 177    vg_line( fl, fr, 0xff00ffff );
 178    vg_line( fr, bc, 0xff00ffff );
 179    vg_line( bc, fl, 0xff00ffff );
 180
 181    v3f norm;
 182    v3f v0, v1;
 183    v3_sub( fr, fl, v0 );
 184    v3_sub( bc, fl, v1 );
 185    v3_cross( v1, v0, norm );
 186    v3_normalize( norm );
 187
 188    /*
 189     * Jesus take the wheel
 190     */
 191    float steer_penalty = 1.0f-v3_dot( dir, driver->transform[0] );
 192    steer_penalty /= ktimestep;
 193    steer_penalty *= 30.0f;
 194
 195    float target_speed = vg_maxf( 16.0f * (1.0f-steer_penalty), 0.1f ),
 196          accel = target_speed - driver->speed;
 197    driver->speed = stable_force( driver->speed, accel*ktimestep*2.0f );
 198    driver->t += driver->speed*mod*ktimestep;
 199
 200    /*
 201     * Update transform
 202     */
 203    v3_cross( dir, norm, side );
 204    v3_copy( dir, driver->transform[0] );
 205    v3_copy( norm, driver->transform[1] );
 206    v3_copy( side, driver->transform[2] );
 207
 208    v3_add( fl, fr, pc );
 209    v3_add( bc, pc, pc );
 210    v3_muls( pc, 1.0f/3.0f, pc );
 211    v3_copy( pc, driver->transform[3] );
 212 }
 213
 214 static void traffic_visualize( traffic_node *system, int count )
 215 {
 216    for( int i=0; i<count; i++ )
 217    {
 218       traffic_node *tn = &system[i];
 219
 220       traffic_visualize_link( tn, tn->next );
 221       traffic_visualize_link( tn, tn->next1 );
 222    }
 223 }
 224
 225 static void traffic_visualize_car( traffic_driver *driver )
 226 {
 227    vg_line_boxf_transformed( driver->transform,
 228                                        (boxf){{-1.0f,0.0f,-0.5f},
 229                                               { 1.0f,0.0f, 0.5f}}, 0xff00ff00 );
 230 }
 231
 232 #endif /* TRAFFIC_H */