Zaktualizuj 'Main.cpp'

3 testy start1,2,3

Main.cpp

@@ -1,15 +1,33 @@
-#include<iostream>
+#include<iostream>
 #include<stdlib.h>
 #include<windows.h>
 #include<conio.h>
 #include<string>
+#include<list>
+#include<set>
+#include<math.h>
+#include<stack>
+
+#define maxfloat 999999
 
 using namespace std;
 
+const float maxFloat=maxfloat;
+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;
+char currentWay = 'S';
 
-void color(string foregroundColor,string backgroundColor)
+void color(string foregroundColor, string backgroundColor)
 {
 	HANDLE hOut;
 	hOut = GetStdHandle(STD_OUTPUT_HANDLE);
@@ -80,7 +98,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)
@@ -118,6 +136,10 @@ void updatePola()
 			{
 				color("red", "dark_yellow");
 			}break;
+			case 'G':
+			{
+				color("lime", "dark_yellow");
+			}break;
 			case '.':
 			{
 				color("yellow", "dark_yellow");
@@ -135,6 +157,43 @@ void updatePola()
 	}
 }
 
+void correctMovement(char wantedWay)
+{
+	while (currentWay != wantedWay)
+	{
+		switch (currentWay)
+		{
+			case 'N':
+			{
+				if (wantedWay == 'S')
+					currentWay = wantedWay;
+				else
+					currentWay = 'W';
+			}break;
+			case 'S':
+			{
+				if (wantedWay == 'N')
+					currentWay = wantedWay;
+				else
+					currentWay = 'W';
+			}break;
+			case 'W':
+			{
+				if (wantedWay == 'E')
+					currentWay = wantedWay;
+				else
+					currentWay = 'N';
+			}break;
+			case 'E':
+			{
+				if (wantedWay == 'W')
+					currentWay = wantedWay;
+				else
+					currentWay = 'N';
+			}break;
+		}
+	}
+}
 void Move(char kierunek)
 {
 	switch (kierunek)
@@ -142,58 +201,359 @@ void Move(char kierunek)
 		//góra-(w)
 		case 'w':
 		{
-			if (pole[pozycjaTraktoraX - 1][pozycjaTraktoraY][0] != '#')
+			if (pole[pozycjaTraktoraY - 1][pozycjaTraktoraX][0] != '#')
 			{
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
+				correctMovement('N');
-				pozycjaTraktoraX--;
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][0] = '.';
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
+				pozycjaTraktoraY--;
-				
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][0] = 'T';
 			}
 			updatePola();
 		}break;
