Prześlij pliki do ''

Tested AStar

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':
+			case 'B':
-				{
+			{
-					color("purple", "dark_yellow");
+				color("purple", "dark_yellow");
-				}break;
+			}break;
-				case 'T':
+			case 'T':
-				{
+			{
-					color("red", "dark_yellow");
+				color("red", "dark_yellow");
-				}break;
+			}break;
-				case '.':
+			case '.':
-				{
+			{
-					color("yellow", "dark_yellow");
+				color("yellow", "dark_yellow");
-				}break;
+			}break;
-				case '#':
+			case '#':
-				{
+			{
-					color("light_gray", "gray");
+				color("light_gray", "gray");
-				}break;
+			}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();
+bool isValid(int x, int y)
-		}break;
+{
+	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;