A* 算法求第 K 短路

Remmarguts' Date

求 S 到 T 的第 K 短路。

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

int n, m, S, T, K, ans=-1;
int head[1005], nex[100005], to[100005], w[100005];
int rhead[1005], rnex[100005], rto[100005], rw[100005];
int h[1005]; bool v[1005];
struct node {
    int t, d;
    bool operator < (const node& A) const {return d>A.d; }
};
priority_queue<node> q;
struct star {
    int t, g, f;
    bool operator < (const star& A) const {return f>A.f || f==A.f && g>A.g; }
};
priority_queue<star> Q;

inline void add(int x, int y, int z) {
    nex[++head[0]]=head[x], head[x]=head[0], to[head[0]]=y, w[head[0]]=z;
}
inline void radd(int x, int y, int z) {
    rnex[++rhead[0]]=rhead[x], rhead[x]=rhead[0], rto[rhead[0]]=y, rw[rhead[0]]=z;
}

inline void astar() {
    if (S==T) ++K;
    if (h[S]==0x3f3f3f3f) return;
    Q.push((star) {S, 0, h[S]});
    register int cnt=0;

    while (!Q.empty()) {
        register star now=Q.top(); Q.pop();
        if (now.t==T) if (++cnt>=K) {ans=now.g; return; }
        for (int i=head[now.t]; i; i=nex[i])
            Q.push((star) {to[i], now.g+w[i], now.g+w[i]+h[to[i]]});
    }
}

int main() {
    while (~scanf("%d%d", &n, &m)) {
        memset(head, 0, sizeof head), memset(rhead, 0, sizeof rhead);
        for (int i=1, A, B, C; i<=m; ++i) scanf("%d%d%d", &A, &B, &C),
            add(A, B, C), radd(B, A, C);
        scanf("%d%d%d", &S, &T, &K);

        memset(h, 0x3f, sizeof h);
        q.push((node) {T, h[T]=0});
        while (!q.empty()) {
            register node now=q.top(); q.pop();
            if (v[now.t]) continue; v[now.t]=true;
            for (int i=rhead[now.t]; i; i=rnex[i])
                if (now.d+rw[i] < h[rto[i]])
                    h[rto[i]] = now.d + rw[i], q.push((node) {rto[i], h[rto[i]]});
        }

        astar();
        printf("%d\n", ans);
    }
    return 0;
}