Pavel
/
tipc21
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

add cost function to alryslib.c

This commit is contained in:
Pavel 2021-10-16 12:11:39 +02:00
parent 14a4aa60c4
commit 9b8a5936ba
1 changed files with 30 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
alrys/alryslib.c
View File

@ -103,6 +103,36 @@ void segments(double x1, double y1, double x2, double y2, double *lx, double *ly
++*ls;
}
double cost(double x1, double y1, double x2, double y2)
{
int c1, c2, k, ls;
double result, x, y, lx[128], ly[128];
c1 = data[(int)(x1) + (int)(y1) * 146];
c2 = data[(int)(x2) + (int)(y2) * 146];
if(c1 == 1 || c2 == 1 || c1 == 3 || c2 == 3) return -1.0;
segments(x1, y1, x2, y2, lx, ly, &ls);
result = 0.1;
x = x1;
y = y1;
for(k = 1; k < ls; ++k)
{
c1 = data[(int)(MIN(x, lx[k])) + (int)(MIN(y, ly[k])) * 146];
c2 = data[(int)(lx[k]) + (int)(ly[k]) * 146];
if(c1 == 1 || c2 == 1 || c1 == 3 || c2 == 3) return -1.0;
result += hypot(lx[k] - x, ly[k] - y) * (0.2 + (c1 == 5) * 0.1 + (c1 == 4) * 0.2 + (c1 == 2) * 0.3);
x = lx[k];
y = ly[k];
}
return result;
}
void forward(struct STATE *state, double l)
{
int c1, c2, i, k, xr, yr, in, ls;