--- /dev/null
+#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; i<res; i++ )
+ {
+ float t = (float)(i+1)*m;
+ eval_bezier_time(p0,p1,h0,h1,t,p);
+ length += v3_dist( p,l );
+ v3_copy( p, l );
+ }
+
+ return length;
+}
+
+static void traffic_finalize( traffic_node *system, int count )
+{
+ for( int i=0; i<count; i++ )
+ {
+ traffic_node *tn = &system[i];
+
+ if( tn->mn_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; i<count; i++ )
+ {
+ traffic_node *tn = &system[i];
+
+ traffic_visualize_link( tn, tn->next );
+ 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 */
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff