2008-07-13 16:48:14 +00:00
|
|
|
#include <stdio.h>
|
|
|
|
#include <math.h>
|
2008-07-13 21:13:44 +00:00
|
|
|
#include "lookahead.h"
|
2008-07-13 16:48:14 +00:00
|
|
|
|
2008-07-13 17:42:36 +00:00
|
|
|
|
2008-07-13 20:05:06 +00:00
|
|
|
double turn2w(trial* t, turn_t turn) {
|
2008-07-13 17:42:36 +00:00
|
|
|
switch(turn){
|
|
|
|
case(TURN_HARD_LEFT): return t->map.max_hard_turn/1000.0;
|
|
|
|
case(TURN_LEFT): return t->map.max_turn/1000.0;
|
|
|
|
case(TURN_STRAIGHT): return 0;
|
|
|
|
case(TURN_RIGHT): return -t->map.max_turn/1000.0;
|
|
|
|
case(TURN_HARD_RIGHT): return -t->map.max_hard_turn/1000.0;
|
2008-07-13 20:05:06 +00:00
|
|
|
default:
|
|
|
|
fprintf(stderr,"Unknown turn state in turn2w\n");
|
|
|
|
return 0;
|
2008-07-13 17:42:36 +00:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2008-07-13 16:48:14 +00:00
|
|
|
struct collcheck {
|
|
|
|
int collision;
|
|
|
|
double x,y;
|
|
|
|
};
|
|
|
|
|
|
|
|
void check_coll(gpointer key, gpointer value, gpointer ud){
|
2008-07-13 17:42:36 +00:00
|
|
|
struct collcheck* coll;
|
|
|
|
object* ob;
|
|
|
|
|
2008-07-13 20:05:06 +00:00
|
|
|
UNUSED(key);
|
|
|
|
|
2008-07-13 17:42:36 +00:00
|
|
|
coll = (struct collcheck*) ud;
|
|
|
|
ob = (object*) value;
|
|
|
|
|
2008-07-13 16:48:14 +00:00
|
|
|
double dx,dy;
|
2008-07-13 17:42:36 +00:00
|
|
|
dx = ob->x - coll->x;
|
|
|
|
dy = ob->y - coll->y;
|
|
|
|
coll->collision |= (sqrt(dx*dx + dy*dy) - ob->rad) < 1;
|
2008-07-13 16:48:14 +00:00
|
|
|
}
|
|
|
|
|
2008-07-13 16:56:28 +00:00
|
|
|
void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
|
2008-07-13 16:48:14 +00:00
|
|
|
double a1,a2;
|
|
|
|
double w1,w2;
|
|
|
|
double dt1,dt2;
|
2008-07-13 17:42:36 +00:00
|
|
|
vehicle *v1, *v2, *v3;
|
|
|
|
|
|
|
|
v1 = &t1->vehicle;
|
|
|
|
v2 = &t2->vehicle;
|
|
|
|
v3 = &t3->vehicle;
|
2008-07-13 16:48:14 +00:00
|
|
|
|
|
|
|
dt1 = (t2->ts - t1->ts);
|
|
|
|
dt2 = (t3->ts - t2->ts);
|
|
|
|
|
2008-07-13 21:57:10 +00:00
|
|
|
a1 = 1000*(v2->speed - v1->speed)/dt1;
|
|
|
|
a2 = 1000*(v3->speed - v2->speed)/dt2;
|
2008-07-13 16:48:14 +00:00
|
|
|
|
|
|
|
if(a1/a2 > 1.1 || a1/a2 < 0.9){
|
2008-07-13 17:42:36 +00:00
|
|
|
fprintf(stderr,"Inconsistent accelerations: a1: %f a2: %f. inaccurate results possible\n",a1,a2);
|
2008-07-13 16:48:14 +00:00
|
|
|
}
|
|
|
|
|
2008-07-13 16:56:28 +00:00
|
|
|
m->a = (a1 + a2)/2;
|
|
|
|
m->k = m->a/m->max_speed/m->max_speed;
|
2008-07-13 16:48:14 +00:00
|
|
|
|
2008-07-13 17:42:36 +00:00
|
|
|
w1 = (v2->dir - v1->dir)/dt1;
|
|
|
|
w2 = (v3->dir - v2->dir)/dt2;
|
2008-07-13 16:48:14 +00:00
|
|
|
|
2008-07-13 16:56:28 +00:00
|
|
|
m->aw = 2*(w2 - w1)/(dt1 + dt2);
|
2008-07-13 16:48:14 +00:00
|
|
|
}
|
|
|
|
|
2008-07-13 20:52:08 +00:00
|
|
|
|
|
|
|
void radar_dgl(trial* t, timestamp ts){
|
|
|
|
timestamp step = ts- t->sim.tm.ts;
|
|
|
|
if (step > 0){
|
2008-07-13 21:36:04 +00:00
|
|
|
dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,1,step);
|
2008-07-13 20:52:08 +00:00
|
|
|
t->sim.tm.ts += step;
|
|
|
|
t->sim.steps++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*updates the simulation state when new telemetry arrives*/
|
|
|
|
void sim_update(trial* t){
|
|
|
|
|
|
|
|
telemetry *tm,*tmp;
|
|
|
|
tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
|
2008-07-13 21:12:52 +00:00
|
|
|
fprintf(stderr, "Time diff: %i\n", tm->ts - t->sim.tm.ts);
|
2008-07-13 20:52:08 +00:00
|
|
|
/*run simulation up to new telemetry timestamp*/
|
|
|
|
radar_dgl(t,tm->ts);
|
|
|
|
|
|
|
|
/*calculate misspredict*/
|
2008-07-13 22:02:01 +00:00
|
|
|
fprintf(stderr, "Miss predict: dx=%f, dy=%f, ddir=%f (%f %f), steps=%u\n",
|
2008-07-13 20:52:08 +00:00
|
|
|
t->sim.tm.vehicle.x - t->vehicle.x,
|
|
|
|
t->sim.tm.vehicle.y - t->vehicle.y,
|
2008-07-13 22:02:01 +00:00
|
|
|
fixangle(t->sim.tm.vehicle.dir - t->vehicle.dir),
|
2008-07-13 20:52:08 +00:00
|
|
|
t->sim.steps);
|
2008-07-13 21:36:04 +00:00
|
|
|
// fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
|
2008-07-13 21:57:10 +00:00
|
|
|
fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
|
2008-07-13 20:52:08 +00:00
|
|
|
|
|
|
|
/*update simulation vehicle from telemetry: everything and calculate w*/
|
|
|
|
t->sim.tm.vehicle = tm->vehicle;
|
|
|
|
g_array_set_size(t->sim.tm.objects, 0);
|
|
|
|
g_array_append_vals(t->sim.tm.objects, tm->objects->data, tm->objects->len);
|
|
|
|
t->sim.steps = 0;
|
|
|
|
if(t->telemetry.length > 1){
|
|
|
|
tmp = (telemetry*) g_queue_peek_tail_link(&t->telemetry)->prev->data;
|
|
|
|
t->sim.tm.vehicle.w = (tm->vehicle.dir - tmp->vehicle.dir)/(tm->ts - tmp->ts);
|
|
|
|
}
|
2008-07-13 21:36:04 +00:00
|
|
|
// fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
|
2008-07-13 21:57:10 +00:00
|
|
|
fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
|
2008-07-13 20:52:08 +00:00
|
|
|
}
|
|
|
|
|
2008-07-13 19:51:00 +00:00
|
|
|
int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){
|
2008-07-13 17:42:36 +00:00
|
|
|
double a,k,aw;
|
|
|
|
double wsoll;
|
2008-07-13 16:48:14 +00:00
|
|
|
vehicle tmp = *before;
|
|
|
|
vehicle diff;
|
2008-07-13 19:51:00 +00:00
|
|
|
timestamp dt = h;
|
2008-07-13 16:48:14 +00:00
|
|
|
int i,end=deltat/dt;
|
|
|
|
struct collcheck collision;
|
|
|
|
|
2008-07-13 17:42:36 +00:00
|
|
|
if(deltat < dt)
|
|
|
|
return 0;
|
|
|
|
|
2008-07-13 16:48:14 +00:00
|
|
|
a = t->map.a;
|
|
|
|
k = t->map.k;
|
2008-07-13 17:42:36 +00:00
|
|
|
|
|
|
|
wsoll = turn2w(t,before->turn);
|
2008-07-13 16:48:14 +00:00
|
|
|
|
2008-07-13 19:51:00 +00:00
|
|
|
for(i = 0;i<=end;i++){
|
2008-07-13 21:36:04 +00:00
|
|
|
if(i == end) dt = deltat - end*dt;
|
2008-07-13 21:57:10 +00:00
|
|
|
aw = t->map.aw * ((before->w > wsoll) ? -1 : 1);
|
2008-07-13 16:48:14 +00:00
|
|
|
|
|
|
|
/*calculate derivative*/
|
2008-07-13 21:57:10 +00:00
|
|
|
diff.x = cos(tmp.dir*M_PI/180)*tmp.speed/1000;
|
|
|
|
diff.y = sin(tmp.dir*M_PI/180)*tmp.speed/1000;
|
|
|
|
diff.speed = a - k*tmp.speed*tmp.speed;
|
2008-07-13 21:36:04 +00:00
|
|
|
// diff.speed = fmax(-tmp.speed/1000,a - k*tmp.speed*tmp.speed*1e-6);
|
2008-07-13 16:48:14 +00:00
|
|
|
diff.w = (tmp.w == wsoll) ? 0 : aw;
|
|
|
|
diff.dir = tmp.w;
|
|
|
|
/*Euler Step*/
|
|
|
|
tmp.x += dt*diff.x;
|
|
|
|
tmp.y += dt*diff.y;
|
2008-07-13 21:36:04 +00:00
|
|
|
// tmp.speed += (tmp.speed > 0) ? 1000*dt*diff.speed : 0;
|
2008-07-13 21:57:10 +00:00
|
|
|
tmp.speed = fmax(0, tmp.speed + dt*diff.speed);
|
2008-07-13 16:48:14 +00:00
|
|
|
tmp.w += dt*diff.w;
|
2008-07-13 19:51:00 +00:00
|
|
|
tmp.dir += dt*diff.dir;
|
2008-07-13 16:48:14 +00:00
|
|
|
|
|
|
|
/*check for collisions TODO: optimize*/
|
|
|
|
collision.x = tmp.x;
|
|
|
|
collision.y = tmp.y;
|
|
|
|
collision.collision = 0;
|
2008-07-13 21:36:04 +00:00
|
|
|
// g_hash_table_foreach(t->map.solid_objects,check_coll,&collision);
|
|
|
|
// if(collision.collision){
|
|
|
|
// return -1;
|
|
|
|
// }
|
2008-07-13 16:48:14 +00:00
|
|
|
|
|
|
|
}
|
|
|
|
|
2008-07-13 19:51:00 +00:00
|
|
|
|
2008-07-13 17:42:36 +00:00
|
|
|
after->x = tmp.x;
|
|
|
|
after->y = tmp.y;
|
|
|
|
after->speed = tmp.speed;
|
|
|
|
after->w = tmp.w;
|
2008-07-13 22:02:01 +00:00
|
|
|
after->dir = fixangle(tmp.dir);
|
2008-07-13 17:42:36 +00:00
|
|
|
return 0;
|
2008-07-13 16:48:14 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void lookahead(trial* t){
|
|
|
|
int accel, turn;
|
|
|
|
vehicle current, future;
|
|
|
|
|
|
|
|
current = t->vehicle;
|
|
|
|
|
|
|
|
for(accel=0; accel < 3;accel++){
|
|
|
|
current.accel = accel;
|
|
|
|
for(turn=0; turn < 5; turn++){
|
|
|
|
current.turn = turn;
|
2008-07-13 19:57:27 +00:00
|
|
|
if(dgl(t,&future,¤t,20,10) == -1) continue;
|
2008-07-13 16:48:14 +00:00
|
|
|
/*TODO: Bewertung*/
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|