K最短問題(単一ソース点最短パス+A*アルゴリズム)

/*
 *    ：
 *           ,              ,         ;
 *         ,           ,  K     ;
 *
 *    ：
 *       +    A*;
 *A*                ;
 *                                   ;
 *              ,           ;
 *
 *          f(h)         p      ,           ;
 *        ,        ;
 *  A*,    =   +          , f(p)=g(p)+h(p),              ;
 *
 *  K      ,g(p)    s p        ;h(p)  p t       ;
 *f(p)     s        p      t        ;
 *
 *      ,h(p)   A*         ,           ,    t                  h(p) ;
 *
 *    ：
 *(1),          ,    t   ,  t         ;
 *(2),        ,   s      ;
 *(3),         f(p)    p,   p  t,   t     ;
 *     t  k   ,          s t  k     ,    ;
 *     p       ,      p              ;
 *
 *    ：
 *PKU2449(Remmarguts' Date)
 *
 *    ：
 *  s t  k     ;
 */

#include
#include
#include
#include
#include
#include
#include
using namespace std;

const int INF=0xffffff;
const int N=1010;
const int M=100010;

struct node1
{
    int to;
    int w;
    int next;
};

node1 edge1[M],edge2[M];
int head1[M],head2[M];
int idx1,idx2;
int dist[N];

struct node2
{
    int to;
    //g(p)    s p        ;h(p)  p t       ;
    int g,f;//f=g+h,f(p)     s        p      t        ;
    bool operatorQ1;
    int inq[N];
    for(int i=0; i<=n; i++)
    {
        dist[i]=INF;
        inq[i]=0;
    }
    dist[s]=0;
    Q1.push(s);
    inq[s]++;
    while(!Q1.empty())
    {
        int q=Q1.front();
        Q1.pop();
        inq[q]--;
        if(inq[q]>n)//   
            return false;
        int k=head[q];
        while(k>=0)
        {
            if(dist[edge[k].to]>dist[q]+edge[k].w)
            {
                dist[edge[k].to]=edge[k].w+dist[q];
                if(!inq[edge[k].to])
                {
                    inq[edge[k].to]++;
                    Q1.push(edge[k].to);
                }
            }
            k=edge[k].next;
        }
    }
    return true;
}

int A_star(int s,int t,int n,int k,int head[],node1 edge[],int dist[])
{
    node2 e,ne;
    int cnt=0;
    priority_queueQ;
    if(s==t)// s==t ,   0       k   ,     k+1  ;
        k++;
    if(dist[s]==INF)
        return -1;
    e.to=s;
    e.g=0;
    e.f=e.g+dist[e.to];
    Q.push(e);

    while(!Q.empty())
    {
        e=Q.top();
        Q.pop();
        if(e.to==t)//        
        {
            cnt++;
        }
        if(cnt==k)//  k  
        {
            return e.g;
        }
        for(int i=head[e.to]; i!=-1; i=edge[i].next)
        {
            ne.to=edge[i].to;
            ne.g=e.g+edge[i].w;
            ne.f=ne.g+dist[ne.to];
            Q.push(ne);
        }
    }
    return -1;
}

int main()
{
    int n,m;
    //freopen("C:\\Users\\Administrator\\Desktop\\kd.txt","r",stdin);
    while(~scanf("%d%d",&n,&m))
    {
        memset(head1, -1, sizeof(head1));
        memset(head2, -1, sizeof(head2));
        idx1=idx2=0;
        int u,v,w;
        for(int i=0; i