搜索专题-A*算法初学（求第K短路）

思路：A*算法裸题

Code：

#include <bits/stdc++.h>
#define INF 0x3f3f3f3f
using namespace std;
const int AX = 1e5+66;
const int MAXN = 1e3+66;
int n , m , k;
int s ,t ;
int tot ;
int retot ;
struct edge{
    int to , w ;
    int next1;
}G[AX] , RG[AX];
//f = g + h 
struct Node{
    int v;
    int f , h , g;
    bool operator < ( const Node &a ) const{
        if( f == a.f ){
            return g > a.g;
        }
        return f > a.f;
    }
};

int dis[MAXN];
int head[MAXN];
int rehead[AX];
int vis[MAXN];
void addedge( int u , int v , int c ){
    G[tot].to = v;
    G[tot].w = c;
    G[tot].next1 = head[u];
    head[u] = tot++;

    RG[retot].to = u;
    RG[retot].w = c;
    RG[retot].next1 = rehead[v];
    rehead[v] = retot ++;
}
void SPFA(){
    for( int i = 1 ; i <= n ; i++ ){
        dis[i] = INF;
    }   
    dis[t] = 0;
    queue<int>Q;
    Q.push(t);
    while( !Q.empty() ){
        int u = Q.front();
        Q.pop();
        for( int i = rehead[u] ; ~i ; i = RG[i].next1 ){
            int v = RG[i].to ;
            int w = RG[i].w ;
            if( dis[v] > dis[u] + w ){
                dis[v] = dis[u] + w;
                Q.push(v);
            }
        }
    }   
}

int Astar( Node a ){
    memset( vis , 0 ,sizeof(vis) );
    if( dis[s] == INF ) return -1;
    if( s == t ) k++;
    priority_queue<Node>Q;
    Q.push(a);
    while( !Q.empty() ){
        Node tmp = Q.top();
        Q.pop();
        int v = tmp.v;
        vis[v] ++ ;
        if( vis[t] == k ) return tmp.g;
        for( int i = head[v] ; ~i ; i = G[i].next1 ){
            Node p;
            p.v = G[i].to;
            p.h = dis[G[i].to];
            p.g = tmp.g + G[i].w;
            p.f = p.g + p.h;
            Q.push(p);
        }
    }
    return -1;
}

int main(){
    tot = 0 ;
    retot = 0;
    memset( head , -1 , sizeof(head) );
    memset( rehead , -1 , sizeof(rehead) );
    scanf("%d%d%d",&n,&m,&k);
    scanf("%d%d",&s,&t);
    int x , y , w ;
    for( int i = 0 ; i < m ; i++ ){
        scanf("%d%d%d",&x,&y,&w);
        addedge( x , y , w );
    }
    SPFA();
    Node a;
    a.v = s ;
    a.g = 0;
    a.h = dis[s];
    a.f = a.g + a.h;
    int g = Astar(a);
    printf("%d\n",g);
    return 0 ;
}