A* ( A star ) Search algorithm source code

/********************************************************************************************************
    Astar 4-directional  *********************************************************************************************************/



#include <iostream>

#include <string>

#include <map>

#include <queue>

#include <stack>

#include <algorithm>

#include <list>

#include <set>

#include <cmath>

#include <cstring>

#include <stdio.h>

#include <string.h>

#include <sstream>

#include <stdlib.h>

#include <vector>

#include <iomanip>



using namespace std;



#ifdef __GNUC__

typedef long long ll;typedef unsigned long long ull;

#else

typedef __int64 ll;  typedef unsigned __int64 ull;

#endif



#define inf (1<<30)-1

#define SIZE 10001

#define pi (2*acos(0.0))

#define forn(i,n) for (i=0;i<n;i++)

#define forr(i,n) for (i=n-1;i>=0;i--)

#define forab(i,p,k) for (i=p; i<=k;i++)

#define FOREACH(it,x) for(__typeof((x).begin()) it=(x.begin()); it!=(x).end(); ++it)



#define bug(x)    cout<< "->" <<#x<<": "<<x<<endl

#define Sort(x) sort(x.begin(),x.end())

#define Reverse(x) reverse(x.begin(),x.end())

#define mp(a,b) make_pair(a,b)

#define zero(x,with) memset(x,with,sizeof(x))

#define copy(c,r)   memcpy(c,r,sizeof(r))

#define sz(x) (int)x.size()

#define length(x) (int)x.length()

#define all(x) x.begin(),x.end()

#define pb push_back

#define popcount(i) __builtin_popcount(i)

#define gcd(a,b)    __gcd(a,b)



#define fs first

#define sc second

#define two(X) (1<<(X))

#define twoL(X) (((int64)(1))<<(X))

#define contain(S,X) (((S)&two(X))!=0)

#define containL(S,X) (((S)&twoL(X))!=0)

#define maxi(v) *max_element(all(v))

#define mini(v) *min_element(all(v))

#define CS c_str()

#define clr clear();

#define err 1e-9

#define even(a) ((a)%2==0)

#define odd(a) ((a)%2==1)



typedef pair<int,int> pii;

typedef pair<double,double> pdd;

typedef vector<int> vi;

typedef vector<double>vd;

typedef vector<ll>vll;

typedef vector<string> vs;

typedef vector<vi>vvi;

typedef vector<vll>vvll;

typedef vector<vd>vvd;

typedef vector<pii>vpii;

typedef map<string,int> msi;

typedef map<int,int>mii;

typedef map<pii,int>mpi;



template<class T> inline T gcd(T a,T b) {if(!b) return a; else return gcd(b,a%b);}

template<class T> void b_sort(vector<T>& VT) {for(int i=VT.sz;i>0;i--)for(int j=1;j<i;j++)if(VT[j]<VT[j-1]) swap(VT[j],VT[j-1]);}

template<class T> inline T sqr(T x){return x*x;}

template<class T> inline bool isprime(T n){if(n<=1)return false;for (T i=2;i*i<=n;i++) if (n%i==0) return false;return true;}

template<class T> inline T mod(T n,T m) {return (n%m+m)%m;} //For Positive Negative No.

template<class T> string tostring(T n){ostringstream oss;oss<<n;oss.flush();return oss.str();}

int toint(string s){int r=0;istringstream sin(s);sin>>r;return r;}

ll toll(string s){ll r=0;istringstream sin(s); sin>>r; return r;}

template<class T> void debug(const T& e){cout<<e<<endl;}

template<class T1,class T2> void debug(const T1& e1,const T2& e2){cout<<e1<<"\t"<<e2<<endl;}

template<class T1,class T2,class T3> void debug(const T1& e1,const T2& e2,const T3& e3){cout<<e1<<"\t"<<e2<<"\t"<<e3<<endl;}

