server loopback tool
[carveJwlIkooP6JGAAIwe30JlM.git] / 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 */