A* Algorithm 單純求最短路徑, 啟發 H(x) 並不好估計, 但是求 第 k 短, 它就派得上用場了
內容轉至 http://www.cppblog.com/MatoNo1/archive/2011/05/01/145456.html
【问题描述】
给出一个图G和指定的源点s、汇点t，求图中从点s到点t的第K短路。
【算法】
本题有一种最朴素的办法：改造Dijkstra，一开始路径(s, 0)（这里设路径(i, l)表示从s到i的一条长度为l的路径）入队。然后，每次取队中长度最短的路径，该路径(i, l)出队，可以证明，若这是终点为i的路径第x次出队，该路径一定是图中从s到i的第x短路（若x>K则该路径已无用，舍弃）。然后从点i扩展，将扩展到的路径全部入队。这样直到终点为t的路径第K次出队即可。
该算法容易实现（借助priority_queue），但时间复杂度可能达到O(MK)，需要优化。
优化：容易发现该算法其实有A*的思想，或者说，该算法其实是所有结点的估价函数h()值均为0的A*算法。为了优化此题，需要将h()值改大。显然，h(i)值可以设为从i到t的最短路径长度（容易证明它是一致的），然后g(i)=目前结点代表的路径长度，f(i)=g(i)+h(i)，然后A*即可。

注意：更改路径条数应该在出队时更改，而不能在入队时更改，因为可能在该路径出队之前会有新的比它更短的路径入队。

以下是我在 ZeroJudge 的實踐

/**********************************************************************************/
/*  Problem: d243 "圖論專家" from                                             */
/*  Language: C                                                                   */
/*  Result: AC (3ms, 304KB) on ZeroJudge                                          */
/*  Author: morris1028 at 2011-07-21 10:41:43                                     */
/**********************************************************************************/


#include<stdio.h>
#define MaxV 1000000
typedef struct Link {
    int x, y, w;
}Link;
struct Heap {
    int v, node;
}Heap[1001];/*min heap*/
int L;
void Swap(int, int);
void siftup(int);
void siftdown(int);
void insHeap(int, int, int);
void delHeap();
void MergeSort(int, int, Link[]);
void Merge(int, int, int, Link[]);

