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;
}
相关文章
|
算法 定位技术
Threejs中使用A*算法寻路导航,Threejs室内室外地图导航
Threejs中使用A*算法寻路导航,Threejs室内室外地图导航
1604 0
Threejs中使用A*算法寻路导航,Threejs室内室外地图导航
|
4月前
|
算法 机器人 定位技术
【机器人路径规划】基于流场寻路算法(Flow Field Pathfinding)的机器人路径规划(Python代码实现)
【机器人路径规划】基于流场寻路算法(Flow Field Pathfinding)的机器人路径规划(Python代码实现)
232 4
|
算法
互动游戏解决遇到问题之基于射线投射寻路算法的问题如何解决
互动游戏解决遇到问题之基于射线投射寻路算法的问题如何解决
166 0
|
算法
Threejs中使用astar(A*)算法寻路导航,Threejs寻路定位导航
Threejs中使用astar(A*)算法寻路导航,Threejs寻路定位导航
1022 0
Threejs中使用astar(A*)算法寻路导航,Threejs寻路定位导航
|
算法 定位技术 图形学
unity3d寻路算法
unity3d寻路算法
471 8
|
Dart 算法 数据可视化
用flutter实现五种寻路算法的可视化效果,快来看看!
半年前我写了一篇有关排序算法可视化的文章,挺有意思,还被张风捷特烈-张老师收录进了FlutterUnit,今天让我们再来做一个有关寻路算法的可视化效果吧!
|
存储 人工智能 算法
Unity 实现A* 寻路算法
Unity 实现A* 寻路算法
771 2
Unity 实现A* 寻路算法
|
算法
cocoscreator A* 寻路算法
cocoscreator A* 寻路算法
549 0
|
算法 定位技术
“ 探索迷局:解密广度寻路算法 “(二)
“ 探索迷局:解密广度寻路算法 “
|
存储 算法 定位技术
“ 探索迷局:解密广度寻路算法 “(一)
“ 探索迷局:解密广度寻路算法 “