POJ 2449-第K短絡、Astar、優先キュー

#include <cstring>
#include <iostream>
#include <cstdio>
#include <queue>
using namespace std;

const int NN=1005;
const int MM=100010;
const int INF=0x1fffffff;

struct node{   //        
   int v,g,h;
   bool operator <(node a) const
   {
       return a.g+a.h<g+h;    //Astar         
   }
};
struct Edge{
   int u,v,dis,next,next2;
}edge[MM];
int head[NN],head2[NN],h[NN];
int n,m,ecnt,S,T,k;

void addedge(int u,int v,int dis)
{
    edge[ecnt].u=u;
    edge[ecnt].v=v;
    edge[ecnt].dis=dis;
    edge[ecnt].next=head[u];
    edge[ecnt].next2=head2[v];  //next2     spfa     
    head[u]=ecnt;
    head2[v]=ecnt++;
}

bool spfa()  //    T   ，      h[i]
{
    bool inq[NN];
    memset(inq,false,sizeof(inq));
    for (int i=1; i<=n; i++) h[i]=INF;
    h[T]=0;
    queue<int> q;
    q.push(T);
    while (!q.empty())
    {
        int u=q.front();
        q.pop();
        inq[u]=false;
        for (int i=head2[u]; i!=-1; i=edge[i].next2)
        {
            int v=edge[i].u;
            if (h[v]>h[u]+edge[i].dis)
            {
                h[v]=h[u]+edge[i].dis;
                if (!inq[v])
                {
                    inq[v]=true;
                    q.push(v);
                }
            }
        }
    }
    if (h[S]==INF) return false;
    return true;
}

int kth_shortest()
{
    if (!spfa()) return -1;   //S,T         
    if (S==T) k++;          //  S,T   ,S T      0,     0,    1
    int cou[NN];             //      
    memset(cou,0,sizeof(cou));
    priority_queue<node> q;
    node x,y;
    x.v=S; x.g=0; x.h=h[S];
    q.push(x);
    while (!q.empty())
    {
        x=q.top();
        q.pop(); 
        if (cou[x.v]==k) continue; //        k ,      
        if (++cou[x.v]==k && x.v==T) return x.g;   //     ,             
        for (int i=head[x.v]; i!=-1; i=edge[i].next)
        {
            y.v=edge[i].v;
            if (cou[y.v]==k) continue;
            y.g=x.g+edge[i].dis;
            y.h=h[y.v];
            q.push(y);
        }
    }
    return -1;  //      ，    。。。
}

int main()
{
    ecnt=0;
    memset(head,-1,sizeof(head));
    memset(head2,-1,sizeof(head2));
    scanf("%d%d",&n,&m);
    int x,y,z;
    for (int i=1; i<=m; i++)
    {
        scanf("%d%d%d",&x,&y,&z);
        addedge(x,y,z);
    }
    scanf("%d%d%d",&S,&T,&k);
    printf("%d
",kth_shortest());
    return 0;
}