128 lines
2.1 KiB
C++
128 lines
2.1 KiB
C++
/*
|
|
compilation command:
|
|
g++ -O3 -shared -std=c++11 -fPIC `python3 -m pybind11 --includes` astar.cpp -o astar`python3-config --extension-suffix`
|
|
*/
|
|
|
|
#include <algorithm>
|
|
#include <vector>
|
|
#include <queue>
|
|
#include <unordered_map>
|
|
|
|
#include <pybind11/pybind11.h>
|
|
#include <pybind11/stl.h>
|
|
|
|
#define MAX(a,b) (((a)>(b))?(a):(b))
|
|
|
|
namespace py = pybind11;
|
|
|
|
using namespace std;
|
|
|
|
int deltas[4] = {-16, 16, -1, 1};
|
|
|
|
union bytes
|
|
{
|
|
int i;
|
|
char b[4];
|
|
};
|
|
|
|
struct result
|
|
{
|
|
double cost;
|
|
int players;
|
|
};
|
|
|
|
struct result cost(const vector<double> &t, int players, int exits, int m, int l)
|
|
{
|
|
int i, n, dn;
|
|
double c, v;
|
|
union bytes p, e;
|
|
struct result r;
|
|
p.i = players;
|
|
e.i = exits;
|
|
dn = deltas[m];
|
|
c = 1;
|
|
for(i = 0; i < 4; ++i)
|
|
{
|
|
n = p.b[i];
|
|
if(n == e.b[i]) continue;
|
|
n += dn;
|
|
v = t[n + i * l];
|
|
if(v < 0) continue;
|
|
p.b[i] = n;
|
|
c += v;
|
|
}
|
|
r.cost = c;
|
|
r.players = p.i;
|
|
return r;
|
|
}
|
|
|
|
double heuristic(int u, int d)
|
|
{
|
|
int dx[4], i;
|
|
union bytes a, b;
|
|
a.i = u;
|
|
b.i = d;
|
|
for(i = 0; i < 4; ++i)
|
|
{
|
|
dx[i] = abs(a.b[i] % 16 - b.b[i] % 16);
|
|
}
|
|
return MAX(dx[0], dx[2]) + MAX(dx[1], dx[3]);
|
|
}
|
|
|
|
vector<int> astar(const vector<double> &t, int s, int d, int l)
|
|
{
|
|
int m, n, u;
|
|
double c, cu;
|
|
struct result r;
|
|
vector<int> path;
|
|
typedef pair<double, int> element;
|
|
priority_queue<element, vector<element>, greater<element> > q;
|
|
unordered_map<int, int> prev, move;
|
|
unordered_map<int, double> dist;
|
|
|
|
q.emplace(0, s);
|
|
prev[s] = s;
|
|
dist[s] = 0;
|
|
move[s] = 0;
|
|
|
|
while(!q.empty())
|
|
{
|
|
u = q.top().second;
|
|
q.pop();
|
|
if(u == d) break;
|
|
cu = dist[u];
|
|
for(m = 0; m < 4; ++m)
|
|
{
|
|
r = cost(t, u, d, m, l);
|
|
c = r.cost;
|
|
n = r.players;
|
|
if(u == n) continue;
|
|
c += cu;
|
|
if(dist.find(n) == dist.end() || c < dist[n])
|
|
{
|
|
q.emplace(c + heuristic(n, d), n);
|
|
prev[n] = u;
|
|
dist[n] = c;
|
|
move[n] = m;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(u == d)
|
|
{
|
|
while(u != s)
|
|
{
|
|
path.push_back(move[u]);
|
|
u = prev[u];
|
|
}
|
|
reverse(path.begin(), path.end());
|
|
}
|
|
|
|
return path;
|
|
}
|
|
|
|
PYBIND11_MODULE(astar, m)
|
|
{
|
|
m.def("astar", &astar, "");
|
|
}
|