Other way

src/control.h

@@ -11,6 +11,10 @@
 #define INLINE static inline
 #define UNUSED(x) ((void)(x))
 
+#define VEHICLE_RADIUS 0.5
+#define HOMEBASE_RADIUS 5
+#define MARTIAN_RADIUS 0.4
+
 typedef enum { RUN_ERROR, RUN_DONE, RUN_GO_ON, RUN_FINISHED } run_t;
 
 typedef enum { ACCEL, ROLL, BRAKE } accel_t;

src/control_parser.rl

@@ -108,7 +108,7 @@ static timestamp extract_ts(context *ctx, char *fpc) {
 	};
 	homebase = "h" SP x SP y SP r SP; # ignore it
 	martian = "m" SP x SP y SP dir SP speed SP % {
-		object o = { MARTIAN, ctx->x, ctx->y, ctx->r, ctx->dir, ctx->speed };
+		object o = { MARTIAN, ctx->x, ctx->y, MARTIAN_RADIUS, ctx->dir, ctx->speed };
 		g_array_append_val(ctx->tm->objects, o);
 	};
 	object = boulder | crater | homebase | martian;

src/grid.c

@@ -0,0 +1,48 @@
+
+#include "grid.h"
+
+#define BLOCK(p, left, step) (int)((p-left)/step)
+
+static inline double pyt_h2(double r, double x) {
+	return sqrt(r*r-x*x);
+}
+
+static void _insert(grid *g, object *o, double l, double r, double t, double b) {
+	double hstep = g->top - g->bottom / GRID_SIZE;
+	double wstep = g->right - g->left / GRID_SIZE, wstep2 = wstep/2;
+	int y,
+		ndx_b = max(0,            BLOCK(b, g->bottom, hstep)),
+		ndx_t = min(GRID_SPLIT-1, BLOCK(t, g->bottom, hstep));
+	for (y = ndx_b; y <= ndx_t; y++) {
+		double posy = g->bottom + wstep * (y+0.5);
+		if (posy > o->y) posy -= wstep2;
+		else posy += wstep2;
+		double dx = pyt_h2(o->r, posy - o->y);
+		int x,
+			ndx_l = max(0,            BLOCK(o->x-dx, g->left, wstep)),
+			ndx_r = min(GRID_SPLIT-1, BLOCK(o->y-dy, g->left, wstep));
+		for (x = ndx_l; x <= ndx_r; x++) {
+			if (g->nodes[y][x].grid) {
+				_insert(g->nodes[y][x].grid, o, l, r, t, b);
+			}
+			g_queue_push_back(g->nodes[y][x].objects, o);
+		}
+	}
+}
+
+void grid_insert(grid* g, object *o) {
+	object *co = g_slice_new(object);
+	memcpy(co, o, sizeof(*co));
+	_insert(g, co, o->x - o->r, o->x + o->r, o->y - o->r, o->y + o->r);
+	g_queue_push_back(g->nodes[y][x].objects, o);
+}
+
+void _do_split(gpointer _o, gpointer g) {
+	object *o = (object*) o;
+	_insert((grid*) g, co, o->x - o->r, o->x + o->r, o->y - o->r, o->y + o->r);
+}
+
+void grid_split(gridnode *n) {
+	n->grid = g_slice_new0(grid);
+	g_queue_foreach(n->objects, _do_split, n->grid);
+}

src/grid.h

@@ -0,0 +1,27 @@
+#ifndef _GRID_H
+#define _GRID_H
+
+#define GRID_SPLIT 16
+
+struct grid;
+typedef struct grid grid;
+
+struct gridnode;
+typedef struct gridnode gridnode;
+
+#include "control.h"
+
+struct gridnode {
+	grid *g;
+	GQueue *objects;
+};
+
+struct grid {
+	double left, right, top, bottom;
+	gridnode nodes[GRID_SPLIT][GRID_SPLIT];
+};
+
+void grid_insert(grid* g, object *o);
+void grid_split(grid *g);
+
+#endif

