A*算法

#include <cstdio>
#include <cstring>
#include <iostream>
#include <algorithm>
#include <queue>

#define MAXN 1005
#define MAXM 500005
#define INF 1000000000

using namespace std;

struct Edge {
    int to, cost, next;
}edge1[MAXM], edge2[MAXM];

struct Node {
    int x, f, g;
    bool operator < (const Node &node) const {
        if (f != node.f) return f > node.f;
        return g > node.g;
    }
}node1, node2;

int n, m, k;
int tot, head1[MAXN], head2[MAXN];
bool vis[MAXN];
int dis[MAXN];
int que[MAXN*5];

void init() {
    tot = 0;
    memset(head1, -1, sizeof(head1));
    memset(head2, -1, sizeof(head2));
}

void addedge(int u, int v, int cost) {
    edge1[tot].to = v, edge1[tot].cost = cost;
    edge1[tot].next = head1[u];
    head1[u] = tot;
    edge2[tot].to = u, edge2[tot].cost = cost;
    edge2[tot].next = head2[v];
    head2[v] = tot++;
}

void spfa(int s) {
    for (int i = 1; i <= n; i++) dis[i] = INF;
    memset(vis, false, sizeof(vis));
    int l = 0, r = 1;
    que[0] = s;
    dis[s] = 0;
    while (l < r) {
        int u = que[l++];
        vis[u] = false;
        for (int i = head2[u]; i != -1; i = edge2[i].next) {
            int v = edge2[i].to, cost = edge2[i].cost;
            if (dis[v] > dis[u] + cost) {
                dis[v] = dis[u] + cost;
                if (!vis[v]) {
                    vis[v] = true;
                    que[r++] = v;
                }

            }
        }
    }
}

int a_star(int s, int t) {
    int cnt = 0;
    if (dis[s] == INF) return -1;
    if (s == t) k++;
    priority_queue<Node> q;
    node1.x = s, node1.g = 0, node1.f = dis[s];
    q.push(node1);
    while (!q.empty()) {
        node2 = q.top(); q.pop();
        if (node2.x == t) {
            cnt++;
            if (cnt == k) return node2.f;
        }
        for (int i = head1[node2.x]; i != -1; i = edge1[i].next) {
            node1.x = edge1[i].to;
            node1.g = node2.g + edge1[i].cost;
            node1.f = node1.g + dis[node1.x];
            q.push(node1);
        }
    }
    return -1;
}

int main() {
    int u, v, cost, s, t;
    while (scanf("%d %d", &n, &m)!=EOF) {
        init();
        for (int i = 1; i <= m; i++) {
            scanf("%d%d%d", &u, &v, &cost);
            addedge(u, v, cost);
        }
        scanf("%d%d%d", &s, &t, &k);
        spfa(t);
        printf("%d\n", a_star(s, t));
    }
    return 0;
}