POJ 2449 第K短路 SPFA+A*

               先找出来最短路，然后在最短路的基础上，从s往外扩展，当某个节点扩展K次时，就找到了第K短路，搜索的时候用A*

#include <iostream>
#include <cstdio>
#include <cstring>
#include <algorithm>
#include <cmath>
#include <queue>
#define inf 999999999
using namespace std;

struct node{
    int v;
    int len;
    int next;
}edge[211111];

bool v[1111];
int dis[1111];
int head[1111];
int tail[1111];
int e,n,m;
void addedge(int u,int v,int w){
    edge[e].v = v;
    edge[e].len = w;
    edge[e].next = head[u];
    head[u] = e++;
    edge[e].v = u;
    edge[e].len = w;
    edge[e].next = tail[v];
    tail[v] = e++;
}
void SPFA(int src,int p[]){
    queue<int> q;
    memset(v,0,sizeof(v));
    for(int i = 0 ; i < 1111 ; i ++){
        dis[i] = inf;
    }
    dis[src] = 0;
    q.push(src);
    while(!q.empty()){
        int temp = q.front();
        q.pop();
        v[temp] = 0;
        for(int i = p[temp] ; i != -1 ; i = edge[i].next){
            if(dis[temp] + edge[i].len < dis[edge[i].v]){
                dis[edge[i].v] = dis[temp] + edge[i].len;
                if(v[edge[i].v] == 0){
                    q.push(edge[i].v);
                    v[edge[i].v] = 1;
                }
            }
        }
    }
}

struct State{
    int g,h,id;
    int len;
    bool operator<(State a)const{
        return a.g + a.h < g + h;
    }
};

int cnt[1111];
int Astar(int s,int t,int k){
    priority_queue<State> q;
    memset(cnt,0,sizeof(cnt));
    if(s == t)
        k++;
    State src;
    src.id = s;
    src.g = 0;
    src.h = dis[s];
    q.push(src);
    while(!q.empty()){
        State temp = q.top();
        q.pop();
        cnt[temp.id]++;
        if(cnt[temp.id] > k)
            continue;
        if(cnt[temp.id] == k)
            return temp.g+temp.h;
        for(int i = head[temp.id] ; i != -1 ; i = edge[i].next){
            State nt;
            nt.id = edge[i].v;
            nt.g = temp.g + edge[i].len;
            nt.h = dis[nt.id];
            q.push(nt);
        }
    }
    return -1;
}

void init(){
    e = 0;
    memset(head,-1,sizeof(head));
    memset(tail,-1,sizeof(tail));
}
int main()
{
    scanf("%d%d",&n,&m);
    init();
    int u,v,w;
    int s,t,k;
    for(int i = 0 ; i < m ; i ++){
        scanf("%d%d%d",&u,&v,&w);
        addedge(u,v,w);
    }
    scanf("%d%d%d",&s,&t,&k);
    SPFA(t,tail);
    cout<<Astar(s,t,k)<<endl;
    return 0;
}