From 9b8a5936bae2bd70e4fe59c6d12d4d1bc46f0258 Mon Sep 17 00:00:00 2001 From: Pavel Date: Sat, 16 Oct 2021 12:11:39 +0200 Subject: [PATCH] add cost function to alryslib.c --- alrys/alryslib.c | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/alrys/alryslib.c b/alrys/alryslib.c index 5aba951..759d4eb 100644 --- a/alrys/alryslib.c +++ b/alrys/alryslib.c @@ -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;