From 303d950e912d2e6a4d954a0d3e81315976ceeb20 Mon Sep 17 00:00:00 2001
From: Johannes Reinhardt <jreinhardt@jr-thinkpad.(none)>
Date: Sun, 13 Jul 2008 21:51:00 +0200
Subject: [PATCH] fooling around and missing the bugs

---
 src/lookahead.c  | 16 +++++++---------
 src/radar_main.c |  5 ++++-
 2 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/src/lookahead.c b/src/lookahead.c
index 03f58b2..bfe32c3 100644
--- a/src/lookahead.c
+++ b/src/lookahead.c
@@ -62,22 +62,18 @@ void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
 	m->aw = 2*(w2 - w1)/(dt1 + dt2);
 }
 
-int dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){
+int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){
 	double a,k,aw;
 	double wsoll;
 	vehicle tmp = *before;
 	vehicle diff;
-	timestamp dt = 10;
+	timestamp dt = h;
 	int i,end=deltat/dt;
 	struct collcheck collision;
 
 	if(deltat < dt)
 		return 0;
 
-	if((deltat/dt)*dt != deltat)
-		fprintf(stderr,"deltat not a multiple of %d, possibly inaccurate results\n",dt);
-	
-	/*TODO: find out a,k,aw*/
 	a = t->map.a;
 	k = t->map.k;
 	aw = t->map.aw;
@@ -85,12 +81,13 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){
 	wsoll = turn2w(t,before->turn);
 	aw *= (before->w > wsoll) ? -1 : 1;
 
-	for(i = 0;i<end;i++){
+	for(i = 0;i<=end;i++){
+		if(i == end) dt = deltat - (end-1)*dt;
 		
 		/*calculate derivative*/
 		diff.x = cos(tmp.dir)*tmp.speed/1000;
 		diff.y = sin(tmp.dir)*tmp.speed/1000;
-		diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed);
+		diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed*1e-6);
 		diff.w = (tmp.w == wsoll) ? 0 : aw;
 		diff.dir = tmp.w;
 		/*Euler Step*/
@@ -98,7 +95,7 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){
 		tmp.y += dt*diff.y;
 		tmp.speed += (tmp.speed > 0) ? dt*diff.speed : 0;
 		tmp.w += dt*diff.w;
-		tmp.dir = dt*diff.dir;
+		tmp.dir += dt*diff.dir;
 
 		/*check for collisions TODO: optimize*/
 		collision.x = tmp.x;
@@ -111,6 +108,7 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){
 		
 	}
 
+
 	after->x = tmp.x;
 	after->y = tmp.y;
 	after->speed = tmp.speed;
diff --git a/src/radar_main.c b/src/radar_main.c
index 8c28205..9c19fcc 100644
--- a/src/radar_main.c
+++ b/src/radar_main.c
@@ -31,8 +31,11 @@ void trial_loop(trial *t) {
 
 		}
 		while (t->alive) {
+			timestamp step=getcurts(t) + LATENCY - t->sim.tm.ts;
+			
 			t->sim.tm.vehicle.w = turn2w(t->sim.tm.vehicle.turn);
-			dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,getcurts(t) + LATENCY - t->sim.tm.ts);
+
+			dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,step);
 			t->sim.steps++;
 //			simulate(t, getcurts(t) + LATENCY);
 			goradar(t);