Link Data[1001], X[1001], RevD[1001];
int NodeT[1001], NodeL[1001];
int RevNT[1001], RevNL[1001];
int Hdis[1001], Gdis[1001];/*f(x) = g(x) + h(x)*/
void prepare(int st, int ed, int n) { /*SPFA*/
    int a, b, c;
    int Queue[n*n], Qt = 0, Used[n], tn;
    for(a = 0; a < n; a++)    Hdis[a] = MaxV, Used[a] = 0;
    Queue[0] = ed, Used[ed] = 1, Hdis[ed] = 0;
    for(a = 0; a <= Qt; a++) {
        tn = Queue[a], Used[tn] = 0;
        for(b = RevNL[tn], c = 0; c < RevNT[tn]; b++, c++) {
            if(Hdis[tn] + RevD[b].w < Hdis[RevD[b].y]) {
                Hdis[RevD[b].y] = Hdis[tn] + RevD[b].w;
                if(Used[RevD[b].y] == 0) {
                    Used[RevD[b].y] = 1, Queue[++Qt] = RevD[b].y;
                }
            }
        }
    }
}
void solve_kpath(int st, int ed, int k, int n) {
    L = 0;
    int OutT[n], a, b, tn, tw;
    for(a = 0; a < n; a++)    OutT[a] = 0, Gdis[n] = MaxV;
    for(a = NodeL[st], b = 0; b < NodeT[st]; a++, b++) {
        L++, insHeap(L, Data[a].y, Data[a].w + Hdis[Data[a].y]);
    }
    OutT[st]++;
    while(L) {
        tn = Heap[1].node, tw = Heap[1].v;
        delHeap();
        if(OutT[tn] >= k || tw == Gdis[tn]) continue;
        Gdis[tn] = tw, OutT[tn]++, tw = tw - Hdis[tn];
        if(OutT[tn] == k && tn == ed)    {printf("%d\n", tw);return;}
        for(a = NodeL[tn], b = 0; b < NodeT[tn]; a++, b++) {
            L++, insHeap(L, Data[a].y, tw + Data[a].w + Hdis[Data[a].y]);
        }
    }
    puts("?");
}
main() {
    int n, m, a, b, C = 0;
    int x, y, d, p, k, st, ed;
    while(scanf("%d %d", &n, &m) == 2) {
        int rt, t;       
        for(a = 0; a < n; a++)/*init*/
            NodeT[a] = RevNT[a] = 0, NodeL[a] = RevNL[a] = MaxV;
        for(a = 0, t = 0, rt = 0; a < m; a++) {
            scanf("%d %d %d", &x, &y, &d);
            Data[t].x = x, Data[t].y = y, Data[t].w = d, t++;
            Data[t].x = y, Data[t].y = x, Data[t].w = d, t++;
            NodeT[x]++, NodeT[y]++;
            RevD[rt].x = y, RevD[rt].y = x, RevD[rt].w = d, rt++;
            RevD[rt].x = x, RevD[rt].y = y, RevD[rt].w = d, rt++;
            RevNT[y]++, RevNT[x]++;
        }
        MergeSort(0, t-1, Data);
        MergeSort(0, rt-1, RevD);
        for(a = 0; a < t; a++) {
            NodeL[Data[a].x] = NodeL[Data[a].x] < a ? NodeL[Data[a].x]: a;
            RevNL[RevD[a].x] = RevNL[RevD[a].x] < a ? RevNL[RevD[a].x]: a;
        }
        scanf("%d", &p);
        printf("Set #%d\n", ++C);
        while(p--) {
            scanf("%d %d %d", &st, &ed, &k);
            prepare(st, ed, n);
            solve_kpath(st, ed, k, n);
        }
    }
    return 0;
}
void Swap(int P, int S) {
    int T;
    T=Heap[P].v, Heap[P].v=Heap[S].v, Heap[S].v=T;
    T=Heap[P].node, Heap[P].node=Heap[S].node, Heap[S].node=T;
}
void siftup (int site) {
    int S = site, P = site >> 1;
    while(S >= 2 && Heap[P].v > Heap[S].v)
        Swap(P, S), S = P, P = S >> 1;
}
void siftdown(int site) {
    int P = site, S = site << 1;
    while(S <= L) {
        if(S < L && Heap[S+1].v < Heap[S].v)    S++;
        if(Heap[P].v <= Heap[S].v)    break;
        Swap(P, S), P = S, S = P << 1;
    }
}
void insHeap(int site, int node, int t) {/*insert new node*/
    Heap[site].node = node, Heap[site].v = t;
    siftup(site);
}
void delHeap() {/*delete last node*/
    Swap(1, L), L--;
    siftdown(1);
}
void MergeSort(int l, int h, Link Data[]) {
    if(l < h) {
        int m = (l+h)/2;
        MergeSort(l, m, Data);
        MergeSort(m+1, h, Data);
        Merge(l, m, h, Data);
    }
}
void Merge(int l, int m, int h, Link Data[]) {
    int In1 = l, In2 = m+1;
    int a, b, Top = 0;
    while(In1 <= m && In2 <= h) {
        if(Data[In1].x < Data[In2].x || (Data[In1].x == Data[In2].x && Data[In1].w <= Data[In2].w))
            X[Top++] = Data[In1++];
        else
            X[Top++] = Data[In2++];
    }
    while(In1 <= m) X[Top++] = Data[In1++];
    while(In2 <= h)    X[Top++] = Data[In2++];
    for(a = 0, b = l; a < Top; a++, b++)
        Data[b] = X[a];
}