A*寻路算法

//http://poj.org/problem?id=2449



#include <iostream>
#include <utility>
#include <vector>
#include <queue>
using namespace std;

typedef pair<int, int> pii;//距离,顶点
struct Arc
{
	int vex;
	int weight;
};

const int MAX_VEX_NUM = 1010;
const int MAX = 1<<20;

vector<Arc> Adjlist[MAX_VEX_NUM];
vector<Arc> AdjlistRev[MAX_VEX_NUM];//反向图,求h(n)
int h[MAX_VEX_NUM];

int S,T,K;

void Init()
{
	int N,M;
	int A,B,T;
	cin>>N>>M;
	int i;
	for(i = 0; i < N; i++)
	{
		Adjlist[i].clear();
		AdjlistRev[i].clear();
	}
	Arc arc;
	for(i = 0; i < M; i++)
	{
		cin>>A>>B>>T;
		arc.vex = B;
		arc.weight = T;
		Adjlist[A].push_back(arc);
		arc.vex = A;
		AdjlistRev[B].push_back(arc);
	}
	cin>>S>>::T>>K;
}

//计算h[n]
void Dijkstra(int u)
{
	priority_queue<pii, vector<pii>, greater<pii> > pq_dij;
	bool traversed[MAX_VEX_NUM];
	memset(traversed, false, MAX_VEX_NUM * sizeof(bool));
	for(int i = 0; i < MAX_VEX_NUM; i++)
	{
		h[i] = MAX;
	}
	
	h[u] = 0;
	pq_dij.push(make_pair(h[u], u));
	while(!pq_dij.empty())
	{
		pii pq_node = pq_dij.top();
		pq_dij.pop();
		int v = pq_node.second;
		if(traversed[v])
			continue;
		traversed[v] = true;
		for(vector<Arc>::iterator iter = AdjlistRev[v].begin(); iter != AdjlistRev[v].end(); iter++)
		{
			int w = iter->vex;
			int weight = iter->weight;
			if(h[w] > h[v] + weight)
			{
				h[w] = h[v] + weight;
				pq_dij.push(make_pair(h[w], w));
			}
		}
	}
}

int Astar(int s, int t, int k)
{
	priority_queue<pii, vector<pii>, greater<pii> > pq_astar;
	int cnt[MAX_VEX_NUM];
	memset(cnt, 0, MAX_VEX_NUM * sizeof(int));
	pq_astar.push(make_pair(h[s], s));
	
	while(!pq_astar.empty())
	{
		pii node = pq_astar.top();
		pq_astar.pop();
		int u = node.second;
		int cost = node.first;
		cnt[u]++;
		if(cnt[t] == k)
			return cost;
		if(cnt[u] > k)
			continue;
		for(vector<Arc>::iterator iter = Adjlist[u].begin(); iter != Adjlist[u].end(); iter++)
		{
			int v = iter->vex;
			int weight = iter->weight;
			if(h[v] != MAX)
			{
				pii adjArc = make_pair(cost - h[u] + weight + h[v], v);
				pq_astar.push(adjArc);
			}
		}
	}
	return -1;
}


int main()
{
	//freopen("in.txt", "r", stdin);

	Init();
	Dijkstra(T);

	if(S == T)
		K++;
	cout<<Astar(S, T, K)<<endl;
	return 0;
}