// Astar.cpp
// http://en.wikipedia.org/wiki/A*
// Compiler: Dev-C++ 4.9.9.2
// FB - 201012256
#include <iostream>
#include <iomanip>
#include <queue>
#include <string>
#include <math.h>
#include <ctime>
using namespace std;
const int n=60; // horizontal size of the map
const int m=60; // vertical size size of the map
static int map[n][m];
static int closed_nodes_map[n][m]; // map of closed (tried-out) nodes
static int open_nodes_map[n][m]; // map of open (not-yet-tried) nodes
static int dir_map[n][m]; // map of directions
const int dir=8; // number of possible directions to go at any position
// if dir==4
//static int dx[dir]={1, 0, -1, 0};
//static int dy[dir]={0, 1, 0, -1};
// if dir==8
static int dx[dir]={1, 1, 0, -1, -1, -1, 0, 1};
static int dy[dir]={0, 1, 1, 1, 0, -1, -1, -1};
class node
{
// current position
int xPos;
int yPos;
// total distance already travelled to reach the node
int level;
// priority=level+remaining distance estimate
int priority; // smaller: higher priority
public:
node(int xp, int yp, int d, int p)
{xPos=xp; yPos=yp; level=d; priority=p;}
int getxPos() const {return xPos;}
int getyPos() const {return yPos;}
int getLevel() const {return level;}
int getPriority() const {return priority;}
void updatePriority(const int & xDest, const int & yDest)
{
priority=level+estimate(xDest, yDest)*10; //A*
}
// give better priority to going strait instead of diagonally
void nextLevel(const int & i) // i: direction
{
level+=(dir==8?(i%2==0?10:14):10);
}
// Estimation function for the remaining distance to the goal.
const int & estimate(const int & xDest, const int & yDest) const
{
static int xd, yd, d;
xd=xDest-xPos;
yd=yDest-yPos;
// Euclidian Distance
d=static_cast<int>(sqrt(xd*xd+yd*yd));
// Manhattan distance
//d=abs(xd)+abs(yd);
// Chebyshev distance
//d=max(abs(xd), abs(yd));
return(d);
}
};
// Determine priority (in the priority queue)
bool operator<(const node & a, const node & b)
{
return a.getPriority() > b.getPriority();
}
// A-star algorithm.
// The route returned is a string of direction digits.
string pathFind( const int & xStart, const int & yStart,
const int & xFinish, const int & yFinish )
{
static priority_queue<node> pq[2]; // list of open (not-yet-tried) nodes
static int pqi; // pq index
static node* n0;
static node* m0;
static int i, j, x, y, xdx, ydy;
static char c;
pqi=0;
// reset the node maps
for(y=0;y<m;y++)
{
for(x=0;x<n;x++)
{
closed_nodes_map[x][y]=0;
open_nodes_map[x][y]=0;
}
}
// create the start node and push into list of open nodes
n0=new node(xStart, yStart, 0, 0);
n0->updatePriority(xFinish, yFinish);
pq[pqi].push(*n0);
open_nodes_map[x][y]=n0->getPriority(); // mark it on the open nodes map
// A* search
while(!pq[pqi].empty())
{
// get the current node w/ the highest priority
// from the list of open nodes
n0=new node( pq[pqi].top().getxPos(), pq[pqi].top().getyPos(),
pq[pqi].top().getLevel(), pq[pqi].top().getPriority());
x=n0->getxPos(); y=n0->getyPos();
pq[pqi].pop(); // remove the node from the open list
open_nodes_map[x][y]=0;
// mark it on the closed nodes map
closed_nodes_map[x][y]=1;
// quit searching when the goal state is reached
//if((*n0).estimate(xFinish, yFinish) == 0)
if(x==xFinish && y==yFinish)
{
// generate the path from finish to start
// by following the directions
string path="";
while(!(x==xStart && y==yStart))
{
j=dir_map[x][y];
c='0'+(j+dir/2)%dir;
path=c+path;
x+=dx[j];
y+=dy[j];
}
// garbage collection
delete n0;
// empty the leftover nodes
while(!pq[pqi].empty()) pq[pqi].pop();
return path;
}
// generate moves (child nodes) in all possible directions
for(i=0;i<dir;i++)
{
xdx=x+dx[i]; ydy=y+dy[i];
if(!(xdx<0 || xdx>n-1 || ydy<0 || ydy>m-1 || map[xdx][ydy]==1
|| closed_nodes_map[xdx][ydy]==1))
{
// generate a child node
m0=new node( xdx, ydy, n0->getLevel(),
n0->getPriority());
m0->nextLevel(i);
m0->updatePriority(xFinish, yFinish);
// if it is not in the open list then add into that
if(open_nodes_map[xdx][ydy]==0)
{
open_nodes_map[xdx][ydy]=m0->getPriority();
pq[pqi].push(*m0);
// mark its parent node direction
dir_map[xdx][ydy]=(i+dir/2)%dir;
}
else if(open_nodes_map[xdx][ydy]>m0->getPriority())
{
// update the priority info
open_nodes_map[xdx][ydy]=m0->getPriority();
// update the parent direction info
dir_map[xdx][ydy]=(i+dir/2)%dir;
// replace the node
// by emptying one pq to the other one
// except the node to be replaced will be ignored
// and the new node will be pushed in instead
while(!(pq[pqi].top().getxPos()==xdx &&
pq[pqi].top().getyPos()==ydy))
{
pq[1-pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pq[pqi].pop(); // remove the wanted node
// empty the larger size pq to the smaller one
if(pq[pqi].size()>pq[1-pqi].size()) pqi=1-pqi;
while(!pq[pqi].empty())
{
pq[1-pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pqi=1-pqi;
pq[pqi].push(*m0); // add the better node instead
}
else delete m0; // garbage collection
}
}
delete n0; // garbage collection
}
return ""; // no route found
}
int main()
{
srand(time(NULL));
// create empty map
for(int y=0;y<m;y++)
{
for(int x=0;x<n;x++) map[x][y]=0;
}
// fillout the map matrix with a '+' pattern
for(int x=n/8;x<n*7/8;x++)
{
map[x][m/2]=1;
}
for(int y=m/8;y<m*7/8;y++)
{
map[n/2][y]=1;
}
// randomly select start and finish locations
int xA, yA, xB, yB;
switch(rand()%8)
{
case 0: xA=0;yA=0;xB=n-1;yB=m-1; break;
case 1: xA=0;yA=m-1;xB=n-1;yB=0; break;
case 2: xA=n/2-1;yA=m/2-1;xB=n/2+1;yB=m/2+1; break;
case 3: xA=n/2-1;yA=m/2+1;xB=n/2+1;yB=m/2-1; break;
case 4: xA=n/2-1;yA=0;xB=n/2+1;yB=m-1; break;
case 5: xA=n/2+1;yA=m-1;xB=n/2-1;yB=0; break;
case 6: xA=0;yA=m/2-1;xB=n-1;yB=m/2+1; break;
case 7: xA=n-1;yA=m/2+1;xB=0;yB=m/2-1; break;
}
cout<<"Map Size (X,Y): "<<n<<","<<m<<endl;
cout<<"Start: "<<xA<<","<<yA<<endl;
cout<<"Finish: "<<xB<<","<<yB<<endl;
// get the route
clock_t start = clock();
string route=pathFind(xA, yA, xB, yB);
if(route=="") cout<<"An empty route generated!"<<endl;
clock_t end = clock();
double time_elapsed = double(end - start);
cout<<"Time to calculate the route (ms): "<<time_elapsed<<endl;
cout<<"Route:"<<endl;
cout<<route<<endl<<endl;
// follow the route on the map and display it
if(route.length()>0)
{
int j; char c;
int x=xA;
int y=yA;
map[x][y]=2;
for(int i=0;i<route.length();i++)
{
c =route.at(i);
j=atoi(&c);
x=x+dx[j];
y=y+dy[j];
map[x][y]=3;
}
map[x][y]=4;
// display the map with the route
for(int y=0;y<m;y++)
{
for(int x=0;x<n;x++)
if(map[x][y]==0)
cout<<".";
else if(map[x][y]==1)
cout<<"O"; //obstacle
else if(map[x][y]==2)
cout<<"S"; //start
else if(map[x][y]==3)
cout<<"R"; //route
else if(map[x][y]==4)
cout<<"F"; //finish
cout<<endl;
}
}
getchar(); // wait for a (Enter) keypress
return(0);
}
Diff to Previous Revision
--- revision 3 2010-12-25 22:02:56
+++ revision 4 2010-12-25 23:36:35
@@ -2,15 +2,6 @@
// http://en.wikipedia.org/wiki/A*
// Compiler: Dev-C++ 4.9.9.2
// FB - 201012256
-
-// Description:
-// Create N*M integer matrix as a map.
-// Element: 0 => passable, 1 => non-passable
-// Randomly select 2 passable positions in the matrix
-// and generate the shortest path in between them.
-// Path will be a string of digits with each 0 to 7 (using 8 directions).
-// (If no path is possible then an empty string is returned.)
-
#include <iostream>
#include <iomanip>
#include <queue>
@@ -19,36 +10,19 @@
#include <ctime>
using namespace std;
-// map matrix
-const int n=60; // horizontal size
-const int m=60; // vertical size
+const int n=60; // horizontal size of the map
+const int m=60; // vertical size size of the map
static int map[n][m];
-
-// create empty map of closed (tried-out) nodes
-static int closed_nodes_map[n][m];
-// create empty map of open (not-yet-tried) nodes
-static int open_nodes_map[n][m];
-
-// create empty map of directions
-static int dir_map[n][m];
-
-// number of possible directions to go at any position is 8
-const int dir=8; // another option canbe 4
-int dx[dir];
-int dy[dir];
-// create the direction arrays
-void create_dir(void)
-{
- const float pi=4*atan(1);
- float ang;
-
- for(int i=0;i<dir;i++)
- {
- ang=2*pi/dir*i;
- dx[i]=static_cast<int>(round(cos(ang)));
- dy[i]=static_cast<int>(round(sin(ang)));
- }
-}
+static int closed_nodes_map[n][m]; // map of closed (tried-out) nodes
+static int open_nodes_map[n][m]; // map of open (not-yet-tried) nodes
+static int dir_map[n][m]; // map of directions
+const int dir=8; // number of possible directions to go at any position
+// if dir==4
+//static int dx[dir]={1, 0, -1, 0};
+//static int dy[dir]={0, 1, 0, -1};
+// if dir==8
+static int dx[dir]={1, 1, 0, -1, -1, -1, 0, 1};
+static int dy[dir]={0, 1, 1, 1, 0, -1, -1, -1};
class node
{
@@ -77,7 +51,7 @@
// give better priority to going strait instead of diagonally
void nextLevel(const int & i) // i: direction
{
- level+=((i%2==0)?10:14);
+ level+=(dir==8?(i%2==0?10:14):10);
}
// Estimation function for the remaining distance to the goal.
@@ -106,7 +80,8 @@
return a.getPriority() > b.getPriority();
}
-// Pathfinder using A-star algorithm
+// A-star algorithm.
+// The route returned is a string of direction digits.
string pathFind( const int & xStart, const int & yStart,
const int & xFinish, const int & yFinish )
{
@@ -168,8 +143,7 @@
// garbage collection
delete n0;
// empty the leftover nodes
- while(!pq[pqi].empty()) pq[pqi].pop();
-
+ while(!pq[pqi].empty()) pq[pqi].pop();
return path;
}
@@ -223,23 +197,17 @@
}
pqi=1-pqi;
pq[pqi].push(*m0); // add the better node instead
-
}
else delete m0; // garbage collection
}
}
delete n0; // garbage collection
}
-
return ""; // no route found
}
-
-//
int main()
{
- create_dir(); // create the direction arrays
-
srand(time(NULL));
// create empty map
@@ -258,11 +226,8 @@
map[n/2][y]=1;
}
- cout<<"Map Size (X,Y): "<<n<<","<<m<<endl;
-
+ // randomly select start and finish locations
int xA, yA, xB, yB;
-
- // randomly select start and finish locations
switch(rand()%8)
{
case 0: xA=0;yA=0;xB=n-1;yB=m-1; break;
@@ -275,9 +240,9 @@
case 7: xA=n-1;yA=m/2+1;xB=0;yB=m/2-1; break;
}
+ cout<<"Map Size (X,Y): "<<n<<","<<m<<endl;
cout<<"Start: "<<xA<<","<<yA<<endl;
cout<<"Finish: "<<xB<<","<<yB<<endl;
-
// get the route
clock_t start = clock();
string route=pathFind(xA, yA, xB, yB);
@@ -285,7 +250,6 @@
clock_t end = clock();
double time_elapsed = double(end - start);
cout<<"Time to calculate the route (ms): "<<time_elapsed<<endl;
-
cout<<"Route:"<<endl;
cout<<route<<endl<<endl;
@@ -320,12 +284,9 @@
cout<<"R"; //route
else if(map[x][y]==4)
cout<<"F"; //finish
-
cout<<endl;
}
-
- }
-
+ }
getchar(); // wait for a (Enter) keypress
return(0);
}
