traffic.h

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