Radar improving, save all objects

This commit is contained in:
Stefan Bühler 2008-07-13 18:42:05 +02:00
parent 07830c6bbd
commit ba292a3991
7 changed files with 139 additions and 74 deletions

View File

@ -3,7 +3,7 @@
"timeLimit" : 30000, "timeLimit" : 30000,
"vehicleParams" : { "vehicleParams" : {
"maxSpeed" : 20, "maxSpeed" : 20,
"accel" : 4, "accel" : 2,
"brake" : 3, "brake" : 3,
"turn" : 20, "turn" : 20,
"hardTurn" : 60, "hardTurn" : 60,

View File

@ -75,6 +75,30 @@ void telemetry_print(telemetry *t){
vehicle_print(&t->vehicle); vehicle_print(&t->vehicle);
} }
guint object_hash(gconstpointer _o) {
object *o = (object*) _o;
const gchar *p = (const gchar*) (&o->x);
gsize n = 2 * sizeof(double);
guint h = 0;
while (n--) { h = (h << 5) - h + *p; p++; }
return h;
}
gboolean object_equal(gconstpointer _a, gconstpointer _b) {
object *a = (object*) _a, *b = (object*) _b;
return (a->type == b->type && a->x == b->x && a->y == b->y && a->rad == b->rad);
}
object* object_new(object_t type, double x, double y, double rad, double dir, double speed) {
object *o = g_slice_new(object);
o->type = type; o->x = x; o->y = y; o->rad = rad; o->dir = dir; o->speed = speed;
return o;
}
void _object_free(gpointer p) {
g_slice_free(object, (object*) p);
}
trial *trial_new(const char *hostname, const char *port) { trial *trial_new(const char *hostname, const char *port) {
trial *t; trial *t;
int sock; int sock;
@ -84,7 +108,7 @@ trial *trial_new(const char *hostname, const char *port) {
t = g_slice_new0(trial); t = g_slice_new0(trial);
t->socket = sock; t->socket = sock;
g_queue_init(&t->telemetry); g_queue_init(&t->telemetry);
t->map.solid_objects = g_array_new(FALSE, FALSE, sizeof(object)); t->map.solid_objects = g_hash_table_new_full(object_hash, object_equal, NULL, _object_free);
t->sim.tm.objects = g_array_new(FALSE, FALSE, sizeof(object)); t->sim.tm.objects = g_array_new(FALSE, FALSE, sizeof(object));
control_parser_new(t); control_parser_new(t);
t->runcnt = 0; t->runcnt = 0;
@ -170,7 +194,7 @@ void trial_free(trial *t) {
close(t->socket); close(t->socket);
g_queue_foreach(&t->telemetry, telemetry_free, NULL); g_queue_foreach(&t->telemetry, telemetry_free, NULL);
g_queue_clear(&t->telemetry); g_queue_clear(&t->telemetry);
g_array_free(t->map.solid_objects, TRUE); g_hash_table_destroy(t->map.solid_objects);
g_array_free(t->sim.tm.objects, TRUE); g_array_free(t->sim.tm.objects, TRUE);
control_parser_free(t); control_parser_free(t);
g_slice_free(trial, t); g_slice_free(trial, t);

View File

@ -69,7 +69,7 @@ struct map {
float min_sensor, max_sensor; float min_sensor, max_sensor;
float max_speed, max_turn, max_hard_turn; float max_speed, max_turn, max_hard_turn;
GArray *solid_objects; GHashTable *solid_objects;
}; };
struct simulated { struct simulated {
@ -106,6 +106,8 @@ int trial_check_input(trial *t);
int trial_wait_for_input(trial *t); int trial_wait_for_input(trial *t);
void trial_free(trial *t); void trial_free(trial *t);
object* object_new(object_t type, double x, double y, double rad, double dir, double speed);
/* debugging stuff */ /* debugging stuff */
void object_print(object *o); void object_print(object *o);
void vehicle_print(vehicle *v); void vehicle_print(vehicle *v);

View File

@ -27,7 +27,7 @@ static timestamp extract_ts(context *ctx, char *fpc) {
return atoi(ctx->tmp->str); return atoi(ctx->tmp->str);
} }
#line 142 "control_parser.rl" #line 144 "control_parser.rl"
@ -243,7 +243,7 @@ static const int control_parser_error = 0;
static const int control_parser_en_main = 1; static const int control_parser_en_main = 1;
#line 145 "control_parser.rl" #line 147 "control_parser.rl"
static int control_parser_has_error(context *ctx) { static int control_parser_has_error(context *ctx) {
return ctx->cs == control_parser_error; return ctx->cs == control_parser_error;
@ -260,7 +260,7 @@ void control_parser_new(trial *t) {
{ {
( ctx->cs) = control_parser_start; ( ctx->cs) = control_parser_start;
} }
#line 157 "control_parser.rl" #line 159 "control_parser.rl"
ctx->buffer = g_string_sized_new(0); ctx->buffer = g_string_sized_new(0);
ctx->tmp = g_string_sized_new(0); ctx->tmp = g_string_sized_new(0);
ctx->mark = -1; ctx->mark = -1;
@ -274,7 +274,7 @@ void control_parser_reset(trial *t) {
{ {
( ctx->cs) = control_parser_start; ( ctx->cs) = control_parser_start;
} }
#line 166 "control_parser.rl" #line 168 "control_parser.rl"
g_string_truncate(ctx->tmp, 0); g_string_truncate(ctx->tmp, 0);
ctx->mark = -1; ctx->mark = -1;
/* fprintf(stderr, "Parser reset\n"); */ /* fprintf(stderr, "Parser reset\n"); */
@ -522,25 +522,27 @@ _match:
case 27: case 27:
#line 113 "control_parser.rl" #line 113 "control_parser.rl"
{ {
object o = { BOLDER, ctx->x, ctx->y, ctx->r, 0, 0 }; object *o = object_new(BOLDER, ctx->x, ctx->y, ctx->r, 0, 0);
g_array_append_val(ctx->tm->objects, o); /* g_array_append_vals(ctx->tm->objects, o, 1); */
g_hash_table_replace(t->map.solid_objects, o, o);
} }
break; break;
case 28: case 28:
#line 117 "control_parser.rl" #line 118 "control_parser.rl"
{ {
object o = { CRATER, ctx->x, ctx->y, ctx->r, 0, 0 }; object *o = object_new(CRATER, ctx->x, ctx->y, ctx->r, 0, 0);
g_array_append_val(ctx->tm->objects, o); /* g_array_append_vals(ctx->tm->objects, o, 1); */
g_hash_table_replace(t->map.solid_objects, o, o);
} }
break; break;
case 29: case 29:
#line 122 "control_parser.rl" #line 124 "control_parser.rl"
{ {
object o = { MARTIAN, ctx->x, ctx->y, MARTIAN_RADIUS, 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);
} }
break; break;
#line 544 "control_parser.c" #line 546 "control_parser.c"
} }
} }
@ -552,7 +554,7 @@ _again:
_test_eof: {} _test_eof: {}
_out: {} _out: {}
} }
#line 189 "control_parser.rl" #line 191 "control_parser.rl"
ctx->pos = p - ctx->buffer->str; ctx->pos = p - ctx->buffer->str;
if (ctx->mark == -1) { if (ctx->mark == -1) {

View File

@ -111,12 +111,14 @@ static timestamp extract_ts(context *ctx, char *fpc) {
score = (digit*) >mark % { printf("Score %u\n", extract_ts(ctx, fpc)); }; score = (digit*) >mark % { printf("Score %u\n", extract_ts(ctx, fpc)); };
boulder = "b" SP x SP y SP r SP % { boulder = "b" SP x SP y SP r SP % {
object o = { BOLDER, ctx->x, ctx->y, ctx->r, 0, 0 }; object *o = object_new(BOLDER, ctx->x, ctx->y, ctx->r, 0, 0);
g_array_append_val(ctx->tm->objects, o); /* g_array_append_vals(ctx->tm->objects, o, 1); */
g_hash_table_replace(t->map.solid_objects, o, o);
}; };
crater = "c" SP x SP y SP r SP % { crater = "c" SP x SP y SP r SP % {
object o = { CRATER, ctx->x, ctx->y, ctx->r, 0, 0 }; object *o = object_new(CRATER, ctx->x, ctx->y, ctx->r, 0, 0);
g_array_append_val(ctx->tm->objects, o); /* g_array_append_vals(ctx->tm->objects, o, 1); */
g_hash_table_replace(t->map.solid_objects, o, o);
}; };
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 % {

View File

@ -33,43 +33,73 @@ static double fixangle(double a) {
return a; return a;
} }
int distance_weight(double distance) {
if (distance > 50) {
if (distance > 150) return 0;
else return 1;
} else {
if (distance > 20) return 10;
else return 200;
}
}
static inline gboolean insert_object(GSequence *angles, object *o, vehicle *v, double ve_ho_angle) {
double phi, phi2, avoid = 0, oangle;
double distance;
int distweight;
distance = sqrt((o->y - v->y) * (o->y - v->y) + (o->x - v->x) * (o->x - v->x));
distweight = distance_weight(distance - o->rad);
if (distweight == 0) return FALSE;
oangle = fixangle(atan2(-o->y, -o->x)*180/M_PI - ve_ho_angle);
if (fabs(oangle) > 90) return FALSE;
phi = fixangle(atan2(o->y - v->y, o->x - v->x)*180/M_PI - ve_ho_angle);
if (fabs(phi) > 120) return FALSE;
switch (o->type) {
case BOLDER:
avoid = 1 * o->rad + VEHICLE_RADIUS + 1; break;
case CRATER:
avoid = 1 * o->rad + VEHICLE_RADIUS + 2; break;
case MARTIAN:
/* todo: guess martian movement */
avoid = o->rad + VEHICLE_RADIUS + 5; break;
}
phi2 = atan(avoid / distance )*180/M_PI;
g_sequence_insert_sorted(angles, angle_new(phi - phi2, 1), cmp_angle, NULL);
g_sequence_insert_sorted(angles, angle_new(phi + phi2, -1), cmp_angle, NULL);
return TRUE;
}
void goradar(trial *t) { void goradar(trial *t) {
vehicle v; vehicle v;
telemetry *tm; telemetry *tm;
GSequence *angles = g_sequence_new((GDestroyNotify) angle_free); GSequence *angles = g_sequence_new((GDestroyNotify) angle_free);
GSequenceIter *prev, *cur, *end; GSequenceIter *prev, *cur, *end;
angle *acur, *aprev; angle *acur, *aprev;
double hang, ang = 1000, weight = 0; double ve_ho_angle, ang = 1000, weight = 0;
int c; int c;
guint i; GHashTableIter iter;
guint i, objects = 0;
gpointer key;
object *value;
v = t->sim.tm.vehicle; v = t->sim.tm.vehicle;
hang = atan2(-v.y, -v.x)*180/M_PI; ve_ho_angle = atan2(-v.y, -v.x)*180/M_PI;
tm = &t->sim.tm; tm = &t->sim.tm;
g_sequence_insert_sorted(angles, angle_new(-45, 0), cmp_angle, NULL); g_sequence_insert_sorted(angles, angle_new(-120, 0), cmp_angle, NULL);
g_sequence_insert_sorted(angles, angle_new(45, 0), cmp_angle, NULL); g_sequence_insert_sorted(angles, angle_new(120, 0), cmp_angle, NULL);
for (i = 0; i < tm->objects->len; i++) { for (i = 0; i < tm->objects->len; i++) {
object *o = &g_array_index(tm->objects, object, i); object *o = &g_array_index(tm->objects, object, i);
double phi, phi2, avoid = 0, oangle; if (insert_object(angles, o, &v, ve_ho_angle)) objects++;
double distance = sqrt((o->y - v.y) * (o->y - v.y) + (o->x - v.x) * (o->x - v.x));
oangle = fixangle(atan2(-o->y, -o->x)*180/M_PI - hang);
if (fabs(oangle) > 180) continue;
// if (distance > 100) continue;
phi = fixangle(atan2(o->y - v.y, o->x - v.x)*180/M_PI - hang);
if (fabs(phi) > 45) continue;
switch (o->type) {
case BOLDER:
avoid = 1.2 * o->rad + VEHICLE_RADIUS; break;
case CRATER:
avoid = 1.2 * o->rad + VEHICLE_RADIUS + 1; break;
case MARTIAN:
avoid = o->rad + VEHICLE_RADIUS + 5; break;
} }
phi2 = atan(avoid / distance )*180/M_PI; g_hash_table_iter_init(&iter, t->map.solid_objects);
g_sequence_insert_sorted(angles, angle_new(phi - phi2, 1), cmp_angle, NULL); while (g_hash_table_iter_next(&iter, &key, (gpointer*) &value)) {
g_sequence_insert_sorted(angles, angle_new(phi + phi2, -1), cmp_angle, NULL); if (insert_object(angles, value, &v, ve_ho_angle)) objects++;
} }
prev = g_sequence_get_begin_iter(angles), end = g_sequence_get_end_iter(angles); prev = g_sequence_get_begin_iter(angles), end = g_sequence_get_end_iter(angles);
@ -79,10 +109,8 @@ void goradar(trial *t) {
while ((cur = g_sequence_iter_next(prev)) != end) { while ((cur = g_sequence_iter_next(prev)) != end) {
acur = (angle*) g_sequence_get(cur); acur = (angle*) g_sequence_get(cur);
assert(aprev->a <= acur->a); assert(aprev->a <= acur->a);
if (!c) {
// fprintf(stderr, "Gap between %f and %f\n", aprev->a, acur->a);
if (acur->a - aprev->a > 1) { if (acur->a - aprev->a > 1) {
double ax, ad, w1 = 1E20, w2 = 1E20, w; double ax, w;
if (aprev->a <= 0) { if (aprev->a <= 0) {
if (acur->a <= 0) { if (acur->a <= 0) {
ax = acur->a; ax = acur->a;
@ -92,17 +120,14 @@ void goradar(trial *t) {
} else { } else {
ax = aprev->a; ax = aprev->a;
} }
if (ax != 0) w1 = 1/fabs(ax); w = 30 - fabs(ax)/30 - c;
ad = fixangle(ax - v.dir + hang); // fprintf(stderr, "Angle %f, weight %f\n", ax, w);
if (ad != 0) w2 = 1/fabs(ad);
w = w1+w2;
if (w > weight) { if (w > weight) {
fprintf(stderr, "Selected %f\n", ax); // fprintf(stderr, "Selected %f\n", ax);
ang = ax; ang = ax;
weight = w; weight = w;
} }
} }
}
c += acur->c; c += acur->c;
prev = cur; prev = cur;
@ -110,22 +135,16 @@ void goradar(trial *t) {
} }
} }
if (weight == 0) { ang = fixangle(ang - v.dir + ve_ho_angle);
fprintf(stderr, "Potential\n"); // fprintf(stderr, "Angle: %f, objects: %u\n", ang, objects);
godown(t);
return;
}
ang = fixangle(ang - v.dir + hang);
fprintf(stderr, "Angle: %f, objects: %u\n", ang, tm->objects->len);
if (ang < 1) { if (ang < 1) {
if (ang < -50) { if (ang < -15) {
vehicle_hard_right(t); vehicle_hard_right(t);
} else { } else {
vehicle_right(t); vehicle_right(t);
} }
} else if (ang > 1) { } else if (ang > 1) {
if (ang > 50) { if (ang > 15) {
vehicle_hard_left(t); vehicle_hard_left(t);
} else { } else {
vehicle_left(t); vehicle_left(t);
@ -134,7 +153,7 @@ void goradar(trial *t) {
ang = fabs(ang); ang = fabs(ang);
if (ang > 90) { if (ang > 90) {
vehicle_break(t); vehicle_break(t);
} else if (ang > 45) { } else if (ang > 60) {
vehicle_roll(t); vehicle_roll(t);
} else { } else {
vehicle_accel(t); vehicle_accel(t);

View File

@ -10,6 +10,22 @@ void trial_loop(trial *t) {
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;
if (t->runcnt == 0) {
goradar(t);
vehicle_accel(t);
switch (t->v.turn) {
case TURN_LEFT:
case TURN_STRAIGHT:
vehicle_hard_left(t); break;
case TURN_RIGHT:
vehicle_hard_right(t); break;
default: break;
}
while (t->telemetry->length < 4) {
if (-1 == trial_wait_for_input(t)) return;
}
/* TODO: call constant calculatoin */
}
while (t->alive) { while (t->alive) {
simulate(t, getcurts(t) + LATENCY); simulate(t, getcurts(t) + LATENCY);
goradar(t); goradar(t);