176 lines
4.0 KiB
C
176 lines
4.0 KiB
C
#include "lookahead.h"
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
|
|
double turn2w(trial* t, turn_t turn) {
|
|
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;
|
|
default:
|
|
fprintf(stderr,"Unknown turn state in turn2w\n");
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
struct collcheck {
|
|
int collision;
|
|
double x,y;
|
|
};
|
|
|
|
void check_coll(gpointer key, gpointer value, gpointer ud){
|
|
struct collcheck* coll;
|
|
object* ob;
|
|
|
|
UNUSED(key);
|
|
|
|
coll = (struct collcheck*) ud;
|
|
ob = (object*) value;
|
|
|
|
double dx,dy;
|
|
dx = ob->x - coll->x;
|
|
dy = ob->y - coll->y;
|
|
coll->collision |= (sqrt(dx*dx + dy*dy) - ob->rad) < 1;
|
|
}
|
|
|
|
void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
|
|
double a1,a2;
|
|
double w1,w2;
|
|
double dt1,dt2;
|
|
vehicle *v1, *v2, *v3;
|
|
|
|
v1 = &t1->vehicle;
|
|
v2 = &t2->vehicle;
|
|
v3 = &t3->vehicle;
|
|
|
|
dt1 = (t2->ts - t1->ts);
|
|
dt2 = (t3->ts - t2->ts);
|
|
|
|
a1 = (v2->speed - v1->speed)/dt1;
|
|
a2 = (v3->speed - v2->speed)/dt2;
|
|
|
|
if(a1/a2 > 1.1 || a1/a2 < 0.9){
|
|
fprintf(stderr,"Inconsistent accelerations: a1: %f a2: %f. inaccurate results possible\n",a1,a2);
|
|
}
|
|
|
|
m->a = (a1 + a2)/2;
|
|
m->k = m->a/m->max_speed/m->max_speed;
|
|
|
|
w1 = (v2->dir - v1->dir)/dt1;
|
|
w2 = (v3->dir - v2->dir)/dt2;
|
|
|
|
m->aw = 2*(w2 - w1)/(dt1 + dt2);
|
|
}
|
|
|
|
|
|
void radar_dgl(trial* t, timestamp ts){
|
|
timestamp step = ts- t->sim.tm.ts;
|
|
if (step > 0){
|
|
dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,step,1);
|
|
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);
|
|
fprintf(stderr, "Time diff: %i\n", tm->ts - t->sim.tm.ts);
|
|
/*run simulation up to new telemetry timestamp*/
|
|
radar_dgl(t,tm->ts);
|
|
|
|
/*calculate misspredict*/
|
|
fprintf(stderr, "Miss predict: dx=%f, dy=%f, ddir=%f, steps=%u\n",
|
|
t->sim.tm.vehicle.x - t->vehicle.x,
|
|
t->sim.tm.vehicle.y - t->vehicle.y,
|
|
t->sim.tm.vehicle.dir - t->vehicle.dir,
|
|
t->sim.steps);
|
|
|
|
/*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);
|
|
}
|
|
}
|
|
|
|
int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat){
|
|
double a,k,aw;
|
|
double wsoll;
|
|
vehicle tmp = *before;
|
|
vehicle diff;
|
|
timestamp dt = h;
|
|
int i,end=deltat/dt;
|
|
struct collcheck collision;
|
|
|
|
if(deltat < dt)
|
|
return 0;
|
|
|
|
a = t->map.a;
|
|
k = t->map.k;
|
|
aw = t->map.aw;
|
|
|
|
wsoll = turn2w(t,before->turn);
|
|
aw *= (before->w > wsoll) ? -1 : 1;
|
|
|
|
for(i = 0;i<=end;i++){
|
|
if(i == end) dt = deltat - (end-1)*dt;
|
|
|
|
/*calculate derivative*/
|
|
diff.x = cos(tmp.dir)*tmp.speed/1000;
|
|
diff.y = sin(tmp.dir)*tmp.speed/1000;
|
|
diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed*1e-6);
|
|
diff.w = (tmp.w == wsoll) ? 0 : aw;
|
|
diff.dir = tmp.w;
|
|
/*Euler Step*/
|
|
tmp.x += dt*diff.x;
|
|
tmp.y += dt*diff.y;
|
|
tmp.speed += (tmp.speed > 0) ? dt*diff.speed : 0;
|
|
tmp.w += dt*diff.w;
|
|
tmp.dir += dt*diff.dir;
|
|
|
|
/*check for collisions TODO: optimize*/
|
|
collision.x = tmp.x;
|
|
collision.y = tmp.y;
|
|
collision.collision = 0;
|
|
g_hash_table_foreach(t->map.solid_objects,check_coll,&collision);
|
|
if(collision.collision){
|
|
return -1;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
after->x = tmp.x;
|
|
after->y = tmp.y;
|
|
after->speed = tmp.speed;
|
|
after->w = tmp.w;
|
|
after->dir = tmp.dir;
|
|
return 0;
|
|
}
|
|
|
|
|
|
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;
|
|
if(dgl(t,&future,¤t,20,10) == -1) continue;
|
|
/*TODO: Bewertung*/
|
|
}
|
|
}
|
|
}
|