From 66d5b5f065aba6b79fa3a4ac41619a3d145ecaf9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Stefan=20B=C3=BChler?= <stbuehler@web.de>
Date: Sun, 13 Jul 2008 23:57:10 +0200
Subject: [PATCH] pr0n!

---
 src/control.h    | 14 ++++++++++++++
 src/lookahead.c  | 19 ++++++++++---------
 src/radar.c      |  6 ------
 src/radar_main.c |  1 -
 4 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/src/control.h b/src/control.h
index 8b9ec30..4f46259 100644
--- a/src/control.h
+++ b/src/control.h
@@ -133,6 +133,8 @@ INLINE void vehicle_right(trial *t);
 INLINE void vehicle_hard_right(trial *t);
 
 INLINE timestamp getcurts(trial *t);
+INLINE double fixangle(double a);
+INLINE double fixdirection(double a);
 
 /* needed by parser */
 telemetry* telemetry_new();
@@ -229,4 +231,16 @@ INLINE timestamp getcurts(trial *t) {
 	return ts;
 }
 
+INLINE double fixangle(double a) {
+	while (a < -180) a += 360;
+	while (a > 180) a -= 360;
+	return a;
+}
+
+INLINE double fixdirection(double a) {
+	while (a < 0) a += 360;
+	while (a > 360) a -= 360;
+	return a;
+}
+
 #endif
diff --git a/src/lookahead.c b/src/lookahead.c
index 9c53a09..43d9e73 100644
--- a/src/lookahead.c
+++ b/src/lookahead.c
@@ -49,8 +49,8 @@ void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
 	dt1 = (t2->ts - t1->ts);
 	dt2 = (t3->ts - t2->ts);
 
-	a1 = (v2->speed - v1->speed)/dt1;
-	a2 = (v3->speed - v2->speed)/dt2;
+	a1 = 1000*(v2->speed - v1->speed)/dt1;
+	a2 = 1000*(v3->speed - v2->speed)/dt2;
 
 	if(a1/a2 > 1.1 || a1/a2 < 0.9){
 		fprintf(stderr,"Inconsistent accelerations: a1: %f a2: %f. inaccurate results possible\n",a1,a2);
@@ -91,6 +91,7 @@ void sim_update(trial* t){
 		t->sim.tm.vehicle.dir - t->vehicle.dir,
 		t->sim.steps);
 // 	fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
+	fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
 
 	/*update simulation vehicle from telemetry: everything and calculate w*/
 	t->sim.tm.vehicle = tm->vehicle;
@@ -102,6 +103,7 @@ void sim_update(trial* t){
 		t->sim.tm.vehicle.w = (tm->vehicle.dir - tmp->vehicle.dir)/(tm->ts - tmp->ts);
 	}
 // 	fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
+	fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
 }
 
 int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){
@@ -118,18 +120,17 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat
 
 	a = t->map.a;
 	k = t->map.k;
-	aw = t->map.aw;
 
 	wsoll = turn2w(t,before->turn);
-	aw *= (before->w > wsoll) ? -1 : 1;
 
 	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)*tmp.speed/1000;
-		diff.y = sin(tmp.dir)*tmp.speed/1000;
-		diff.speed = a - k*tmp.speed*tmp.speed*1e-6;
+		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.dir = tmp.w;
@@ -137,7 +138,7 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat
 		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*1000*diff.speed);
+		tmp.speed = fmax(0, tmp.speed + dt*diff.speed);
 		tmp.w += dt*diff.w;
 		tmp.dir += dt*diff.dir;
 
@@ -157,7 +158,7 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat
 	after->y = tmp.y;
 	after->speed = tmp.speed;
 	after->w = tmp.w;
-	after->dir = tmp.dir;
+	after->dir = fixdirection(tmp.dir);
 	return 0;	
 }
 
diff --git a/src/radar.c b/src/radar.c
index 1fa1a51..e321f70 100644
--- a/src/radar.c
+++ b/src/radar.c
@@ -27,12 +27,6 @@ angle* angle_new(double a, int c) {
 	return x;
 }
 
-static double fixangle(double a) {
-	while (a < -180) a += 360;
-	while (a > 180) a -= 360;
-	return a;
-}
-
 int distance_weight(double distance) {
 	if (distance > 50) {
 		if (distance > 150) return 0;
diff --git a/src/radar_main.c b/src/radar_main.c
index c7234ad..48da07f 100644
--- a/src/radar_main.c
+++ b/src/radar_main.c
@@ -33,7 +33,6 @@ void trial_loop(trial *t) {
 					(telemetry*) g_queue_peek_nth(&t->telemetry,4),
 					&t->map);
 			fprintf(stderr,"Fitparameter: a: %f aw: %f k: %f\n",t->map.a,t->map.aw,t->map.k);
-
 		}
 		while (t->alive) {
 			radar_dgl(t, getcurts(t) + LATENCY);