92 lines
1.8 KiB
C
92 lines
1.8 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, vr, angle, vdirx, vdiry;
|
|
|
|
/* todo: simulate movement */
|
|
v = t->sim.tm.vehicle;
|
|
|
|
tm = &t->sim.tm;
|
|
|
|
vr = sqrt(v.x*v.x + v.y*v.y);
|
|
if (vr < 4) return; /* done */
|
|
xpot = 0.1 * v.x/vr;
|
|
ypot = 0.1 * v.y/vr;
|
|
vr = vr*vr;
|
|
vdirx = cos(v.dir);
|
|
vdiry = sin(v.dir);
|
|
|
|
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:
|
|
r = (o->x * o->x + o->y + o->y);
|
|
if ((vdirx*(o->x-v.x) + vdiry*(o->y-v.y)) < 0) continue;
|
|
sigma = 2 * o->rad + 1;
|
|
h = 1;
|
|
break;
|
|
case CRATER:
|
|
r = (o->x * o->x + o->y + o->y);
|
|
if ((vdirx*(o->x-v.x) + vdiry*(o->y-v.y)) < 0) continue;
|
|
sigma = 2 * o->rad + 3;
|
|
h = 1;
|
|
break;
|
|
case MARTIAN:
|
|
sigma = 6;
|
|
h = 1;
|
|
break;
|
|
}
|
|
|
|
r = sqrt((o->x - v.x) * (o->x - v.x) + (o->y - v.y) * (o->y - v.y)) - o->rad;
|
|
r = r*r;
|
|
// r = r*r*r * 10;
|
|
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);
|
|
}
|
|
}
|
|
// fprintf(stderr, "Angle: %f, objects: %u\n", angle, tm->objects->len);
|
|
angle = fabs(angle);
|
|
if (angle > 60) {
|
|
vehicle_break(t);
|
|
} else if (angle > 50) {
|
|
vehicle_roll(t);
|
|
} else {
|
|
vehicle_accel(t);
|
|
}
|
|
}
|