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;

}