|
#include
<stdio.h> // used for reading in the txt file
and for printf output
#include
<assert.h> // some debugging code extra's
#include
<math.h> // for sqrt function
#include
<vector> // STL for our two arrays.
using
namespace std;
// Our data from our text file - map.txt, is stored in this array.
char
map[10][10];
void
ReadMap()
{
FILE* fp = fopen("map.txt", "r");
char c;
int i=0, j=0;
while( fread(&c, 1, 1, fp) != EOF )
{
if( (c != ' ') && (c!='\n') )
// skip white spaces
{
map[i][j]=c;
j++;
}
if( j== 10 && i==9 )
break;
if( j== 10 ){ j=0; i++;}
}//
End while loop
fclose(fp);
}//
End of ReadMap()
// Used for debugging, to check taht we have read in the map
// correctly - else its unused.
void
PrintMap()
{
printf("\n");
for(int y=0;
y<10; y++)
{
for(int
x=0; x<10; x++)
{
printf("%c ", map[y][x] );
}// End inner for loop
printf("\n");
}//
End outer for loop
}//
End of PrintMap()
struct
node
{
node* parent;
int x, y;
char c;
float f, g, h;
node()
{
parent=NULL;
}
};
char
GetPosValue(int x,
int y)
{
// top, top_right, right, bottom_right, bottom,
left_bottom, left, top_left
if( y < 0 ) return
'1'; // outside the map, hence return wall;
if( y > 9 ) return
'1';
if( x < 0 ) return
'1';
if( x > 9 ) return
'1';
return map[y][x];
}//
end of closed
void
FindEndPoint(int* xend,
int* yend)
{
*xend
= 0;
*yend
= 0;
// Find our end point -3- in this case
for(int y=0;
y<10; y++)
{
for(int
x=0; x<10; x++)
{
if( map[y][x] == '3')
{
*xend = x;
*yend = y;
break;
}
}// End inner for loop
}//
End of outer for loop
}//
End of FindEndPoint()
float
DistanceToTarget(int xpoint,
int ypoint,
int xend,
int yend)
{
float dist = (float)((xend
- xpoint)*(xend - xpoint) + (yend - ypoint)*(yend - ypoint));
dist = (float)sqrtf((float)dist);
return dist;
}//
End of DistanceToTarget()
// It takes the parent node, and an array of 8 nodes e.g. n[8]...
// it then fills in the values for the 8 surounding nodes.
void
GenerateSuccessors(node* n, node* p, int
xend, int yend)
{
int x = p->x;
int y = p->y;
float g = p->g;
/*
(top_left) (top) (top_right)
(left) (p) (right)
(bottom_left) (bottom) (bottom_right)
*/
n[0].x = x;
n[0].y = y-1;
n[0].c = GetPosValue(x, y-1); // top
n[0].parent = p;
n[0].g = g + 1;
int k = 0;
n[0].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[0].f = n[k].g + n[k].h;
n[1].c = GetPosValue(x+1, y-1); // top_right
n[1].x = x+1;
n[1].y = y-1;
n[1].parent = p;
n[1].g = g + sqrtf(2.0f);//0.5f;
k =
1;
n[1].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[1].f = n[k].g + n[k].h;
n[2].c = GetPosValue(x+1, y); // right
n[2].x = x+1;
n[2].y = y;
n[2].parent = p;
n[2].g = g + 1;
k =
2;
n[2].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[2].f = n[k].g + n[k].h;
n[3].c = GetPosValue(x+1, y+1); // bottom_right
n[3].x = x+1;
n[3].y = y+1;
n[3].parent = p;
n[3].g = g + sqrtf(2.0f);;
k =
3;
n[3].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[3].f = n[k].g + n[k].h;
n[4].c = GetPosValue(x, y+1); // bottom
n[4].x = x;
n[4].y = y+1;
n[4].parent = p;
n[4].g = g + 1;
k =
4;
n[4].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[4].f = n[k].g + n[k].h;
n[5].c = GetPosValue(x-1, y+1); // bottom_left
n[5].x = x-1;
n[5].y = y+1;
n[5].parent = p;
n[5].g = g + sqrtf(2.0f);;
k =
5;
n[5].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[5].f = n[k].g + n[k].h;
n[6].c = GetPosValue(x-1, y); // left
n[6].x = x-1;
n[6].y = y;
n[6].parent = p;
n[6].g = g + 1;
k =
6;
n[6].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[6].f = n[k].g + n[k].h;
n[7].c = GetPosValue(x-1, y-1); // top_left
n[7].x = x-1;
n[7].y = y-1;
n[7].parent = p;
n[7].g = g + sqrtf(2.0f);;
k =
7;
n[7].h = DistanceToTarget(n[k].x, n[k].y, xend,yend);
n[7].f = n[k].g + n[k].h;
}//
End of GenerateSuccessors(..)
int
GetNodeWithLeastDist(node* n)
{
// Find the first node, which isn't a wall, e.g.
not '1'
int sel=0;
while( n[sel].c != '0' ) sel++;
// Possible bug, so I'll put an assert here -
incase we silly enough
// put all walls around our starting point.
assert( !(sel >= 8) && ("Error no walls in GetNodeWithLeastDist(..)") );
// Now compare our value with the others, of
course checking
// that its not a wall.
for( int i=1;
i<8; i++)
{
if( (n[i].f < n[sel].f) && (n[i].c
== '0') )
{
sel = i;
}
}//
End for loop
return sel;
}//
End of GetNodeWithLeastDist(..)
// You could probably fix the one line that needs this function, but it
// makes the program easier to follow - its only got one line different
// from the LeastDist version above.
int
GetNodeWithLargestDist(node* n)
{
// Find the first node, which isn't a wall, e.g.
not '1'
int sel=0;
while( n[sel].c != '0' ) sel++;
// Now compare our value with the others, of
course checking
// that its not a wall.
for( int i=1;
i<8; i++)
{
if( (n[i].f > n[sel].f) && (n[i].c
== '0') )
{
sel = i;
}
}//
End for loop
return sel;
}//
End of GetNodeWithLargestDist(..)
// Go through, or read in map of characters, and find the starting
// position, which in this case is a '2'
void
GetStartPos(int* xstart,
int* ystart)
{
// Find our starting point -2- in this case
for(int y=0;
y<10; y++)
{
for(int
x=0; x<10; x++)
{
if( map[y][x] == '2')
{
*xstart = x;
*ystart = y;
break;
}
}// End inner for loop
}//
End of outer for loop
}//
End of GetStartPos(..)
// Determines if the passed node is in the closed list, true
// for yes...and false for no.
bool
inClosedList(vector<node>* closedlist, node n)
{
int count = (int)closedlist->size();
for (vector<node>::iterator nn = closedlist->begin();
nn != closedlist->end(); nn++)
{
if( (nn->x == n.x)&&(nn->y == n.y) )
{
return
true;
}// end if
}//
end for loop nn
return false;
// not in our closedlist
}//
End inClosedList(..)
// Returns true, if the node we have passed in, is at our goal
// position - hence success!
bool
CheckForSuccess(node* n, int* iWhich)
{
for( int i=0;
i<8; i++ )
{
if( n[i].c == '3' )
// reached goal
{
*iWhich = i;
return
true;
}
}//
end for loop
return false;
}//
End of CheckForSuccess(..)
// program entry point
void
main()
{
// Read in our 10x10 map from a text file
ReadMap();
// Debug - print our data to the screen, just to
make sure its read
// in correctly.
//PrintMap();
int xstart=0;
int ystart=0;
node startnode;
GetStartPos(&xstart, &ystart);
int xend, yend;
FindEndPoint(&xend, ¥d);
vector<node> openlist;
vector<node> closelist;
startnode.x = xstart;
startnode.y = ystart;
startnode.c = 2;
startnode.f = 0;
startnode.g = DistanceToTarget(xstart, ystart, xend,yend);
openlist.push_back(startnode);
bool bFound = false;
int s = (int)openlist.size();
while( (s > 0) && (bFound==false)
)
{
int h = (int)openlist.size();
node q = openlist[h-1];
// Generate the 8 successors for the
start node, and fill
// in there values
node n[8];
GenerateSuccessors(n, &q, xend,yend);
// Check if any of the successors have
reached the goal
int iWhich = 0;
bool success = CheckForSuccess(n,
&iWhich);
if( success ==
true )
{
closelist.push_back(q);
closelist.push_back(n[iWhich]);
openlist.push_back(n[iWhich]);
bFound= true;
break;
}
// Loop through each node, and pick
the node thats the least
// distance and is not a wall ...and
of course a node we havn't
// been down before, which is stored
in close list.
bool bPicked =
false;
for(int
i=0; i<8; i++)
{
// returns the node with the
least distance, and doesn't
// return a wall!..checks that
its not a '1'
int sel =
GetNodeWithLeastDist(n);
//Check if a node with this
position is in the closed list
bool bCheck = inClosedList(&closelist,
n[sel]);
if( bCheck )
{
// Sort of a hack fix, so
that the next time round, we don't just
// pick the same node with
the lowest value, instead we make this one
// the largest.
n[sel].f = n[GetNodeWithLargestDist(n)].f+1;
continue;
}
else
{
openlist.push_back( n[sel] );
bPicked = true;
//Debug - uncomment line
to see which nodes are picked as the
//program itterates along.
//printf("node: %d %d\n",
n[sel].x, n[sel].y);
break;
}
}// End for loop
if( bPicked==false
)
{
openlist.pop_back();
//openlist.erase( openlist.begin()
+ 0 );
}
closelist.push_back(q);
s = (int)openlist.size();
}//
End of while loop
// Displays the most optimal path from start to
end.
printf("\n\nOpenList (x,y coords of shortest path)\n");
for(unsigned
int i=0; i< openlist.size(); i++)
{
printf("node: %d %d\n", openlist[i].x, openlist[i].y);
}
// Displays the complete path that was searched,
step by step
printf("\n\nCloseList (x,y of how we got our optimised path)\n");
for(unsigned
int i=0; i< closelist.size(); i++)
{
printf("node: %d %d\n", closelist[i].x, closelist[i].y);
}
}//
End main() |
