136 lines
2.8 KiB
C
136 lines
2.8 KiB
C
#include "control.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;
|
|
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;
|
|
|
|
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);
|
|
}
|
|
|
|
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*/
|
|
}
|
|
}
|
|
}
|