A*寻路算法

//http://poj.org/problem?id=2449

 

 

 

#include <iostream>

#include <utility>

#include <vector>

#include <queue>

using namespace std;

 

typedef pair<int, int> pii;//距离,顶点

struct Arc

{

    int vex;

    int weight;

};

 

const int MAX_VEX_NUM = 1010;

const int MAX = 1<<20;

 

vector<Arc> Adjlist[MAX_VEX_NUM];

vector<Arc> AdjlistRev[MAX_VEX_NUM];//反向图,求h(n)

int h[MAX_VEX_NUM];

 

int S,T,K;

 

void Init()

{

    int N,M;

    int A,B,T;

    cin>>N>>M;

    int i;

    for(i = 0; i < N; i++)

    {

        Adjlist[i].clear();

        AdjlistRev[i].clear();

    }

    Arc arc;

    for(i = 0; i < M; i++)

    {

        cin>>A>>B>>T;

        arc.vex = B;

        arc.weight = T;

        Adjlist[A].push_back(arc);

        arc.vex = A;

        AdjlistRev[B].push_back(arc);

    }

    cin>>S>>::T>>K;

}

 

//计算h[n]

void Dijkstra(int u)

{

    priority_queue<pii, vector<pii>, greater<pii> > pq_dij;

    bool traversed[MAX_VEX_NUM];

    memset(traversed, false, MAX_VEX_NUM * sizeof(bool));

    for(int i = 0; i < MAX_VEX_NUM; i++)

    {

        h[i] = MAX;

    }

     

    h[u] = 0;

    pq_dij.push(make_pair(h[u], u));

    while(!pq_dij.empty())

    {

        pii pq_node = pq_dij.top();

        pq_dij.pop();

        int v = pq_node.second;

        if(traversed[v])

            continue;

        traversed[v] = true;

        for(vector<Arc>::iterator iter = AdjlistRev[v].begin(); iter != AdjlistRev[v].end(); iter++)

        {

            int w = iter->vex;

            int weight = iter->weight;

            if(h[w] > h[v] + weight)

            {

                h[w] = h[v] + weight;

                pq_dij.push(make_pair(h[w], w));

            }

        }

    }

}

 

int Astar(int s, int t, int k)

{

    priority_queue<pii, vector<pii>, greater<pii> > pq_astar;

    int cnt[MAX_VEX_NUM];

    memset(cnt, 0, MAX_VEX_NUM * sizeof(int));

    pq_astar.push(make_pair(h[s], s));

     

    while(!pq_astar.empty())

    {

        pii node = pq_astar.top();

        pq_astar.pop();

        int u = node.second;

        int cost = node.first;

        cnt[u]++;

        if(cnt[t] == k)

            return cost;

        if(cnt[u] > k)

            continue;

        for(vector<Arc>::iterator iter = Adjlist[u].begin(); iter != Adjlist[u].end(); iter++)

        {

            int v = iter->vex;

            int weight = iter->weight;

            if(h[v] != MAX)

            {

                pii adjArc = make_pair(cost - h[u] + weight + h[v], v);

                pq_astar.push(adjArc);

            }

        }

    }

    return -1;

}

 

 

int main()

{

    //freopen("in.txt", "r", stdin);

 

    Init();

    Dijkstra(T);

 

    if(S == T)

        K++;

    cout<<Astar(S, T, K)<<endl;

    return 0;

}