POJ2449 【第k短路/A*】

  1 #include<stdio.h>
  2 #include<string.h>
  3 #include<queue>
  4 #include<algorithm>
  5 #define mem(a, b) memset(a, b, sizeof(a))
  6 using namespace std;
  7 typedef long long ll;
  8 const int MAXN = 1e3 + 100;
  9 const int MAXM = 1e5 + 100;
 10 const int inf = 0x3f3f3f3f;
 11 
 12 int n, m;    //n个点 m条有向边 
 13 int head[MAXN], cnt, r_head[MAXN], r_cnt;
 14 int vis[MAXN];
 15 ll h[MAXN]; 
 16 
 17 struct Edge
 18 {
 19     int to, next, w;
 20 }e[MAXM], r_e[MAXM];
 21 
 22 struct Node
 23 {
 24     int pot;
 25     ll g, h;
 26     bool operator < (const Node &a)const //优先选择f值小的点 A*算法精髓所在 
 27     {
 28         return g + h > a.g + a.h;
 29     }
 30 }node;
 31 
 32 void add(int a, int b, int c)
 33 {
 34     cnt ++;
 35     e[cnt].to = b;
 36     e[cnt].next = head[a];
 37     e[cnt].w = c;
 38     head[a] = cnt;
 39 }
 40 
 41 void r_add(int a, int b, int c) //反向边 与正向边的编号一致 
 42 {
 43     r_cnt ++;   
 44     r_e[r_cnt].to = b;
 45     r_e[r_cnt].next = r_head[a];
 46     r_e[r_cnt].w = c;
 47     r_head[a] = r_cnt;
 48 }
 49 
 50 int spfa(int st, int ed)  //反向图 跑 h 评估函数(在这里也就是终点到该点的最短距离) A*算法精髓
 51 {
 52     mem(h, inf), mem(vis, 0);
 53     queue<int> Q;
 54     while(!Q.empty())    Q.pop();
 55     vis[st] = 1;
 56     h[st] = 0;
 57     Q.push(st);
 58     while(!Q.empty())
 59     {
 60         int a = Q.front();
 61         Q.pop();
 62         vis[a] = 0;
 63         for(int i = r_head[a]; i != -1; i = r_e[i].next)
 64         {
 65             int to = r_e[i].to;
 66             if(h[to] > h[a] + r_e[i].w * 1ll)
 67             {
 68                 h[to] = h[a] + r_e[i].w * 1ll;
 69                 if(!vis[to])
 70                 {
 71                     vis[to] = 1;
 72                     Q.push(to);
 73                 }
 74             }
 75         }
 76     }
 77     return h[ed] != inf;
 78 }
 79 
 80 ll A_star(int st, int ed, int k) //优先队列 优先选择f值小的点 跑正向图 
 81 {
 82     int num = 0;
 83     priority_queue<Node> QQ;
 84     while(!QQ.empty())    QQ.pop();
 85     node.pot = st, node.g = 0, node.h = h[st];
 86     QQ.push(node);
 87     while(!QQ.empty())
 88     {
 89         Node a = QQ.top();
 90         QQ.pop();
 91 //        printf("%d\n", a.pot);
 92         if(a.pot == ed)  //如果第k次找到终点 就得出了答案 
 93         {
 94             num ++;
 95             if(num == k)
 96                 return a.g + a.h;
 97         }
 98         for(int i = head[a.pot]; i != -1; i = e[i].next)
 99         {
100             int to = e[i].to;
101             node.pot = to;
102             node.g = a.g + e[i].w * 1ll;
103             node.h = h[to];
104             QQ.push(node);
105         }
106     }
107     return -1;
108 }
109 
110 int main()
111 {
112     scanf("%d%d", &n, &m);
113     mem(head, -1), cnt = 0;
114     mem(r_head, -1), r_cnt = 0;
115     for(int i = 1; i <= m; i ++)
116     {
117         int a, b, c;
118         scanf("%d%d%d", &a, &b, &c);
119         add(a, b, c);
120         r_add(b, a, c);
121     }
122     int a, b, k; //a 到 b 的第 k 短路 
123     scanf("%d%d%d", &a, &b, &k);
124     if(a == b)
125         k ++;
126     if(!spfa(b, a)) 
127         printf("-1\n"); //若起点到终点本身就不连通 就不存在第k短路 输出-1 
128     else
129         printf("%lld\n", A_star(a, b, k));
130     return 0;
131 }