82 lines
1.5 KiB
C
82 lines
1.5 KiB
C
|
|
#include "control.h"
|
|
|
|
#include <math.h>
|
|
#include <stdlib.h>
|
|
|
|
static double angle_for_rot(double from, double to) {
|
|
double a = to - from;
|
|
if (a > 180) a = -360 + a;
|
|
else if (a < -180) a = 360 + a;
|
|
return a;
|
|
}
|
|
|
|
void godown(trial *t) {
|
|
double C1 = 1 / sqrt(2*M_PI);
|
|
|
|
vehicle v;
|
|
telemetry *tm;
|
|
|
|
double xpot, ypot, r, angle;
|
|
|
|
/* todo: simulate movement */
|
|
v = t->vehicle;
|
|
|
|
tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
|
|
|
|
r = sqrt(v.x*v.x + v.y*v.y);
|
|
if (r < 4) return; /* done */
|
|
xpot = v.x/r;
|
|
ypot = v.y/r;
|
|
|
|
for (guint i = 0; i < tm->objects->len; i++) {
|
|
object *o = &g_array_index(tm->objects, object, i);
|
|
|
|
double sigma = 0, h = 0;
|
|
|
|
switch (o->type) {
|
|
case BOLDER:
|
|
sigma = 2 * o->rad;
|
|
h = 1;
|
|
break;
|
|
case CRATER:
|
|
sigma = 2 * o->rad;
|
|
h = 3;
|
|
break;
|
|
case MARTIAN:
|
|
sigma = 4 * o->rad;
|
|
h = 1;
|
|
break;
|
|
}
|
|
|
|
r = sqrt( (o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y) );
|
|
r = C1 * h / (sigma*sigma) / exp (r / (2*sigma) );
|
|
xpot += (o->x - v.x) * r;
|
|
ypot += (o->y - v.y) * r;
|
|
}
|
|
|
|
angle = atan2(-ypot, -xpot)*180/M_PI;
|
|
angle = angle_for_rot(v.dir, angle);
|
|
if (angle < 1) {
|
|
if (angle < -20) {
|
|
vehicle_hard_right(t);
|
|
} else {
|
|
vehicle_right(t);
|
|
}
|
|
} else if (angle > 1) {
|
|
if (angle > 20) {
|
|
vehicle_hard_left(t);
|
|
} else {
|
|
vehicle_left(t);
|
|
}
|
|
}
|
|
angle = abs(angle);
|
|
if (angle > 60) {
|
|
vehicle_break(t);
|
|
} else if (angle > 30) {
|
|
vehicle_roll(t);
|
|
} else {
|
|
vehicle_accel(t);
|
|
}
|
|
}
|