2008-07-13 13:19:43 +00:00
|
|
|
|
|
|
|
#include "control.h"
|
|
|
|
|
|
|
|
#include <math.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <assert.h>
|
|
|
|
|
|
|
|
struct angle {
|
|
|
|
double a;
|
|
|
|
int c;
|
|
|
|
};
|
|
|
|
typedef struct angle angle;
|
|
|
|
|
|
|
|
gint cmp_angle(gconstpointer _a, gconstpointer _b, gpointer p) {
|
|
|
|
const angle *a = (const angle*) _a, *b = (const angle*) _b;
|
|
|
|
UNUSED(p);
|
|
|
|
return (a->a == b->a) ? 0 : (a->a < b->a) ? -1 : 1;
|
|
|
|
}
|
|
|
|
|
|
|
|
void angle_free(angle* a) {
|
|
|
|
g_slice_free(angle, a);
|
|
|
|
}
|
|
|
|
|
|
|
|
angle* angle_new(double a, int c) {
|
|
|
|
angle *x = g_slice_new(angle);
|
|
|
|
x->a = a; x->c = c;
|
|
|
|
return x;
|
|
|
|
}
|
|
|
|
|
2008-07-13 16:42:05 +00:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2008-07-13 13:19:43 +00:00
|
|
|
void goradar(trial *t) {
|
|
|
|
vehicle v;
|
|
|
|
telemetry *tm;
|
|
|
|
GSequence *angles = g_sequence_new((GDestroyNotify) angle_free);
|
|
|
|
GSequenceIter *prev, *cur, *end;
|
|
|
|
angle *acur, *aprev;
|
2008-07-13 16:42:05 +00:00
|
|
|
double ve_ho_angle, ang = 1000, weight = 0;
|
2008-07-13 13:19:43 +00:00
|
|
|
int c;
|
2008-07-13 16:42:05 +00:00
|
|
|
GHashTableIter iter;
|
|
|
|
guint i, objects = 0;
|
|
|
|
gpointer key;
|
|
|
|
object *value;
|
2008-07-13 13:19:43 +00:00
|
|
|
|
|
|
|
v = t->sim.tm.vehicle;
|
2008-07-13 16:42:05 +00:00
|
|
|
ve_ho_angle = atan2(-v.y, -v.x)*180/M_PI;
|
2008-07-13 13:19:43 +00:00
|
|
|
|
|
|
|
tm = &t->sim.tm;
|
|
|
|
|
2008-07-13 16:42:05 +00:00
|
|
|
g_sequence_insert_sorted(angles, angle_new(-120, 0), cmp_angle, NULL);
|
|
|
|
g_sequence_insert_sorted(angles, angle_new(120, 0), cmp_angle, NULL);
|
2008-07-13 13:19:43 +00:00
|
|
|
for (i = 0; i < tm->objects->len; i++) {
|
|
|
|
object *o = &g_array_index(tm->objects, object, i);
|
2008-07-13 16:42:05 +00:00
|
|
|
if (insert_object(angles, o, &v, ve_ho_angle)) objects++;
|
|
|
|
}
|
|
|
|
g_hash_table_iter_init(&iter, t->map.solid_objects);
|
|
|
|
while (g_hash_table_iter_next(&iter, &key, (gpointer*) &value)) {
|
|
|
|
if (insert_object(angles, value, &v, ve_ho_angle)) objects++;
|
2008-07-13 13:19:43 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
prev = g_sequence_get_begin_iter(angles), end = g_sequence_get_end_iter(angles);
|
|
|
|
if (prev != end) {
|
|
|
|
aprev = (angle*) g_sequence_get(prev);
|
|
|
|
c = aprev->c;
|
|
|
|
while ((cur = g_sequence_iter_next(prev)) != end) {
|
|
|
|
acur = (angle*) g_sequence_get(cur);
|
|
|
|
assert(aprev->a <= acur->a);
|
2008-07-13 16:42:05 +00:00
|
|
|
if (acur->a - aprev->a > 1) {
|
|
|
|
double ax, w;
|
|
|
|
if (aprev->a <= 0) {
|
|
|
|
if (acur->a <= 0) {
|
|
|
|
ax = acur->a;
|
2008-07-13 13:19:43 +00:00
|
|
|
} else {
|
2008-07-13 16:42:05 +00:00
|
|
|
ax = 0;
|
2008-07-13 13:19:43 +00:00
|
|
|
}
|
2008-07-13 16:42:05 +00:00
|
|
|
} else {
|
|
|
|
ax = aprev->a;
|
|
|
|
}
|
|
|
|
w = 30 - fabs(ax)/30 - c;
|
|
|
|
// fprintf(stderr, "Angle %f, weight %f\n", ax, w);
|
|
|
|
if (w > weight) {
|
|
|
|
// fprintf(stderr, "Selected %f\n", ax);
|
|
|
|
ang = ax;
|
|
|
|
weight = w;
|
2008-07-13 13:19:43 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
c += acur->c;
|
|
|
|
prev = cur;
|
|
|
|
aprev = acur;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2008-07-13 22:33:51 +00:00
|
|
|
ang = fixangle(ang - (v.dir + 30*v.w) + ve_ho_angle);
|
|
|
|
// fprintf(stderr, "Angle: %f (%f), objects: %u\n", ang, 20*v.w, objects);
|
2008-07-13 13:19:43 +00:00
|
|
|
if (ang < 1) {
|
2008-07-13 22:33:51 +00:00
|
|
|
if (ang < -10) {
|
2008-07-13 13:19:43 +00:00
|
|
|
vehicle_hard_right(t);
|
|
|
|
} else {
|
|
|
|
vehicle_right(t);
|
|
|
|
}
|
|
|
|
} else if (ang > 1) {
|
2008-07-13 22:33:51 +00:00
|
|
|
if (ang > 10) {
|
2008-07-13 13:19:43 +00:00
|
|
|
vehicle_hard_left(t);
|
|
|
|
} else {
|
|
|
|
vehicle_left(t);
|
|
|
|
}
|
2008-07-13 22:33:51 +00:00
|
|
|
} else {
|
|
|
|
vehicle_straight(t);
|
2008-07-13 13:19:43 +00:00
|
|
|
}
|
|
|
|
ang = fabs(ang);
|
|
|
|
if (ang > 90) {
|
|
|
|
vehicle_break(t);
|
2008-07-13 16:42:05 +00:00
|
|
|
} else if (ang > 60) {
|
2008-07-13 13:19:43 +00:00
|
|
|
vehicle_roll(t);
|
|
|
|
} else {
|
|
|
|
vehicle_accel(t);
|
|
|
|
}
|
|
|
|
}
|