Code:
#include <iostream>
#include <conio.h>
#include <vector>
#include <algorithm>
using namespace std;
const int HEIGHT = 7;
const int WIDTH = 7;
const int YES = 1;
const int NO = 0;
class Node
{
public:
int parent;
int x, y;
int ID;
int walkable;
int F, G, H;
int onOpenList;
int onClosedList;
};
void initNodes(char field[][WIDTH], Node aNode[], int targetX, int targetY);
int getPos(char field[][WIDTH], char ch, char dim);
void drawField(char field[][WIDTH]);
int getDistance(int x1, int y1, int x2, int y2);
int main()
{
////////////////////////////////////////////////////////////////////////////////////////////
//Initialization
////////////////////////////////////////////////////////////////////////////////////////////
//Initialize field
char field[HEIGHT][WIDTH] = { { '#', '#', '#', '#', '#', '#', '#' },
{ '#', ' ', ' ', '#', ' ', ' ', '#' },
{ '#', ' ', ' ', '#', ' ', ' ', '#' },
{ '#', ' ', 'S', '#', 'T', ' ', '#' },
{ '#', ' ', ' ', '#', ' ', ' ', '#' },
{ '#', ' ', ' ', ' ', ' ', ' ', '#' },
{ '#', '#', '#', '#', '#', '#', '#' } };
//Set current posistion to the startposition
int startX = getPos(field, 'S', 'x');
int startY = getPos(field, 'S', 'y');
int startID = WIDTH * startY + startX;
int currentX = startX;
int currentY = startY;
int currentID = WIDTH * currentY + currentX;
//Get the goal position
int targetX = getPos(field, 'T', 'x');
int targetY = getPos(field, 'T', 'y');
int targetID = WIDTH * targetY + targetX;
//Initialize nodes
Node aNode[HEIGHT * WIDTH];
initNodes(field, aNode, targetX, targetY);
//Create an openList and a closedList
vector<int> openList;
vector<int> openFList;
vector<int> closedList;
//Add the Starting square ID to open list
openList.push_back(currentID);
aNode[currentID].onOpenList = YES;
//Declare an iterator for later use
vector<int>::const_iterator iter;
//Path has not been found yet
int pathFound = NO;
//////////////////////////////////////////////////////////////////////////////////////////
//End of Initialization
//////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////
//Main Loop
//////////////////////////////////////////////////////////////////////////////////////////
//While we think there is a path and we haven't found it yet
while((!openList.empty()) && (pathFound == NO))
{
//Check ID's on openList and add their F to the openFList
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
//Get the lowest F on top of list
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ if(aNode[i].F == openFList[0])
{
//Make it current square
currentX = aNode[i].x;
currentY = aNode[i].y;
currentID = aNode[i].ID;
//Get off openList and openFList and add to closedList
int counter = 0;
for(iter = openList.begin(); iter < openList.end(); iter++)
{
if(*iter == currentID)
{
openList.erase(openList.begin() + counter);
aNode[counter].onOpenList = NO;
openFList.erase(openFList.begin());
closedList.push_back(currentID);
aNode[counter].onClosedList = YES;
}
counter++;
}
break;
}
}
}
}
//North Square:
//If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
if((aNode[((currentY - 1) * WIDTH) + currentX].walkable == YES) && (aNode[((currentY - 1) * WIDTH) + currentX].onClosedList == NO))
{
//If it isn’t on the open list
if(aNode[((currentY - 1) * WIDTH) + currentX].onOpenList == NO)
{
//Add to openList
openList.push_back(((currentY - 1) * WIDTH) + currentX);
aNode[((currentY - 1) * WIDTH) + currentX].onOpenList == YES;
//Update openFList
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
//Make the current square the parent of this square.
aNode[((currentY - 1) * WIDTH) + currentX].parent = currentID;
//Record the F, G, and H costs of the square. (H is already calculated in initiation)
//G = parents G + 1
aNode[((currentY - 1) * WIDTH) + currentX].G = aNode[currentID].G + 1;
//F = G + H
aNode[((currentY - 1) * WIDTH) + currentX].F = aNode[((currentY - 1) * WIDTH) + currentX].G + aNode[((currentY - 1) * WIDTH) + currentX].H;
}
//If it is on the open list already
if(aNode[((currentY - 1) * WIDTH) + currentX].onOpenList == YES)
{
//check to see if this path to that square is better, using G cost as the measure.
//A lower G cost means that this is a better path.
if((aNode[currentID].G + 1) < aNode[((currentY - 1) * WIDTH) + currentX].G)
{
//Change the parent of the square to the current square,
aNode[((currentY - 1) * WIDTH) + currentX].parent = currentID;
//and recalculate the G and F scores of the square.
aNode[((currentY - 1) * WIDTH) + currentX].G = aNode[currentID].G + 1;
aNode[((currentY - 1) * WIDTH) + currentX].F = aNode[((currentY - 1) * WIDTH) + currentX].G + aNode[((currentY - 1) * WIDTH) + currentX].H;
//Resort F List
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
}
}
}
//East Square:
//If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
if((aNode[((currentY) * WIDTH) + currentX + 1].walkable == YES) && (aNode[((currentY) * WIDTH) + currentX + 1].onClosedList == NO))
{
//If it isn’t on the open list
if(aNode[((currentY) * WIDTH) + currentX + 1].onOpenList == NO)
{
//Add to openList
openList.push_back(currentY * WIDTH + currentX + 1);
aNode[((currentY) * WIDTH) + currentX + 1].onOpenList == YES;
//Update openFList
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
//Make the current square the parent of this square.
aNode[((currentY) * WIDTH) + currentX + 1].parent = currentID;
//Record the F, G, and H costs of the square. (H is already calculated in initiation)
//G = parents G + 1
aNode[((currentY) * WIDTH) + currentX + 1].G = aNode[currentID].G + 1;
//F = G + H
aNode[((currentY) * WIDTH) + currentX + 1].F = aNode[((currentY) * WIDTH) + currentX + 1].G + aNode[((currentY) * WIDTH) + currentX + 1].H;
}
//If it is on the open list already
if(aNode[((currentY) * WIDTH) + currentX + 1].onOpenList == YES)
{
//check to see if this path to that square is better, using G cost as the measure.
//A lower G cost means that this is a better path.
if((aNode[currentID].G + 1) < aNode[((currentY) * WIDTH) + currentX + 1].G)
{
//Change the parent of the square to the current square,
aNode[((currentY) * WIDTH) + currentX + 1].parent = currentID;
//and recalculate the G and F scores of the square.
aNode[((currentY) * WIDTH) + currentX + 1].G = aNode[currentID].G + 1;
aNode[((currentY) * WIDTH) + currentX + 1].F = aNode[((currentY) * WIDTH) + currentX + 1].G + aNode[((currentY) * WIDTH) + currentX + 1].H;
//Update FList
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
}
}
}
//South Square:
//If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
if((aNode[((currentY + 1) * WIDTH) + currentX].walkable == YES) && (aNode[((currentY + 1) * WIDTH) + currentX].onClosedList == NO))
{
//If it isn’t on the open list
if(aNode[((currentY + 1) * WIDTH) + currentX].onOpenList == NO)
{
//Add to openList
openList.push_back(((currentY + 1) * HEIGHT) + currentX);
aNode[((currentY + 1) * WIDTH) + currentX].onOpenList == YES;
//Update openFList
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
//Make the current square the parent of this square.
aNode[((currentY + 1) * WIDTH) + currentX].parent = currentID;
//Record the F, G, and H costs of the square. (H is already calculated in initiation)
//G = parents G + 1
aNode[((currentY + 1) * WIDTH) + currentX].G = aNode[currentID].G + 1;
//F = G + H
aNode[((currentY + 1) * WIDTH) + currentX].F = aNode[((currentY + 1) * WIDTH) + currentX].G + aNode[((currentY + 1) * WIDTH) + currentX].H;
}
//If it is on the open list already
if(aNode[((currentY + 1) * WIDTH) + currentX].onOpenList == YES)
{
//check to see if this path to that square is better, using G cost as the measure.
//A lower G cost means that this is a better path.
if((aNode[currentID].G + 1) < aNode[((currentY + 1) * WIDTH) + currentX].G)
{
//Change the parent of the square to the current square,
aNode[((currentY + 1) * WIDTH) + currentX].parent = currentID;
//and recalculate the G and F scores of the square.
aNode[((currentY + 1) * WIDTH) + currentX].G = aNode[currentID].G + 1;
aNode[((currentY + 1) * WIDTH) + currentX].F = aNode[((currentY + 1) * WIDTH) + currentX].G + aNode[((currentY + 1) * WIDTH) + currentX].H;
//If you are keeping your open list sorted by F score,
//you may need to resort the list to account for the change.
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
}
}
}
//West Square:
//If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
if((aNode[((currentY) * WIDTH) + currentX - 1].walkable == YES) && (aNode[((currentY) * WIDTH) + currentX - 1].onClosedList == NO))
{
//If it isn’t on the open list
if(aNode[((currentY) * WIDTH) + currentX - 1].onOpenList == NO)
{
//Add to openList
openList.push_back(currentY * WIDTH + currentX - 1);
aNode[((currentY) * WIDTH) + currentX - 1].onOpenList == YES;
//Update openFList
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
//Make the current square the parent of this square.
aNode[((currentY) * WIDTH) + currentX - 1].parent = currentID;
//Record the F, G, and H costs of the square. (H is already calculated in initiation)
//G = parents G + 1
aNode[((currentY) * WIDTH) + currentX - 1].G = aNode[currentID].G + 1;
//F = G + H
aNode[((currentY) * WIDTH) + currentX - 1].F = aNode[((currentY) * WIDTH) + currentX - 1].G + aNode[((currentY) * WIDTH) + currentX - 1].H;
}
//If it is on the open list already
if(aNode[((currentY) * WIDTH) + currentX - 1].onOpenList == YES)
{
//check to see if this path to that square is better, using G cost as the measure.
//A lower G cost means that this is a better path.
if((aNode[currentID].G + 1) < aNode[((currentY) * WIDTH) + currentX - 1].G)
{
//Change the parent of the square to the current square
aNode[((currentY) * WIDTH) + currentX - 1].parent = currentID;
//and recalculate the G and F scores of the square.
aNode[((currentY) * WIDTH) + currentX - 1].G = aNode[currentID].G + 1;
aNode[((currentY) * WIDTH) + currentX - 1].F = aNode[((currentY) * WIDTH) + currentX - 1].G + aNode[((currentY) * WIDTH) + currentX - 1].H;
//Update FList
openFList.clear();
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < (HEIGHT * WIDTH); i++)
{ if(aNode[i].ID == *iter)
{ openFList.push_back(aNode[i].F); }
}
}
sort(openFList.begin(), openFList.end());
}
}
}
//Show open and closed in field
for(iter = openList.begin(); iter < openList.end(); iter++)
{ for(int i = 0; i < HEIGHT; i++)
{ for(int j = 0; j < WIDTH; j++)
{ if(*iter == (WIDTH * i) + j)
{ if(field[i][j] != 'S' && field[i][j] != 'T')
{ field[i][j] = 'O'; }
}
}
}
}
for(iter = closedList.begin(); iter < closedList.end(); iter++)
{ for(int i = 0; i < HEIGHT; i++)
{ for(int j = 0; j < WIDTH; j++)
{ if(*iter == (WIDTH * i) + j)
{ if(field[i][j] != 'S' && field[i][j] != 'T')
{field[i][j] = 'C'; }
}
}
}
}
//Some temporary thingy
drawField(field);
cout << "\nOpen List:\n";
for(iter = openList.begin(); iter < openList.end(); iter++)
{ cout << *iter << endl; }
cout << "\nOpen FList:\n";
for(iter = openFList.begin(); iter < openFList.end(); iter++)
{ cout << *iter << endl; }
cout << "\nClosedList:\n";
for(iter = closedList.begin(); iter < closedList.end(); iter++)
{ cout << *iter << endl; }
cout << "\nCurrentID: " << currentID << endl;
cout << "Parent of this square: " << aNode[currentID].parent << endl;
//Check if path has been found
for(iter = closedList.begin(); iter < closedList.end(); iter++)
{ if(*iter == targetID){ pathFound = YES; } }
getch();
}
cout << "\n\nGoal found.\n\n";
//Save the path by tracking back the parents from End to Start
currentID = targetID;
while(currentID != startID)
{
currentID = aNode[currentID].parent;
field[aNode[currentID].y][aNode[currentID].x] = '+';
cout << currentID << endl;
getch();
}
cout << "\n\nPath:\n\n";
drawField(field);
getch();
//////////////////////////////////////////////////////////////////////////////////////////
//End of Main Loop
//////////////////////////////////////////////////////////////////////////////////////////
return 0;
}
//////////////////////////////////////////////////////////////////////////////////////////////
//Functions
//////////////////////////////////////////////////////////////////////////////////////////////
void initNodes(char field[][WIDTH], Node aNode[], int targetX, int targetY)
{
//Initialize nodes
for(int i = 0; i < HEIGHT; i++)
{
for(int j = 0; j < WIDTH; j++)
{
aNode[WIDTH * i + j].parent = -1;
aNode[WIDTH * i + j].onOpenList = NO;
aNode[WIDTH * i + j].onClosedList = NO;
aNode[WIDTH * i + j].x = j;
aNode[WIDTH * i + j].y = i;
aNode[WIDTH * i + j].G = 0;
aNode[WIDTH * i + j].H = getDistance(j, i, targetX, targetY);
aNode[WIDTH * i + j].F = aNode[WIDTH * i + j].G + aNode[WIDTH * i + j].H;
aNode[WIDTH * i + j].ID = WIDTH * i + j;
if(field[i][j] == '#'){ aNode[WIDTH * i + j].walkable = NO; }
else{ aNode[WIDTH * i + j].walkable = YES; }
}
}
}
int getPos(char field[][WIDTH], char ch, char dim)
{
int a;
for(int i = 0; i < HEIGHT; i++)
{
for(int j = 0; j < WIDTH; j++)
{
if(field[i][j] == ch)
{
if(dim == 'x'){ a = j; }
if(dim == 'y'){ a = i; }
}
}
}
return a;
}
void drawField(char field[][WIDTH])
{
for(int i = 0; i < WIDTH; i++){ cout << "+-"; }
cout << "+\n";
for(int i = 0; i < HEIGHT; i++)
{
for(int j = 0; j < WIDTH; j++)
{
cout << "|" << field[i][j];
}
cout << "|\n";
for(int i = 0; i < WIDTH; i++){ cout << "+-"; }
cout << "+\n";
}
cout << endl;
}
int getDistance(int x1, int y1, int x2, int y2)
{
int dist;
int xDif;
int yDif;
if(x1 > x2){ xDif = (x1 - x2); }
if(x2 > x1){ xDif = (x2 - x1); }
if(x1 == x2){ xDif = 0; }
if(y1 > y2){ yDif = (y1 - y2); }
if(y2 > y1){ yDif = (y2 - y1); }
if(y1 == y2){ yDif = 0; }
dist = xDif + yDif;
return dist;
}als je de bovenstaande code compiled en een paar keer enter drukt dan zie je dat er ID's dubbel op de openList komen te staan wat helemaal niet hoort te kunnen
en als hij dan uiteindelijk de target vind en dan via de parents zijn weg terug merkt met +jes dan blijft hij heen en weer gaan tussen twee getallen die blijkbaar elkaar's parent zijn wat ook al niet klopt..ik heb de code al honderden keren overgekeken maar ik zie niet waar de fout(en) zit(ten).
Als iemand mij wil helpen ^^?
?