A*寻路算法代码,C代码库,德仔网

代码语言
代码分类
【C】 A*寻路算法
作者: / 发布于2011/1/17/ 775
<div>//http://poj.org/problem?id=2449</div>
<div></div>
#include <iostream>
#include <utility>
#include <vector>
#include <queue>
using namespace std;</div>
<div>typedef pair<int, int> pii;//距离,顶点
struct Arc
{
int vex;
int weight;
};</div>
<div>const int MAX_VEX_NUM = 1010;
const int MAX = 1<<20;</div>
<div>vector<Arc> Adjlist[MAX_VEX_NUM];
vector<Arc> AdjlistRev[MAX_VEX_NUM];//反向图,求h(n)
int h[MAX_VEX_NUM];</div>
<div>int S,T,K;</div>
<div>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;
}
<div>//计算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));
}
}
}
}
<div>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;
}
<div>
int main()
{
//freopen("in.txt", "r", stdin);</div>
<div>Init();
Dijkstra(T);</div>
if(S == T)
K++;
cout<<Astar(S, T, K)<<endl;
return 0;
}
试试其它关键字
　寻路算法　
同语言下
可能有用的
贡献的其它代码Label