A*-第K短路

# include <stdio.h>
# include <string.h>
# include <queue>
# include <algorithm>
using namespace std;
typedef pair<int ,int> PII;
typedef pair<int ,PII> PIII;
# define N 10001
# define M 200001
int Next[M],ver[M],edge[M],head[N],whead[N];
int n,m,tot;
int S,T,K;
int dist[N],ST[N];
 priority_queue<PII>Q;
void ADD(int x,int y,int z)
{
    ver[++tot]=y;edge[tot]=z;Next[tot]=head[x];head[x]=tot;//正向边
    ver[++tot]=x;edge[tot]=z;Next[tot]=whead[y];whead[y]=tot;//反向边
}
void dijkstra()
{
    int x,y;
    Q.push(make_pair(0,T));//对反向边 求终点到各点的最短路 作为估价函数
    memset(dist,0x3f,sizeof(dist));
    dist[T]=0;
    while(Q.size())
    {
        PII t=Q.top();Q.pop();
        x=t.second;
        if(ST[x])continue;
        ST[x]=true;
        for(int i=whead[x];~i;i=Next[i])
        {
            y=ver[i];
            if(dist[y]>dist[x]+edge[i])
            {
                dist[y]=dist[x]+edge[i];
                Q.push(make_pair(-dist[y],y));
            }
        }
    }
}
int Astar()
{
    int x,y,distance;
    priority_queue<PIII>Q;
    Q.push(make_pair(-dist[S],make_pair(0,S)));
    memset(ST,0,sizeof(ST));
    while(Q.size())
    {
        PIII t=Q.top();
        Q.pop();
        x=t.second.second;distance=t.second.first;
        ST[x]++;
        if(x==T&&ST[x]==K)return distance;
        if(ST[x]>K)continue;
        for(int i=head[x];~i;i=Next[i])
        {
            y=ver[i];
            if(ST[y]!=K)
                Q.push(make_pair(-(distance+edge[i]+dist[y]),make_pair(distance+edge[i],y)));
        }
    }
    return -1;
}
int main()
{
    int x,y,z;
    scanf("%d%d",&n,&m);
    memset(head,-1,sizeof(head));
    memset(whead,-1,sizeof(whead));
    tot=-1;
    for(int i=0;i<m;i++)
    {
        scanf("%d%d%d",&x,&y,&z);
        ADD(x,y,z);
    }
    scanf("%d%d%d",&S,&T,&K);
    if(S==T)K++;
    dijkstra();
    printf("%d\n",Astar());
    return 0;
}