-		//dó³-(s)
+		//dół-(s)
 		case 's':
 		{
-			if (pole[pozycjaTraktoraX + 1][pozycjaTraktoraY][0] != '#')
+			if (pole[pozycjaTraktoraY + 1][pozycjaTraktoraX][0] != '#')
 			{
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
+				correctMovement('S');
-				pozycjaTraktoraX++;
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][0] = '.';
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
+				pozycjaTraktoraY++;
-
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][0] = 'T';
 			}
 			updatePola();
 		}break;
 		//lewo-(a)
 		case 'a':
 		{
-			if (pole[pozycjaTraktoraX][pozycjaTraktoraY - 1][0] != '#')
+			if (pole[pozycjaTraktoraY][pozycjaTraktoraX - 1][0] != '#')
 			{
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
+				correctMovement('W');
-				pozycjaTraktoraY--;
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][0] = '.';
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
+				pozycjaTraktoraX--;
-
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][0] = 'T';
 			}
 			updatePola();
 		}break;
 		//prawo-(d)
 		case 'd':
 		{
-			if (pole[pozycjaTraktoraX][pozycjaTraktoraY + 1][0] != '#')
+			if (pole[pozycjaTraktoraY][pozycjaTraktoraX + 1][0] != '#')
 			{
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = '.';
+				correctMovement('E');
-				pozycjaTraktoraY++;
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][0] = '.';
-				pole[pozycjaTraktoraX][pozycjaTraktoraY][0] = 'T';
+				pozycjaTraktoraX++;
-
+				pole[pozycjaTraktoraY][pozycjaTraktoraX][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 "); //----start info
+	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();
+		if (p.first > pozycjaTraktoraX)
+			Move('d');
+		if (p.first < pozycjaTraktoraX)
+			Move('a');
+		if (p.second > pozycjaTraktoraY)
+			Move('s');
+		if (p.second < pozycjaTraktoraY)
+			Move('w');
+		
+		//printf("-> (%d,%d) ", p.first, p.second);  //---- informacja wierzchołku
+		Sleep(1000);
+	}
+
+	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 = maxFloat;
+			cellDetails[i][j].g = maxFloat;
+			cellDetails[i][j].h = maxFloat;
+			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;
+		double waga = 1.0;
+		waga = ((double)pole[j][i][1] - 48)*1.0;//----waga
+
+		//----------- 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 + waga;
+				hNew = calculateHValue(i - 1, j, dest);
+				fNew = gNew + hNew;
+
+				if (cellDetails[i - 1][j].f == maxFloat ||
+					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 + waga;
+				hNew = calculateHValue(i + 1, j, dest);
+				fNew = gNew + hNew;
+				if (cellDetails[i + 1][j].f == maxFloat ||
+					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 + waga;
+				hNew = calculateHValue(i, j + 1, dest);
+				fNew = gNew + hNew;
+				if (cellDetails[i][j + 1].f == maxFloat ||
+					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 + waga;
+				hNew = calculateHValue(i, j - 1, dest);
+				fNew = gNew + hNew;
+				if (cellDetails[i][j - 1].f == maxFloat ||
+					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 endX,int endY)
+{
+	updatePola();
+	Sleep(1000);
+	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(endX, endY);
+	aStarSearch(grid, src, dest);
+}
+
+void test1()
+{
+	pole[1][3][0] = 'B';
+	pole[1][3][1] = '9';
+	pole[3][1][0] = 'B';
+	pole[3][1][1] = '9';
+}
+void test2()
+{
+	for (int i = 1; i < 26; i++)
+	{
+		for (int j = 1; j < i; j++)
+		{
+			pole[i][j][0] = 'B';
+			pole[i][j][1] = '9';
+		}
+	}
+	test1();
+	updatePola();
+}
+
+void start1()
+{
+	int goalX = 3, goalY = 4;
+	test1();
+	pole[1][1][0] = 'T';
+	pole[1][1][1] = '1';
+	pole[goalY][goalX][0] = 'G';
+	pole[goalY][goalX][1] = '9';
+	gogo(goalX, goalY);
+}
+void start2()
+{
+	int goalX = 6, goalY = 6;
+	test2();
+	pole[1][1][0] = 'T';
+	pole[1][1][1] = '1';
+	pole[goalY][goalX][0] = 'G';
+	pole[goalY][goalX][1] = '9';
+	gogo(goalX, goalY);
+}
+void start3()
+{
+	int goalX = 6, goalY = 9;
+	test2();
+	pole[1][1][0] = 'T';
+	pole[1][1][1] = '1';
+	pole[goalY][goalX][0] = 'G';
+	pole[goalY][goalX][1] = '9';
+	gogo(goalX, goalY);
+}
+
 int main()
 {
 	SetWindow(50, 30);
-
 	//create pola//
 	for (int i = 0; i < 27; i++)
 	{
@@ -201,24 +561,29 @@ int main()
 		pole[0][i][0] = '#';
 		pole[26][i][0] = '#';
 		pole[i][26][0] = '#';
+		pole[i][0][1] = '9';
+		pole[0][i][1] = '9';
+		pole[26][i][1] = '9';
+		pole[i][26][1] = '9';
 	}
 	for (int i = 1; i < 26; i++)
 	{
 		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] = '9';
 	}
 
-    pole[1][1][0] = 'T';
-    
 	updatePola();
 
+	start3();  // testy start 1-3
 
 	//---------start---------//
 	bool traktorDziala = true;