Prześlij pliki do ''
Tested AStar
This commit is contained in:
parent
0df9493538
commit
914bdcca0e
373
Main.cpp
373
Main.cpp
@ -1,15 +1,29 @@
|
||||
#include<iostream>
|
||||
#include<iostream>
|
||||
#include<stdlib.h>
|
||||
#include<windows.h>
|
||||
#include<conio.h>
|
||||
#include<string>
|
||||
#include<list>
|
||||
#include<set>
|
||||
#include<math.h>
|
||||
#include<stack>
|
||||
|
||||
using namespace std;
|
||||
const int ROW = 27;
|
||||
const int COL = 27;
|
||||
typedef pair<int, int> Pair;
|
||||
typedef pair<double, pair<int, int>> pPair;
|
||||
struct cell
|
||||
{
|
||||
int parent_i, parent_j;
|
||||
double f, g, h;
|
||||
};
|
||||
|
||||
|
||||
char pole[27][27][2];
|
||||
int pozycjaTraktoraX = 1, pozycjaTraktoraY = 1;
|
||||
|
||||
void color(string foregroundColor,string backgroundColor)
|
||||
void color(string foregroundColor, string backgroundColor)
|
||||
{
|
||||
HANDLE hOut;
|
||||
hOut = GetStdHandle(STD_OUTPUT_HANDLE);
|
||||
@ -80,7 +94,7 @@ void color(string foregroundColor,string backgroundColor)
|
||||
backgroundCode = 14;
|
||||
if (backgroundColor == "white")
|
||||
backgroundCode = 15;
|
||||
SetConsoleTextAttribute(hOut, foregroundCode + backgroundCode*16);
|
||||
SetConsoleTextAttribute(hOut, foregroundCode + backgroundCode * 16);
|
||||
}
|
||||
|
||||
void SetWindow(int Width, int Height)
|
||||
@ -110,22 +124,22 @@ void updatePola()
|
||||
char item = pole[i][j][0];
|
||||
switch (item)
|
||||
{
|
||||
case 'B':
|
||||
{
|
||||
color("purple", "dark_yellow");
|
||||
}break;
|
||||
case 'T':
|
||||
{
|
||||
color("red", "dark_yellow");
|
||||
}break;
|
||||
case '.':
|
||||
{
|
||||
color("yellow", "dark_yellow");
|
||||
}break;
|
||||
case '#':
|
||||
{
|
||||
color("light_gray", "gray");
|
||||
}break;
|
||||
case 'B':
|
||||
{
|
||||
color("purple", "dark_yellow");
|
||||
}break;
|
||||
case 'T':
|
||||
{
|
||||
color("red", "dark_yellow");
|
||||
}break;
|
||||
case '.':
|
||||
{
|
||||
color("yellow", "dark_yellow");
|
||||
}break;
|
||||
case '#':
|
||||
{
|
||||
color("light_gray", "gray");
|
||||
}break;
|
||||
}
|
||||
cout << pole[i][j][0];
|
||||
|
||||
@ -140,7 +154,63 @@ void Move(char kierunek)
|
||||
switch (kierunek)
|
||||
{
|
||||
//góra-(w)
|
||||
case 'w':
|
||||
case 'w':
|
||||
{
|
||||
if (pole[pozycjaTraktoraX - 1][pozycjaTraktoraY][0] != '#')
|
||||
{
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
|
||||
pozycjaTraktoraX--;
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
|
||||
|
||||
}
|
||||
updatePola();
|
||||
}break;
|
||||
//dó³-(s)
|
||||
case 's':
|
||||
{
|
||||
if (pole[pozycjaTraktoraX + 1][pozycjaTraktoraY][0] != '#')
|
||||
{
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
|
||||
pozycjaTraktoraX++;
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
|
||||
|
||||
}
|
||||
updatePola();
|
||||
}break;
|
||||
//lewo-(a)
|
||||
case 'a':
|
||||
{
|
||||
if (pole[pozycjaTraktoraX][pozycjaTraktoraY - 1][0] != '#')
|
||||
{
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
|
||||
pozycjaTraktoraY--;
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
|
||||
|
||||
}
|
||||
updatePola();
|
||||
}break;
|
||||
//prawo-(d)
|
||||
case 'd':
|
||||
{
|
||||
if (pole[pozycjaTraktoraX][pozycjaTraktoraY + 1][0] != '#')
|
||||
{
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
|
||||
pozycjaTraktoraY++;
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
|
||||
|
||||
}
|
||||
updatePola();
|
||||
}break;
|
||||
}
|
||||
}
|
||||
|
||||
void MoveOwn(char akcja)
|
||||
{
|
||||
/*
|
||||
switch (akcja)
|
||||
{
|
||||
//do przodu
|
||||
case 'd':
|
||||
{
|
||||
if (pole[pozycjaTraktoraX - 1][pozycjaTraktoraY][0] != '#')
|
||||
{
|
||||
@ -151,7 +221,7 @@ void Move(char kierunek)
|
||||
}
|
||||
updatePola();
|
||||
}break;
|
||||
//dó³-(s)
|
||||
//dół-(s)
|
||||
case 's':
|
||||
{
|
||||
if (pole[pozycjaTraktoraX + 1][pozycjaTraktoraY][0] != '#')
|
||||
@ -175,25 +245,253 @@ void Move(char kierunek)
|
||||
}
|
||||
updatePola();
|
||||
}break;
|
||||
//prawo-(d)
|
||||
case 'd':
|
||||
{
|
||||
if (pole[pozycjaTraktoraX][pozycjaTraktoraY + 1][0] != '#')
|
||||
{
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
|
||||
pozycjaTraktoraY++;
|
||||
pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
|
||||
}*/
|
||||
}
|
||||
|
||||
}
|
||||
updatePola();
|
||||
}break;
|
||||
|
||||
bool isValid(int x, int y)
|
||||
{
|
||||
if (pole[x][y][0] != '#')
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool isDestination(int x, int y,Pair dest)
|
||||
{
|
||||
if (dest.first == x && dest.second == y)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
double calculateHValue(int x, int y, Pair dest)
|
||||
{
|
||||
return abs(x - dest.first) + abs(y - dest.second);
|
||||
}
|
||||
|
||||
void tracePath(cell cellDetails[][COL], Pair dest)
|
||||
{
|
||||
printf("\nThe Path is ");
|
||||
int row = dest.first;
|
||||
int col = dest.second;
|
||||
|
||||
stack<Pair> Path;
|
||||
|
||||
while (!(cellDetails[row][col].parent_i == row
|
||||
&& cellDetails[row][col].parent_j == col))
|
||||
{
|
||||
Path.push(make_pair(row, col));
|
||||
int temp_row = cellDetails[row][col].parent_i;
|
||||
int temp_col = cellDetails[row][col].parent_j;
|
||||
row = temp_row;
|
||||
col = temp_col;
|
||||
}
|
||||
|
||||
Path.push(make_pair(row, col));
|
||||
while (!Path.empty())
|
||||
{
|
||||
pair<int, int> p = Path.top();
|
||||
Path.pop();
|
||||
printf("-> (%d,%d) ", p.first, p.second);// jeśli sec+1=posTraktorX to koleny ruch w prawo Move('d')
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void aStarSearch(int grid[][COL],Pair src, Pair dest)
|
||||
{
|
||||
bool closedList[ROW][COL];
|
||||
memset(closedList, false, sizeof(closedList));
|
||||
|
||||
cell cellDetails[ROW][COL];
|
||||
|
||||
int i, j;
|
||||
for (i = 0; i < ROW; i++)
|
||||
{
|
||||
for (j = 0; j < COL; j++)
|
||||
{
|
||||
cellDetails[i][j].f = FLT_MAX;
|
||||
cellDetails[i][j].g = FLT_MAX;
|
||||
cellDetails[i][j].h = FLT_MAX;
|
||||
cellDetails[i][j].parent_i = -1;
|
||||
cellDetails[i][j].parent_j = -1;
|
||||
}
|
||||
}
|
||||
i = src.first, j = src.second;
|
||||
cellDetails[i][j].f = 0.0;
|
||||
cellDetails[i][j].g = 0.0;
|
||||
cellDetails[i][j].h = 0.0;
|
||||
cellDetails[i][j].parent_i = i;
|
||||
cellDetails[i][j].parent_j = j;
|
||||
|
||||
set<pPair> openList;
|
||||
openList.insert(make_pair(0.0, make_pair(i, j)));
|
||||
bool foundDest = false;
|
||||
|
||||
while (!openList.empty())
|
||||
{
|
||||
pPair p = *openList.begin();
|
||||
openList.erase(openList.begin());
|
||||
i = p.second.first;
|
||||
j = p.second.second;
|
||||
closedList[i][j] = true;
|
||||
|
||||
double gNew, hNew, fNew;
|
||||
|
||||
//----------- 1st Successor (North) ------------
|
||||
if (isValid(i - 1, j) == true)
|
||||
{
|
||||
if (isDestination(i - 1, j, dest) == true)
|
||||
{
|
||||
cellDetails[i - 1][j].parent_i = i;
|
||||
cellDetails[i - 1][j].parent_j = j;
|
||||
//printf("The destination cell is found\n");
|
||||
tracePath(cellDetails, dest);
|
||||
foundDest = true;
|
||||
return;
|
||||
}
|
||||
else if (closedList[i - 1][j] == false)
|
||||
{
|
||||
gNew = cellDetails[i][j].g + 1.0; //???????-waga
|
||||
hNew = calculateHValue(i - 1, j, dest);
|
||||
fNew = gNew + hNew;
|
||||
|
||||
if (cellDetails[i - 1][j].f == FLT_MAX ||
|
||||
cellDetails[i - 1][j].f > fNew)
|
||||
{
|
||||
openList.insert(make_pair(fNew,
|
||||
make_pair(i - 1, j)));
|
||||
|
||||
|
||||
cellDetails[i - 1][j].f = fNew;
|
||||
cellDetails[i - 1][j].g = gNew;
|
||||
cellDetails[i - 1][j].h = hNew;
|
||||
cellDetails[i - 1][j].parent_i = i;
|
||||
cellDetails[i - 1][j].parent_j = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//----------- 2nd Successor (South) ------------
|
||||
if (isValid(i + 1, j) == true)
|
||||
{
|
||||
if (isDestination(i + 1, j, dest) == true)
|
||||
{
|
||||
cellDetails[i + 1][j].parent_i = i;
|
||||
cellDetails[i + 1][j].parent_j = j;
|
||||
//printf("The destination cell is found\n");
|
||||
tracePath(cellDetails, dest);
|
||||
foundDest = true;
|
||||
return;
|
||||
}
|
||||
else if (closedList[i + 1][j] == false)
|
||||
{
|
||||
gNew = cellDetails[i][j].g + 1.0;
|
||||
hNew = calculateHValue(i + 1, j, dest);
|
||||
fNew = gNew + hNew;
|
||||
if (cellDetails[i + 1][j].f == FLT_MAX ||
|
||||
cellDetails[i + 1][j].f > fNew)
|
||||
{
|
||||
openList.insert(make_pair(fNew, make_pair(i + 1, j)));
|
||||
cellDetails[i + 1][j].f = fNew;
|
||||
cellDetails[i + 1][j].g = gNew;
|
||||
cellDetails[i + 1][j].h = hNew;
|
||||
cellDetails[i + 1][j].parent_i = i;
|
||||
cellDetails[i + 1][j].parent_j = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//----------- 3rd Successor (East) ------------
|
||||
if (isValid(i, j + 1) == true)
|
||||
{
|
||||
if (isDestination(i, j + 1, dest) == true)
|
||||
{
|
||||
cellDetails[i][j + 1].parent_i = i;
|
||||
cellDetails[i][j + 1].parent_j = j;
|
||||
//printf("The destination cell is found\n");
|
||||
tracePath(cellDetails, dest);
|
||||
foundDest = true;
|
||||
return;
|
||||
}
|
||||
else if (closedList[i][j + 1] == false)
|
||||
{
|
||||
gNew = cellDetails[i][j].g + 1.0;
|
||||
hNew = calculateHValue(i, j + 1, dest);
|
||||
fNew = gNew + hNew;
|
||||
if (cellDetails[i][j + 1].f == FLT_MAX ||
|
||||
cellDetails[i][j + 1].f > fNew)
|
||||
{
|
||||
openList.insert(make_pair(fNew,
|
||||
make_pair(i, j + 1)));
|
||||
cellDetails[i][j + 1].f = fNew;
|
||||
cellDetails[i][j + 1].g = gNew;
|
||||
cellDetails[i][j + 1].h = hNew;
|
||||
cellDetails[i][j + 1].parent_i = i;
|
||||
cellDetails[i][j + 1].parent_j = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//----------- 4th Successor (West) ------------
|
||||
if (isValid(i, j - 1) == true)
|
||||
{
|
||||
if (isDestination(i, j - 1, dest) == true)
|
||||
{
|
||||
cellDetails[i][j - 1].parent_i = i;
|
||||
cellDetails[i][j - 1].parent_j = j;
|
||||
//printf("The destination cell is found\n");
|
||||
tracePath(cellDetails, dest);
|
||||
foundDest = true;
|
||||
return;
|
||||
}
|
||||
else if (closedList[i][j - 1] == false)
|
||||
{
|
||||
gNew = cellDetails[i][j].g + 1.0;
|
||||
hNew = calculateHValue(i, j - 1, dest);
|
||||
fNew = gNew + hNew;
|
||||
if (cellDetails[i][j - 1].f == FLT_MAX ||
|
||||
cellDetails[i][j - 1].f > fNew)
|
||||
{
|
||||
openList.insert(make_pair(fNew,
|
||||
make_pair(i, j - 1)));
|
||||
cellDetails[i][j - 1].f = fNew;
|
||||
cellDetails[i][j - 1].g = gNew;
|
||||
cellDetails[i][j - 1].h = hNew;
|
||||
cellDetails[i][j - 1].parent_i = i;
|
||||
cellDetails[i][j - 1].parent_j = j;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (foundDest == false)
|
||||
printf("Failed to find the Destination Cell\n");
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void gogo()
|
||||
{
|
||||
int grid[27][27];
|
||||
for (int i = 0; i < 27; i++)
|
||||
{
|
||||
for (int j = 0; j < 27; j++)
|
||||
{
|
||||
grid[i][j] = 0;
|
||||
}
|
||||
}
|
||||
Pair src = make_pair(pozycjaTraktoraX, pozycjaTraktoraY);
|
||||
Pair dest = make_pair(pozycjaTraktoraX+2, pozycjaTraktoraY+3);
|
||||
aStarSearch(grid, src, dest);
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
SetWindow(50, 30);
|
||||
|
||||
//create pola//
|
||||
for (int i = 0; i < 27; i++)
|
||||
{
|
||||
@ -207,18 +505,23 @@ int main()
|
||||
for (int j = 1; j < 26; j++)
|
||||
{
|
||||
pole[i][j][0] = '.';
|
||||
pole[i][j][1] = '1';
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 25; i++)
|
||||
{
|
||||
pole[i+1][i+1][0] = 'B';
|
||||
pole[i + 1][i + 1][0] = 'B';
|
||||
pole[i + 1][i + 1][1] = '10';
|
||||
}
|
||||
|
||||
pole[1][1][0] = 'T';
|
||||
pole[1][1][0] = 'T';
|
||||
|
||||
updatePola();
|
||||
|
||||
gogo();
|
||||
|
||||
|
||||
|
||||
//---------start---------//
|
||||
bool traktorDziala = true;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user