void back_to_destination(const vec3d *v) {
double power = 0.0;
double steering = 0.0;
char msg[100];
vec3d dir;
double dis;
double targetVel;
v3sub(v,&loc,&dir);
dis = v3length(&dir);
if (dis!=0.0) v3normalize(&dir);
targetVel = dis > 15 ? 15 : dis;
if (v3dot(&dir,&front)<0.0) {
steering = 0.3 * v3dot(&dir,&left);
power = -300*(targetVel - v3length(&vel));
power = power > 300 ? 300 : power;
power = power < -300 ? -300 : power;
} else {
steering = 1.0;
power = 150;
}
sprintf(msg,"drive %.16f %.16f",power,steering);
my_send(msg);
}
int get_nearest_burden(vec3d *pos,char *id,vec3d *ret) {
int i, idx;
double len, min;
vec3d tmpV;
if (burdens_count==0)
return ERROR;
idx = 0;
min = 1000000.0;
for (i=0;i<burdens_count;i++) {
v3sub(pos,&(burdens[i].loc),&tmpV);
len = v3length(&tmpV);
if (min>len) {
idx = i;
min = len;
}
}
ret->x = burdens[idx].loc.x;
ret->y = burdens[idx].loc.y;
ret->z = burdens[idx].loc.z;
//id = burdens[idx].id;
strcpy(id,burdens[idx].id);
return NO_ERROR;
}
int check_conflict(const vec3d *src,const vec3d *dest,const vec3d *point,double dis) {
vec3d dir;
double dirLength;
vec3d pDir;
double d;
vec3d dir2;
v3sub(dest,src,&dir);
dirLength = v3length(&dir);
if (dirLength!=0.0) v3scale(&dir,1.0/dirLength,&dir);
v3sub(point,src,&pDir);
d = v3dot(&pDir,&dir);
if (d<0.0)
return 0;
if (d>dirLength)
return 0;
setVec3dToVec3d(&dir,&dir2);
v3simpleRotateY(&dir2,90);
d = fabs(v3dot(&pDir,&dir2));
if (d<dis)
return 1;
else
return 0;
}
void make_events() {
vec3d tmpV;
struct event e;
make_events_basic();
// car has arrived at the waiting point?
v3sub(&waitingPoint,&loc,&tmpV);
if (v3length(&tmpV)<1.0) {
e.id = ARRIVAL_WAITING_POINT_EVENT;
process_event(&e);
}
// car has arrived at the switch?
v3sub(&switch1,&loc,&tmpV);
if (v3length(&tmpV)<1.0) {
e.id = ARRIVAL_SWITCH_EVENT;
process_event(&e);
}
}
void
v3normal(float *v)
{
v3scale(v, 1.0f/v3length(v));
}
