From 4409ebfd39588d70449517f1cfe35a36f6b823d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BChler?= Date: Mon, 14 Jul 2008 01:18:45 +0200 Subject: [PATCH] final fix --- src/lookahead.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/lookahead.c b/src/lookahead.c index 8eba906..fca2fb9 100644 --- a/src/lookahead.c +++ b/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*/