template<class T1,class T2,class T3,class T4> void debug(const T1& e1,const T2& e2,const T3& e3,const T4& e4){cout<<e1<<"\t"<<e2<<"\t"<<e3<<"\t"<<e4<<endl;}

template<class T1,class T2,class T3,class T4,class T5> void debug(const T1& e1,const T2& e2,const T3& e3,const T4& e4,const T5& e5){cout<<e1<<"\t"<<e2<<"\t"<<e3<<"\t"<<e4<<"\t"<<e5<<endl;}

template<class T> void debug(vector<T>& e){int i;forn(i,sz(e)) cout<<e[i]<<" ";cout<<endl;}

template<class T> void debug(vector< basic_string<T> >& e){int i,j;forn(i,sz(e)) {forn(j,sz(e[i])) cout<<e[i][j];cout<<endl;} cout<<endl;}

template<class T> void debug(vector< vector<T> >& e,int row,int col){int i,j;forn(i,row) {forn(j,col) cout<<e[i][j]<<"\t";cout<<endl;} cout<<endl;}

template<class T> void debug(T e[SIZE][SIZE],int row,int col){int i,j;forn(i,row) {forn(j,col) cout<<e[i][j]<<" ";cout<<endl;}}

inline bool iseq(double x,double y){if(fabs(x-y)<err)return true;return false;}

template<typename T>inline T bigmod(T b,T p,T m){if(!p)return 1;else if(!(p%2)){T x=bigmod(b,p/2,m);return x*x;}else return ((b%m)*bigmod(b,p-1,m))%m;}



ll Pow(ll B,ll P){  ll R=1;  while(P>0)  {if(P%2==1)  R=(R*B);P/=2;B=(B*B);}return R;} //compute b^p



int dx[]={1,0,-1,0};int dy[]={0,1,0,-1}; //4 Direction

int fx,fy,dist[SIZE][SIZE],row,col;

int board[SIZE][SIZE];



int heuristic(int x,int y)

{

    return abs(x-fx)+abs(y-fy);
}



struct pq

{

    int x,y,cost,h,total_cost;

    pq(int _x,int _y,int _cost)
    {
        x=_x;y=_y;cost=_cost;
        h=heuristic(x,y);
        total_cost=cost+h;
    }
    bool operator<(const pq &b)const
{

return total_cost>b.total_cost;    // Min Priority Queue

}

};



int distance(int x,int y,int p,int q)

{

    if(board[p][q] > board[x][y]) return 3;
    if(board[p][q]==board[x][y]) return 2;
    return 1;
}




int astar(int x,int y)

{

    int i,j,m,n,cost,cnt;
    priority_queue<pq>Q;
    Q.push(pq(x,y,0));
    zero(dist,-1);
    while(!Q.empty())
    {
        pq P=Q.top();
        Q.pop();
        x=P.x;y=P.y;
        //debug(x,y);
        if(dist[x][y] != -1) continue;
        dist[x][y]=P.cost+P.h;
        forn(i,4)
        {
            m=x+dx[i];
            n=y+dy[i];
            if(m>=0 && m<row && n>=0 && n<col)
            {
                cost=distance(x,y,m,n);
                Q.push(pq(m,n,cost+P.cost));
            }
        }
    }
    cnt=0;
    forn(i,row)
    {
        forn(j,col)
        {
            if(dist[i][j] <= dist[fx][fy])
                cnt++;
        }
    }
    return cnt;
}



int main()

{

    int i,n,sx,sy,result,j;
    freopen("astar.in","r",stdin);
    while(scanf("%d %d",&row,&col) == 2)
    {
        forn(i,row) forn(j,col) scanf("%d",&board[i][j]);
        scanf("%d%d%d%d",&sx,&sy,&fx,&fy);
        result = astar(sy,sx);
        cout << "Optimal Output found expanding : " << result << "nodes  !!!!!!!!!!!! \n cost = " << dist[fx][fy]<< endl;
    }
    return 0;
}

Post a Comment

0 Comments