final fix

src/lookahead.c

@@ -126,24 +126,29 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat
 	k = t->map.k;
 
 	wsoll = turn2w(t,before->turn);
+	aw = t->map.aw * ((before->w > wsoll) ? -1 : 1);
+	if (before->w == wsoll) aw = 0;
 
 	for(i = 0;i<=end;i++){
 		if(i == end) dt = deltat - end*dt;
-		aw = t->map.aw * ((before->w > wsoll) ? 1 : -1);
 		
 		/*calculate derivative*/
 		diff.x = cos(tmp.dir*M_PI/180)*tmp.speed/1000;
 		diff.y = sin(tmp.dir*M_PI/180)*tmp.speed/1000;
 		diff.speed = a - k*tmp.speed*tmp.speed;
 // 		diff.speed = fmax(-tmp.speed/1000,a - k*tmp.speed*tmp.speed*1e-6);
-		diff.w = (tmp.w == wsoll) ? 0 : aw;
+		diff.w = aw;
 		diff.dir = tmp.w;
 		/*Euler Step*/
 		tmp.x += dt*diff.x;
 		tmp.y += dt*diff.y;
 // 		tmp.speed += (tmp.speed > 0) ? 1000*dt*diff.speed : 0;
 		tmp.speed = fmax(0, tmp.speed + dt*diff.speed/1000);
-		tmp.w += dt*diff.w;
+		if (aw > 0) {
+			tmp.w = fmin(wsoll, tmp.w + dt*diff.w);
+		} else if (aw < 0) {
+			tmp.w = fmax(wsoll, tmp.w + dt*diff.w);
+		}
 		tmp.dir += dt*diff.dir;
 
 		/*check for collisions TODO: optimize*/