POj 2449 Remmarguts' Date K短路 A*+SPFA

第一道A*，网上教程一大把就不说了。

#include <cstdio>
#include <algorithm>
#include <cstring>
#include <queue>
#include <vector>
using namespace std;
const int INF = 999999999;
const int maxn = 1010;
struct edge
{
    int u, v, w;
    int next;
    edge(){}
}G1[100010], G2[100010];

struct A
{
    int f, g, v;
    A() {}
    A(int f, int g, int v) : f(f), g(g), v(v) {}
    bool operator <(const A a) const
    {
        if(a.f == f)
            return a.g < g;
        return a.f < f;
    }
};
int d[maxn], first[maxn], head[maxn];
bool inq[maxn];
int n, m, cnt1, cnt2;
void AddEdge(int u, int v, int w)
{
    G1[cnt1].v = v;
    G1[cnt1].w = w;
    G1[cnt1].next = first[u];
    first[u] = cnt1++;

    G2[cnt2].v = u;
    G2[cnt2].w = w;
    G2[cnt2].next = head[v];
    head[v] = cnt2++;

}
bool SPFA(int s)
{
    memset(inq, false, sizeof(inq));
    for(int i = 0; i <= n; i++)
        d[i] = INF;
    d[s] = 0;
    queue <int> Q;
    Q.push(s);
    inq[s] = true;
    while(!Q.empty())
    {
        int u = Q.front(); Q.pop();
        inq[u] = false;
        for(int i = head[u]; i != -1; i = G2[i].next)
        {
            edge e = G2[i];
            int v = e.v;
            int w = e.w;
            if(d[v] > d[u] + w)
            {
                d[v] = d[u] + w;
                if(!inq[v])
                {
                    inq[v] = true;
                    Q.push(v);
                }
            }
        }
    }
}
int AStar(int s, int t, int K)
{
    if(d[s] == INF)
        return -1;
    int cnt = 0;
    priority_queue<A> Q;
    if(s == t)
        K++;
    Q.push(A(d[s], 0, s));
    while(!Q.empty())
    {
        A a = Q.top();
        Q.pop();
        if(a.v == t)
        {
            cnt++;
            //printf("%d\n", a.g);
            if(cnt == K)
                return a.g;
        }
        int u = a.v;
        for(int i = first[u]; i != -1; i = G1[i].next)
        {
            edge e = G1[i];
            A b;
            b.v = e.v;
            b.g = a.g + e.w;
            b.f = b.g + d[b.v];
            Q.push(b);
        }
    }
    return -1;
}
int main()
{
    memset(first, -1, sizeof(first));
    memset(head, -1, sizeof(head));
    scanf("%d %d", &n, &m);
    for(int i = 0; i < m; i++)
    {
        int u, v, w;
        scanf("%d %d %d", &u, &v, &w);
        AddEdge(u, v, w);
    }
    int s, t, k;
    scanf("%d %d %d", &s, &t, &k);
    SPFA(t);
    printf("%d\n", AStar(s, t, k));
    return 0;
}