用A*求图的单源单汇第K短路

【问题描述】
给出一个图G和指定的源点s、汇点t，求图中从点s到点t的第K短路。
【具体题目】 PKU2449（注意：本题有一个猥琐之处：不允许空路径。也就是当s等于t时要将K值加1）
【算法】
本题有一种最朴素的办法：改造Dijkstra，一开始路径(s, 0)（这里设路径(i, l)表示从s到i的一条长度为l的路径）入队。然后， 每次取队中长度最短的路径，该路径(i, l)出队，可以证明，若这是终点为i的路径第x次出队，该路径一定是图中从s到i的第x短路（若x>K则该路径已无用，舍弃）。然后从点i扩展，将扩展到的路径全部入队。这样直到终点为t的路径第K次出队即可。
该算法容易实现（借助priority_queue），但时间复杂度可能达到O(MK)，需要优化。
优化：容易发现该算法其实有A*的思想，或者说，该算法 其实是所有结点的估价函数h()值均为0的A*算法。为了优化此题，需要将h()值改大。显然，h(i)值可以设为 从i到t的最短路径长度（容易证明它是一致的），然后g(i)=目前结点代表的路径长度，f(i)=g(i)+h(i)，然后A*即可。

注意：更改路径条数应该在出队时更改，而不能在入队时更改，因为可能在该路径出队之前会有新的比它更短的路径入队。

代码（PKU2449）：

#include  < iostream >
#include  < stdio.h >
#include  < queue >
using   namespace  std;
#define  re(i, n) for (int i=0; i<n; i++)
const   int  MAXN  =   1500 , MAXM  =   150000 , INF  =   ~ 0U   >>   2 ;
struct  edge {
     int  kk, len, next;
} ed[MAXM], ed2[MAXM];
int  n, m, s, t, k_, hd[MAXN], tl[MAXN], hd2[MAXN], tl2[MAXN], h[MAXN], q[MAXN  +   1 ], No[MAXN], res  =   - 1 ;
bool  vst[MAXN];
struct  qnode {
     int  i, g;
};
typedef priority_queue  < qnode, vector < qnode >   >  pq;
pq z;
bool   operator <  (qnode q1, qnode q2)
{
     return  q1.g  +  h[q1.i]  >  q2.g  +  h[q2.i];
}
void  init()
{
     int  a0, b0, l0;
    scanf( " %d%d " ,  & n,  & m);
    re(i, n) hd[i]  =  tl[i]  =  hd2[i]  =  tl2[i]  =   - 1 ;
    re(i, m) {
        scanf( " %d%d%d " ,  & a0,  & b0,  & l0); a0 -- ; b0 -- ;
        ed[i].kk  =  b0; ed[i].len  =  l0; ed[i].next  =   - 1 ;
         if  (hd[a0]  ==   - 1 ) hd[a0]  =  tl[a0]  =  i;  else  tl[a0]  =  ed[tl[a0]].next  =  i;
        ed2[i].kk  =  a0; ed2[i].len  =  l0; ed2[i].next  =   - 1 ;
         if  (hd2[b0]  ==   - 1 ) hd2[b0]  =  tl2[b0]  =  i;  else  tl2[b0]  =  ed2[tl2[b0]].next  =  i;
    }
    scanf( " %d%d%d " ,  & s,  & t,  & k_);  -- s;  -- t; k_  +=  s  ==  t;
}
void  prepare()
{
    re(i, n) {h[i]  =  INF; vst[i]  =   0 ;} h[t]  =   0 ; vst[t]  =   1 ; q[ 0 ]  =  t;
     int  i, h0, j, h1;
     for  ( int  front = 0 , rear = 0 ;  ! ( ! front  &&  rear  ==  n  ||  front  ==  rear  +   1 ); front  ==  n  ?  front  =   0  : front ++ ) {
        i  =  q[front]; h0  =  h[i];
         for  ( int  p = hd2[i]; p  !=   - 1 ; p = ed2[p].next) {
            j  =  ed2[p].kk; h1  =  h0  +  ed2[p].len;
             if  (h1  <  h[j]) {
                h[j]  =  h1;
                 if  ( ! vst[j]) {vst[j]  =   1 ;  if  (rear  ==  n) q[rear  =   0 ]  =  j;  else  q[ ++ rear]  =  j;}
            }
        }
        vst[i]  =   0 ;
    }
}
void  solve()
{
    qnode q0; q0.i  =  s; q0.g  =   0 ; z.push(q0);
    re(i, n) No[i]  =   0 ;
     int  i, d0, j, d1;
     while  ( ! z.empty()) {
        i  =  z.top().i; d0  =  z.top().g; z.pop();
         if  (No[i]  >=  k_)  continue ;
        No[i] ++ ;
         if  (i  ==  t  &&  No[i]  ==  k_) {res  =  d0;  break ;}
         for  ( int  p = hd[i]; p  !=   - 1 ; p = ed[p].next) {
            j  =  ed[p].kk; d1  =  d0  +  ed[p].len;
            q0.i  =  j; q0.g  =  d1; z.push(q0);
        }
    }
}
void  pri()
{
    printf( " %d\n " , res);
}
int  main()
{
    init();
    prepare();
    solve();
    pri();
     return   0 ;
}