Other way

This commit is contained in:
Stefan Bühler 2008-07-12 16:00:47 +02:00
parent 12d711e9c2
commit 778d9ca994
8 changed files with 174 additions and 10 deletions

View File

@ -11,6 +11,10 @@
#define INLINE static inline #define INLINE static inline
#define UNUSED(x) ((void)(x)) #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 { RUN_ERROR, RUN_DONE, RUN_GO_ON, RUN_FINISHED } run_t;
typedef enum { ACCEL, ROLL, BRAKE } accel_t; typedef enum { ACCEL, ROLL, BRAKE } accel_t;

View File

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

48
src/grid.c Normal file
View File

@ -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);
}

27
src/grid.h Normal file
View File

@ -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

View File

@ -4,23 +4,26 @@
#include <stdio.h> #include <stdio.h>
void godown(trial *t);
void trial_loop(trial *t) { void trial_loop(trial *t) {
path *p; // path *p;
int offset; // int offset;
do { do {
if (-1 == trial_wait_for_start(t)) return; if (-1 == trial_wait_for_start(t)) return;
if (t->finished) break; if (t->finished) break;
p = path_new(); // p = path_new();
/*create trivial path towards the origin*/ /*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) { 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_matlab(t);
trial_reset_run(t); trial_reset_run(t);
path_free(p); // path_free(p);
} while (!t->finished); } while (!t->finished);
} }

View File

@ -16,7 +16,7 @@ void command_free(command* c){
g_slice_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; double a = to - from;
if (a > 180) a = -360 + a; if (a > 180) a = -360 + a;
else if (a < -180) a = 360 + a; else if (a < -180) a = 360 + a;

81
src/potential.c Normal file
View File

@ -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);
}
}

View File

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