Radar improving, save all objects

maps/small-scatter.wrld

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

src/control.c

@@ -75,6 +75,30 @@ void telemetry_print(telemetry *t){
 	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 *t;
 	int sock;
@@ -84,7 +108,7 @@ trial *trial_new(const char *hostname, const char *port) {
 	t = g_slice_new0(trial);
 	t->socket = sock;
 	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));
 	control_parser_new(t);
 	t->runcnt = 0;
@@ -170,7 +194,7 @@ void trial_free(trial *t) {
 	close(t->socket);
 	g_queue_foreach(&t->telemetry, telemetry_free, NULL);
 	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);
 	control_parser_free(t);
 	g_slice_free(trial, t);

src/control.h

@@ -69,7 +69,7 @@ struct map {
 	float min_sensor, max_sensor;
 	float max_speed, max_turn, max_hard_turn;
 
-	GArray *solid_objects;
+	GHashTable *solid_objects;
 };
 
 struct simulated {
@@ -106,6 +106,8 @@ int trial_check_input(trial *t);
 int trial_wait_for_input(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 */
 void object_print(object *o);
 void vehicle_print(vehicle *v);

src/control_parser.c

@@ -27,7 +27,7 @@ static timestamp extract_ts(context *ctx, char *fpc) {
 	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;
 
-#line 145 "control_parser.rl"
+#line 147 "control_parser.rl"
 
 static int control_parser_has_error(context *ctx) {
 	return ctx->cs == control_parser_error;
@@ -260,7 +260,7 @@ void control_parser_new(trial *t) {
 	{
 	( ctx->cs) = control_parser_start;
 	}
-#line 157 "control_parser.rl"
+#line 159 "control_parser.rl"
 	ctx->buffer = g_string_sized_new(0);
 	ctx->tmp = g_string_sized_new(0);
 	ctx->mark = -1;
@@ -274,7 +274,7 @@ void control_parser_reset(trial *t) {
 	{
 	( ctx->cs) = control_parser_start;
 	}
-#line 166 "control_parser.rl"
+#line 168 "control_parser.rl"
 	g_string_truncate(ctx->tmp, 0);
 	ctx->mark = -1;
 	/* fprintf(stderr, "Parser reset\n"); */
@@ -522,25 +522,27 @@ _match:
 	case 27:
 #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;
 	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;
 	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 };
 		g_array_append_val(ctx->tm->objects, o);
 	}
 	break;
-#line 544 "control_parser.c"
+#line 546 "control_parser.c"
 		}
 	}
 
@@ -552,7 +554,7 @@ _again:
 	_test_eof: {}
 	_out: {}
 	}
-#line 189 "control_parser.rl"
+#line 191 "control_parser.rl"
 
 		ctx->pos = p - ctx->buffer->str;
 		if (ctx->mark == -1) {

src/control_parser.rl

@@ -111,12 +111,14 @@ static timestamp extract_ts(context *ctx, char *fpc) {
 	score = (digit*) >mark % { printf("Score %u\n", extract_ts(ctx, fpc)); };
 
 	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 % {
-		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
 	martian = "m" SP x SP y SP dir SP speed SP % {

src/radar.c

@@ -33,43 +33,73 @@ static double fixangle(double 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) {
 	vehicle v;
 	telemetry *tm;
 	GSequence *angles = g_sequence_new((GDestroyNotify) angle_free);
 	GSequenceIter *prev, *cur, *end;
 	angle *acur, *aprev;
-	double hang, ang = 1000, weight = 0;
+	double ve_ho_angle, ang = 1000, weight = 0;
 	int c;
-	guint i;
+	GHashTableIter iter;
+	guint i, objects = 0;
+	gpointer key;
+	object *value;
 
 	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;
 
-	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++) {
 		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);
@@ -79,10 +109,8 @@ void goradar(trial *t) {
 		while ((cur = g_sequence_iter_next(prev)) != end) {
 			acur = (angle*) g_sequence_get(cur);
 			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) {
-					double ax, ad, w1 = 1E20, w2 = 1E20, w;
+				double ax, w;
 				if (aprev->a <= 0) {
 					if (acur->a <= 0) {
 						ax = acur->a;
@@ -92,17 +120,14 @@ void goradar(trial *t) {
 				} else {
 					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) {
-						fprintf(stderr, "Selected %f\n", ax);
+// 				fprintf(stderr, "Selected %f\n", ax);
 					ang = ax;
 					weight = w;
 				}
 			}
-			}
 			
 			c += acur->c;
 			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 < -50) {
+		if (ang < -15) {
 			vehicle_hard_right(t);
 		} else {
 			vehicle_right(t);
 		}
 	} else if (ang > 1) {
-		if (ang > 50) {
+		if (ang > 15) {
 			vehicle_hard_left(t);
 		} else {
 			vehicle_left(t);
@@ -134,7 +153,7 @@ void goradar(trial *t) {
 	ang = fabs(ang);
 	if (ang > 90) {
 		vehicle_break(t);
-	} else if (ang > 45) {
+	} else if (ang > 60) {
 		vehicle_roll(t);
 	} else {
 		vehicle_accel(t);

src/radar_main.c

@@ -10,6 +10,22 @@ void trial_loop(trial *t) {
 	do {
 		if (-1 == trial_wait_for_start(t)) return;
 		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) {
 			simulate(t, getcurts(t) + LATENCY);
 			goradar(t);