224 lines
7.4 KiB
C++
224 lines
7.4 KiB
C++
#include <algorithm>
|
|
#include <vector>
|
|
#include <queue>
|
|
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <cmath>
|
|
#include <cerrno>
|
|
|
|
#include "alryslib.c"
|
|
|
|
using namespace std;
|
|
|
|
int index(int x, int a, int y, int b, int width)
|
|
{
|
|
return x * 3 + a + 1 + (y * 3 + b + 1) * width;
|
|
}
|
|
|
|
double cost(int s, int d, int width, int size, int boat)
|
|
{
|
|
int a, b, c1, c2, k, ls;
|
|
double x, x1, x2, y, y1, y2;
|
|
double result;
|
|
|
|
if(s < 0 || s >= size) return -1.0;
|
|
if(d < 0 || d >= size) return -1.0;
|
|
|
|
result = 0.1;
|
|
|
|
a = d % width;
|
|
b = d / width;
|
|
|
|
if(a % 3 == 1) result -= 1.0e-10;
|
|
if(b % 3 == 1) result -= 1.0e-10;
|
|
|
|
x2 = a / 3 + (a % 3 - 1) * 1.0e-14;
|
|
y2 = b / 3 + (b % 3 - 1) * 1.0e-14;
|
|
|
|
|
|
c2 = data[(int)x2 + (int)y2 * (width / 3)];
|
|
if(c2 == 1 || (c2 == 3 && !boat)) return -1.0;
|
|
|
|
a = s % width;
|
|
b = s / width;
|
|
|
|
if(a % 3 == 1) result -= 1.0e-10;
|
|
if(b % 3 == 1) result -= 1.0e-10;
|
|
|
|
x1 = a / 3 + (a % 3 - 1) * 1.0e-14;
|
|
y1 = b / 3 + (b % 3 - 1) * 1.0e-14;
|
|
|
|
segments(x1, y1, x2, y2, &ls);
|
|
x = x1;
|
|
y = y1;
|
|
|
|
for(k = 1; k < ls; ++k)
|
|
{
|
|
c1 = data[(int)MIN(x, lx[k]) + (int)MIN(y, ly[k]) * (width / 3)];
|
|
c2 = data[(int)lx[k] + (int)ly[k] * (width / 3)];
|
|
|
|
if(c1 == 1 || c2 == 1 || ((c1 == 3 || c2 == 3) && !boat)) 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 astar(int s, int d, int width, int size, int boat)
|
|
{
|
|
int a, b, i, j, u, n, *prev;
|
|
double c, x, y, *dist;
|
|
vector<int> path;
|
|
typedef pair<double, int> element;
|
|
priority_queue<element, vector<element>, greater<element> > q;
|
|
|
|
prev = new int[size];
|
|
dist = new double[size];
|
|
|
|
for(i = 0; i < size; ++i)
|
|
{
|
|
prev[i] = -1;
|
|
dist[i] = 1.0e9;
|
|
}
|
|
|
|
q.emplace(0, s);
|
|
prev[s] = s;
|
|
dist[s] = 0;
|
|
|
|
while(!q.empty())
|
|
{
|
|
u = q.top().second;
|
|
q.pop();
|
|
|
|
if(u == d) break;
|
|
|
|
a = u % width;
|
|
b = u / width;
|
|
for(i = -120; i < 121; ++i)
|
|
{
|
|
for(j = -120; j < 121; ++j)
|
|
{
|
|
if(i == 0 && j == 0) continue;
|
|
n = a + i + (b + j) * width;
|
|
c = cost(u, n, width, size, boat);
|
|
if(c < 0) continue;
|
|
c += dist[u];
|
|
if(c < dist[n])
|
|
{
|
|
q.emplace(c, n);
|
|
prev[n] = u;
|
|
dist[n] = c;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if(u == d)
|
|
{
|
|
while(u != s)
|
|
{
|
|
path.push_back(u);
|
|
u = prev[u];
|
|
}
|
|
path.push_back(s);
|
|
reverse(path.begin(), path.end());
|
|
printf("250 0 0\n");
|
|
for(auto u : path)
|
|
{
|
|
a = u % width;
|
|
b = u / width;
|
|
x = a / 3;
|
|
a = a % 3 - 1;
|
|
y = b / 3;
|
|
b = b % 3 - 1;
|
|
printf("%7.3f %2d %6.3f %2d\n", x, a, y, b);
|
|
}
|
|
}
|
|
|
|
delete[] dist;
|
|
delete[] prev;
|
|
}
|
|
|
|
int main(int argc, char *argv[])
|
|
{
|
|
char *end;
|
|
long number;
|
|
int s, d, width, size;
|
|
|
|
errno = 0;
|
|
number = (argc == 2) ? strtol(argv[1], &end, 10) : -1;
|
|
if(errno != 0 || end == argv[1] || number < 0 || number > 15)
|
|
{
|
|
fprintf(stderr, "Usage: search [0-15]\n");
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
width = 146 * 3;
|
|
size = 91 * 3 * width;
|
|
|
|
switch(number)
|
|
{
|
|
/*
|
|
case 0: s = index( 49, 1, 43, 1, width); d = index( 18, -1, 39, -1, width); break;
|
|
case 1: s = index( 18, -1, 39, -1, width); d = index( 38, 1, 49, 1, width); break;
|
|
case 2: s = index( 38, 1, 49, 1, width); d = index( 9, -1, 59, -1, width); break;
|
|
case 3: s = index( 9, -1, 59, -1, width); d = index( 73, -1, 83, 1, width); break;
|
|
case 4: s = index( 73, -1, 83, 1, width); d = index(106, 1, 40, -1, width); break;
|
|
case 5: s = index(106, 1, 40, -1, width); d = index(107, 1, 69, 1, width); break;
|
|
case 6: s = index(107, 1, 69, 1, width); d = index(119, 1, 84, -1, width); break;
|
|
case 7: s = index(119, 1, 84, -1, width); d = index( 73, -1, 83, 1, width); break;
|
|
case 8: s = index( 73, -1, 83, 1, width); d = index( 2, -1, 89, 1, width); break;
|
|
case 9: s = index( 2, -1, 89, 1, width); d = index( 13, -1, 80, 1, width); break;
|
|
case 10: s = index( 13, -1, 80, 1, width); d = index( 72, 1, 83, 1, width); break;
|
|
case 11: s = index( 72, 1, 83, 1, width); d = index( 70, -1, 51, 1, width); break;
|
|
case 12: s = index( 70, -1, 51, 1, width); d = index( 70, -1, 18, -1, width); break;
|
|
case 13: s = index( 70, -1, 18, -1, width); d = index(141, 1, 71, 1, width); break;
|
|
case 14: s = index(141, 1, 71, 1, width); d = index(131, -1, 62, 1, width); break;
|
|
case 15: s = index(131, -1, 62, 1, width); d = index(115, -1, 26, 1, width); break;
|
|
*/
|
|
/*
|
|
case 0: s = index( 49, 1, 43, 1, width); d = index( 18, -1, 39, -1, width); break;
|
|
case 1: s = index( 18, -1, 39, -1, width); d = index( 38, 1, 49, 1, width); break;
|
|
case 2: s = index( 38, 1, 49, 1, width); d = index( 9, -1, 59, -1, width); break;
|
|
case 3: s = index( 9, -1, 59, -1, width); d = index( 72, 1, 83, 1, width); break;
|
|
case 4: s = index( 72, 1, 83, 1, width); d = index( 13, -1, 80, 1, width); break;
|
|
case 5: s = index( 13, -1, 80, 1, width); d = index( 2, -1, 89, 1, width); break;
|
|
case 6: s = index( 2, -1, 89, 1, width); d = index( 73, -1, 83, 1, width); break;
|
|
case 7: s = index( 73, -1, 83, 1, width); d = index(119, 1, 84, -1, width); break;
|
|
case 8: s = index(119, 1, 84, -1, width); d = index(107, 1, 69, 1, width); break;
|
|
case 9: s = index(107, 1, 69, 1, width); d = index(106, 1, 40, -1, width); break;
|
|
case 10: s = index(106, 1, 40, -1, width); d = index( 73, -1, 83, 1, width); break;
|
|
case 11: s = index( 73, -1, 83, 1, width); d = index( 70, -1, 51, 1, width); break;
|
|
case 12: s = index( 70, -1, 51, 1, width); d = index( 70, -1, 18, -1, width); break;
|
|
case 13: s = index( 70, -1, 18, -1, width); d = index(141, 1, 71, 1, width); break;
|
|
case 14: s = index(141, 1, 71, 1, width); d = index(131, -1, 62, 1, width); break;
|
|
case 15: s = index(131, -1, 62, 1, width); d = index(115, -1, 26, 1, width); break;
|
|
*/
|
|
case 0: s = index( 49, 1, 43, 1, width); d = index( 18, -1, 39, -1, width); break;
|
|
case 1: s = index( 18, -1, 39, -1, width); d = index( 38, 1, 49, 1, width); break;
|
|
case 2: s = index( 38, 1, 49, 1, width); d = index( 9, -1, 59, -1, width); break;
|
|
case 3: s = index( 9, -1, 59, -1, width); d = index( 72, 1, 83, 1, width); break;
|
|
case 4: s = index( 72, 1, 83, 1, width); d = index( 2, -1, 89, 1, width); break;
|
|
case 5: s = index( 2, -1, 89, 1, width); d = index(107, 1, 69, 1, width); break;
|
|
case 6: s = index(107, 1, 69, 1, width); d = index(106, 1, 40, -1, width); break;
|
|
case 7: s = index(106, 1, 40, -1, width); d = index(107, 1, 69, 1, width); break;
|
|
case 8: s = index(107, 1, 69, 1, width); d = index(119, 1, 84, -1, width); break;
|
|
case 9: s = index(119, 1, 84, -1, width); d = index( 73, -1, 83, 1, width); break;
|
|
case 10: s = index( 73, -1, 83, 1, width); d = index( 70, -1, 51, 1, width); break;
|
|
case 11: s = index( 70, -1, 51, 1, width); d = index( 70, -1, 18, -1, width); break;
|
|
case 12: s = index( 70, -1, 18, -1, width); d = index(141, 1, 71, 1, width); break;
|
|
case 13: s = index(141, 1, 71, 1, width); d = index(130, 1, 63, -1, width); break;
|
|
case 14: s = index(130, 1, 63, -1, width); d = index(104, -1, 56, 1, width); break;
|
|
case 15: s = index(104, -1, 56, 1, width); d = index(115, -1, 26, 1, width); break;
|
|
}
|
|
|
|
astar(s, d, width, size, number >= 14);
|
|
|
|
return EXIT_SUCCESS;
|
|
}
|