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