|
|
@ -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*/ |
|
|
|