started a new algorithm

extended map with fit parameters

src/control.h

@@ -55,6 +55,8 @@ struct vehicle {
 	turn_t turn;
 	double x, y;
 	double dir, speed;
+	/*Winkelgeschwindigkeit*/
+	double w;
 };
 
 struct telemetry {
@@ -64,10 +66,12 @@ struct telemetry {
 };
 
 struct map {
-	float dx, dy;
+	double dx, dy;
 	timestamp limit;
-	float min_sensor, max_sensor;
+	double min_sensor, max_sensor;
-	float max_speed, max_turn, max_hard_turn;
+	double max_speed, max_turn, max_hard_turn;
+	/*unknown quantities: acceleration, angle acceleration, drag coefficient*/
+	double a, aw, k;
 
 	GArray *solid_objects;
 };

src/lookahead-main.c

@@ -0,0 +1,36 @@
+#include "control.h"
+
+#include <stdio.h>
+
+#define LATENCY 20
+
+void trial_loop(trial *t) {
+	do {
+		if (-1 == trial_wait_for_start(t)) return;
+		if (t->finished) break;
+		while (t->alive) {
+			lookahead(t);
+			if (-1 == trial_check_input(t)) return;
+		}
+		trial_reset_run(t);
+	} while (!t->finished);
+}
+
+int main(int argc, char **argv) {
+	trial *t;
+	
+	if (argc <= 2) {
+		fprintf(stderr, "Syntax: %s hostname port\n", argv[0]);
+		return 1;
+	}
+	
+	if (NULL == (t = trial_new(argv[1], argv[2]))) {
+		return 2;
+	}
+
+	trial_loop(t);
+	
+	trial_free(t);
+	
+	return 0;
+}

src/lookahead.c

@@ -0,0 +1,114 @@
+#include "control.h"
+#include <stdio.h>
+#include <math.h>
+
+struct collcheck {
+	int collision;
+	double x,y;
+};
+
+void check_coll(gpointer key, gpointer value, gpointer ud){
+	double dx,dy;
+	dx = value.x - ud.x;
+	dy = value.y - ud.y;
+	 *ud.collision |= (sqrt(dx*dx + dy*dy) - value.rad) < 1;
+}
+
+void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, double* a, double* aw){
+	double a1,a2;
+	double w1,w2;
+	double dt1,dt2;
+
+	dt1 = (t2->ts - t1->ts);
+	dt2 = (t3->ts - t2->ts);
+
+	a1 = (t2->speed - t1->speed)/dt1;
+	a2 = (t3->speed - t2->speed)/dt2;
+
+	if(a1/a2 > 1.1 || a1/a2 < 0.9){
+		fprintf(stderr,"Inkonsistent accelerations, inaccurate results possible\n");
+	}
+
+	*a = (a1 + a2)/2;
+
+	w1 = (t2->dir - t1->dir)/dt1;
+	w2 = (t3->dir - t2->dir)/dt2;
+
+	*aw = 2*(w2 - w1)/(dt1 + dt2);
+}
+
+void dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){
+	double a,k;
+	vehicle tmp = *before;
+	vehicle diff;
+	timestamp dt = 10;
+	int i,end=deltat/dt;
+	struct collcheck collision;
+
+	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;
+	case(before.turn){
+		case(TURN_HARD_LEFT): wsoll = t->map.max_hard_turn; break;
+		case(TURN_LEFT): wsoll = t->map.max_turn;  break;
+		case(TURN_STRAIGHT): wsoll = 0; break;
+		case(TURN_RIGHT): wsoll = -t->map.max_turn; break;
+		case(TURN_HARD_RIGHT): wsoll = -t->map.max_hard_turn; break;
+	}
+	aw *= (w > wsoll) ? -1 : 1;
+
+	for(i = 0;i<end;i++){
+		
+		/*calculate derivative*/
+		diff.x = cos(tmp.dir)*tmp.speed;
+		diff.y = sin(tmp.dir)*tmp.speed;
+		diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed);
+		/*maybe its logistic: k*w*(wsoll - w)*/
+		diff.w = (tmp.w == wsoll) ? 0 : aw;
+		diff.dir = tmp.w;
+		/*Euler Step*/
+		tmp.x += dt*diff.x;
+		tmp.y += dt*diff.y;
+		tmp.speed += (tmp.speed > 0) ? dt*diff.speed : 0;
+		tmp.w += dt*diff.w;
+		tmp.dir = dt*diff.dir;
+
+		/*check for collisions TODO: optimize*/
+		collision.x = tmp.x;
+		collision.y = tmp.y;
+		collision.collision = 0;
+		g_hash_foreach(t->map.solid->objects,check_coll,&collision);
+		if(collision){
+			return -1;
+		}
+		
+	}
+
+	after.x = tmp.x;
+	after.y = tmp.y;
+	after.speed = tmp.speed;
+	after.w = tmp.w;
+	after.dir = tmp.dir;
+	
+}
+
+
+void lookahead(trial* t){
+	int accel, turn; 
+	vehicle current, future;
+
+	current = t->vehicle;
+
+	for(accel=0; accel < 3;accel++){
+		current.accel = accel;
+		for(turn=0; turn < 5; turn++){
+			current.turn = turn;
+			if(dgl(t,&future,&current,20) == -1) continue;
+			/*TODO: Bewertung*/
+		}
+	}
+}