A*第K短路算法模板题

#include<iostream>
#include<cstdio>
#include<cstring>
#include<algorithm>
#include<cmath>
#include<queue>
using namespace std;
typedef long long ll;
const int N=1000+10;
const int M=1e5+10;
const int INF=0x7f7f7f7f;
struct Edge
{
    int to,w;
    int nxt;
}edge[M],edge0[M];
int first[N],first0[N],tot,tot0;
void addedge(int u,int v,int w)
{
    edge[tot].to=v;
    edge[tot].w=w;
    edge[tot].nxt=first[u];
    first[u]=tot++;
}
void addedge0(int u,int v,int w)
{
    edge0[tot0].to=v;
    edge0[tot0].w=w;
    edge0[tot0].nxt=first0[u];
    first0[u]=tot0++;
}
struct node
{
    int p,d;
    node(int a,int b)
    {
        p=a;d=b;
    }
    bool operator < (const node &u) const
    {
        return d>u.d;
    }
};
struct node0
{
    int p,d,f;
    node0(int a,int b,int c)
    {
        p=a;d=b;f=c;
    }
    bool operator < (const node0 &u) const
    {
        return d+f>u.d+u.f;
    }
};
int dist[N];
void dj(int s,int n)
{
    priority_queue<node> q;
    for(int i=0;i<=n;i++) dist[i]=INF;
    q.push(node(s,0));
    dist[s]=0;
    while(!q.empty())
    {
        node u=q.top();
        q.pop();
        int up=u.p;
        int ud=u.d;
        for(int i=first0[up];i!=-1;i=edge0[i].nxt)
        {
            int np=edge0[i].to;
            int nd=edge0[i].w+ud;
            if(nd<dist[np])
            {
                dist[np]=nd;
                q.push(node(np,nd));
            }
        }
    }
}
int A_star(int s,int t,int k)
{
    if(s==t) k++;
    if(dist[s]==INF) return -1; 
    priority_queue<node0> q;
    q.push(node0(s,0,dist[s]));
    int num=0;
    while(!q.empty())
    {
        node0 u=q.top();
        q.pop();
        int up=u.p;
        int ud=u.d;
        if(up==t) num++;
        if(num==k) return ud+u.f;
        for(int i=first[up];i!=-1;i=edge[i].nxt)
        {
            int np=edge[i].to;
            int nd=edge[i].w+ud;
            q.push(node0(np,nd,dist[np]));
        }
    }
    return -1;
}
int main()
{
    int n,m,a,b,w;
    int s,t,k;
    while(~scanf("%d%d",&n,&m))
    {
        memset(first0,-1,sizeof(first0));
        memset(first,-1,sizeof(first));
        tot=tot0=0;
        for(int i=0;i<m;i++)
        {
            scanf("%d%d%d",&a,&b,&w);
            addedge(a,b,w);
            addedge0(b,a,w);
        }
        scanf("%d%d%d",&s,&t,&k);
        dj(t,n);
        printf("%d\n",A_star(s,t,k));
    }
    return 0;
}