115 lines
2.5 KiB
C
115 lines
2.5 KiB
C
#include "control.h"
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
struct collcheck {
|
|
int collision;
|
|
double x,y;
|
|
};
|
|
|
|
void check_coll(gpointer key, gpointer value, gpointer ud){
|
|
double dx,dy;
|
|
dx = value.x - ud.x;
|
|
dy = value.y - ud.y;
|
|
*ud.collision |= (sqrt(dx*dx + dy*dy) - value.rad) < 1;
|
|
}
|
|
|
|
void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, double* a, double* aw){
|
|
double a1,a2;
|
|
double w1,w2;
|
|
double dt1,dt2;
|
|
|
|
dt1 = (t2->ts - t1->ts);
|
|
dt2 = (t3->ts - t2->ts);
|
|
|
|
a1 = (t2->speed - t1->speed)/dt1;
|
|
a2 = (t3->speed - t2->speed)/dt2;
|
|
|
|
if(a1/a2 > 1.1 || a1/a2 < 0.9){
|
|
fprintf(stderr,"Inkonsistent accelerations, inaccurate results possible\n");
|
|
}
|
|
|
|
*a = (a1 + a2)/2;
|
|
|
|
w1 = (t2->dir - t1->dir)/dt1;
|
|
w2 = (t3->dir - t2->dir)/dt2;
|
|
|
|
*aw = 2*(w2 - w1)/(dt1 + dt2);
|
|
}
|
|
|
|
void dgl(trial* t, vehicle* after, vehicle* before, timestamp deltat){
|
|
double a,k;
|
|
vehicle tmp = *before;
|
|
vehicle diff;
|
|
timestamp dt = 10;
|
|
int i,end=deltat/dt;
|
|
struct collcheck collision;
|
|
|
|
if((deltat/dt)*dt != deltat)
|
|
fprintf(stderr,"deltat not a multiple of %d, possibly inaccurate results\n",dt);
|
|
|
|
/*TODO: find out a,k,aw*/
|
|
a = t->map.a;
|
|
k = t->map.k;
|
|
aw = t->map.aw;
|
|
case(before.turn){
|
|
case(TURN_HARD_LEFT): wsoll = t->map.max_hard_turn; break;
|
|
case(TURN_LEFT): wsoll = t->map.max_turn; break;
|
|
case(TURN_STRAIGHT): wsoll = 0; break;
|
|
case(TURN_RIGHT): wsoll = -t->map.max_turn; break;
|
|
case(TURN_HARD_RIGHT): wsoll = -t->map.max_hard_turn; break;
|
|
}
|
|
aw *= (w > wsoll) ? -1 : 1;
|
|
|
|
for(i = 0;i<end;i++){
|
|
|
|
/*calculate derivative*/
|
|
diff.x = cos(tmp.dir)*tmp.speed;
|
|
diff.y = sin(tmp.dir)*tmp.speed;
|
|
diff.speed = fmax(-tmp.speed,a - k*tmp.speed*tmp.speed);
|
|
/*maybe its logistic: k*w*(wsoll - w)*/
|
|
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_foreach(t->map.solid->objects,check_coll,&collision);
|
|
if(collision){
|
|
return -1;
|
|
}
|
|
|
|
}
|
|
|
|
after.x = tmp.x;
|
|
after.y = tmp.y;
|
|
after.speed = tmp.speed;
|
|
after.w = tmp.w;
|
|
after.dir = tmp.dir;
|
|
|
|
}
|
|
|
|
|
|
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) == -1) continue;
|
|
/*TODO: Bewertung*/
|
|
}
|
|
}
|
|
}
|