src/main.c

@@ -4,23 +4,26 @@
 
 #include <stdio.h>
 
+void godown(trial *t);
+
 void trial_loop(trial *t) {
-	path *p;
+// 	path *p;
-	int offset;
+// 	int offset;
 	do {
 		if (-1 == trial_wait_for_start(t)) return;
 		if (t->finished) break;
-		p = path_new();
+// 		p = path_new();
 		/*create trivial path towards the origin*/
-		offset = path_app_fitseq(p,t,0);
+// 		offset = path_app_fitseq(p,t,0);
-		offset = path_app_target(p,t,offset,0,0);
+// 		offset = path_app_target(p,t,offset,0,0);
 		while (t->alive) {
-			path_execute(t,p);
+// 			path_execute(t,p);
-			if (-1 == trial_check_input(t)) return;
+			godown(t);
+			if (-1 == trial_wait_for_input(t)) return;
 		}
 		trial_matlab(t);
 		trial_reset_run(t);
-		path_free(p);
+// 		path_free(p);
 	} while (!t->finished);
 }
 

src/path.c

@@ -16,7 +16,7 @@ void command_free(command* c){
 	g_slice_free(command, c);
 }
 
-double angle_for_rot(double from, double to) {
+static double angle_for_rot(double from, double to) {
 	double a = to - from;
 	if (a > 180) a = -360 + a;
 	else if (a < -180) a = 360 + a;

src/potential.c

@@ -0,0 +1,81 @@
+
+#include "control.h"
+
+#include <math.h>
+#include <stdlib.h>
+
+static double angle_for_rot(double from, double to) {
+	double a = to - from;
+	if (a > 180) a = -360 + a;
+	else if (a < -180) a = 360 + a;
+	return a;
+}
+
+void godown(trial *t) {
+	double C1 = 1 / sqrt(2*M_PI);
+
+	vehicle v;
+	telemetry *tm;
+	
+	double xpot, ypot, r, angle;
+	
+	/* todo: simulate movement */
+	v = t->vehicle;
+	
+	tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
+	
+	r = sqrt(v.x*v.x + v.y*v.y);
+	if (r < 4) return; /* done */
+	xpot = v.x/r;
+	ypot = v.y/r;
+	
+	for (guint i = 0; i < tm->objects->len; i++) {
+		object *o = &g_array_index(tm->objects, object, i);
+		
+		double sigma = 0, h = 0;
+		
+		switch (o->type) {
+			case BOLDER:
+				sigma = 2 * o->rad;
+				h = 1;
+				break;
+			case CRATER:
+				sigma = 2 * o->rad;
+				h = 3;
+				break;
+			case MARTIAN:
+				sigma = 4 * o->rad;
+				h = 1;
+				break;
+		}
+		
+		r = sqrt( (o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y) );
+		r = C1 * h / (sigma*sigma) / exp (r / (2*sigma) );
+		xpot += (o->x - v.x) * r;
+		ypot += (o->y - v.y) * r;
+	}
+	
+	angle = atan2(-ypot, -xpot)*180/M_PI;
+	angle = angle_for_rot(v.dir, angle);
+	if (angle < 1) {
+		if (angle < -20) {
+			vehicle_hard_right(t);
+		} else {
+			vehicle_right(t);
+		}
+	} else if (angle > 1) {
+		if (angle > 20) {
+			vehicle_hard_left(t);
+		} else {
+			vehicle_left(t);
+		}
+	}
+	angle = abs(angle);
+	if (angle > 60) {
+		vehicle_break(t);
+	} else if (angle > 30) {
+		vehicle_roll(t);
+	} else {
+		vehicle_accel(t);
+	}
+}

src/wscript

@@ -8,6 +8,7 @@ main_source = '''
 	control.c
 	control_parser.c
 	path.c
+	potential.c
 '''
 
 def build(bld):