From 778d9ca994e6e65aaf1e3463b7328a4251035597 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Stefan=20B=C3=BChler?= Date: Sat, 12 Jul 2008 16:00:47 +0200 Subject: [PATCH] Other way --- src/control.h | 4 +++ src/control_parser.rl | 2 +- src/grid.c | 48 +++++++++++++++++++++++++ src/grid.h | 27 +++++++++++++++ src/main.c | 19 +++++----- src/path.c | 2 +- src/potential.c | 81 +++++++++++++++++++++++++++++++++++++++++++ src/wscript | 1 + 8 files changed, 174 insertions(+), 10 deletions(-) create mode 100644 src/grid.c create mode 100644 src/grid.h create mode 100644 src/potential.c diff --git a/src/control.h b/src/control.h index 60a2f3e..2517f30 100644 --- a/src/control.h +++ b/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; diff --git a/src/control_parser.rl b/src/control_parser.rl index c895433..71a0644 100644 --- a/src/control_parser.rl +++ b/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; diff --git a/src/grid.c b/src/grid.c new file mode 100644 index 0000000..7e9fb76 --- /dev/null +++ b/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); +} diff --git a/src/grid.h b/src/grid.h new file mode 100644 index 0000000..f73de67 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/main.c b/src/main.c index bec4d33..64c5731 100644 --- a/src/main.c +++ b/src/main.c @@ -4,23 +4,26 @@ #include +void godown(trial *t); + void trial_loop(trial *t) { - path *p; - int offset; +// path *p; +// 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_target(p,t,offset,0,0); +// offset = path_app_fitseq(p,t,0); +// offset = path_app_target(p,t,offset,0,0); while (t->alive) { - path_execute(t,p); - if (-1 == trial_check_input(t)) return; +// path_execute(t,p); + 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); } diff --git a/src/path.c b/src/path.c index 04a5263..05bdafe 100644 --- a/src/path.c +++ b/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; diff --git a/src/potential.c b/src/potential.c new file mode 100644 index 0000000..1b3350d --- /dev/null +++ b/src/potential.c @@ -0,0 +1,81 @@ + +#include "control.h" + +#include +#include + +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); + } +} diff --git a/src/wscript b/src/wscript index 901738f..1e88a97 100644 --- a/src/wscript +++ b/src/wscript @@ -8,6 +8,7 @@ main_source = ''' control.c control_parser.c path.c + potential.c ''' def build(bld):