#include #define N 15 #define M 30 #define startx 2 #define starty 28 #define endx 2 #define endy 2 #include using namespace std; #include #include class node { public: int id, x, y; bool obstacle = false, inlist1 = false, inlist2 = false; int g, h, f; char symbol; }; node nodes[N][M]; void sorting(deque & list1, bool g) { for (int i = 0; i < list1.size(); i++) { for (int j = i + 1; j < list1.size(); j++) { if (list1[i].f > list1[j].f ||(list1[i].f == list1[j].f && list1[i].h >list1[j].h&& g == true)) { node temp; temp = list1[i]; list1[i] = list1[j]; list1[j] = temp; } } } } bool canigo(node a, node b) { if ((abs(a.x - b.x) == 0 && abs(a.y - b.y) == 1) || (abs(a.x - b.x) == 1 && abs(a.y - b.y) == 0)) { return true; } else return false; } int main() { int xstart = startx; int ystart = starty; int xend = endx; int yend = endy; int temp = 0; for (int i = 0; i < N; i++) { for (int j = 0; j < M; j++) { nodes[i][j].id = temp; temp++; nodes[i][j].x = i, nodes[i][j].y = j; nodes[i][j].g = abs(xstart - nodes[i][j].x) + abs(ystart - nodes[i][j].y), nodes[i][j].h = abs(xend - nodes[i][j].x) + abs(yend - nodes[i][j].y); nodes[i][j].f = nodes[i][j].g + nodes[i][j].h; nodes[i][j].symbol = ' '; if (i == 0 || j == 0 || i == N - 1 || j == M - 1) { nodes[i][j].symbol = 'O', nodes[i][j].obstacle = true; } if (i == xstart && j == ystart) { nodes[i][j].symbol = 'S'; } if (i == xend && j == yend) { nodes[i][j].symbol = 'E'; } } } //adding obstacles for (int k = 0; k < 13; k++) { nodes[k][3].obstacle = true; nodes[k][3].symbol = 'O'; nodes[k][6].obstacle = true; nodes[k][6].symbol = 'O'; nodes[k + 2][9].obstacle = true; nodes[k + 2][9].symbol = 'O'; nodes[k + 2][16].obstacle = true; nodes[k + 2][16].symbol = 'O'; nodes[k][19].obstacle = true; nodes[k][19].symbol = 'O'; nodes[k + 2][25].obstacle = true; nodes[k + 2][25].symbol = 'O'; } /*for (int i = 0; i < N; i++) { for (int j = 0; j < M; j++) { cout << nodes[i][j].symbol; } cout << "\n"; }*/ deque list1; deque list2; int x = xstart, y = ystart; list2.push_back(nodes[xstart][ystart]); nodes[xstart][ystart].inlist2 = true; if (!nodes[xstart + 1][ystart].obstacle)//down { list1.push_back(nodes[xstart + 1][ystart]); nodes[xstart + 1][ystart].inlist1 = true; } if (!nodes[xstart - 1][ystart].obstacle)//up { list1.push_back(nodes[xstart - 1][ystart]); nodes[xstart - 1][ystart].inlist1 = true; } if (!nodes[xstart][ystart + 1].obstacle)//right { list1.push_back(nodes[xstart][ystart + 1]); nodes[xstart][ystart + 1].inlist1 = true; } if (!nodes[xstart][ystart - 1].obstacle)//left { list1.push_back(nodes[xstart][ystart - 1]); nodes[xstart][ystart - 1].inlist1 = true; } sorting(list1, false); while (!nodes[endx][endy].inlist2) { if (!nodes[list1[0].x + 1][list1[0].y].obstacle && !nodes[list1[0].x + 1][list1[0].y].inlist1 && !nodes[list1[0].x + 1][list1[0].y].inlist2)//down { list1.push_back(nodes[list1[0].x + 1][list1[0].y]); nodes[list1[0].x + 1][list1[0].y].inlist1 = true; } if (!nodes[list1[0].x - 1][list1[0].y].obstacle && !nodes[list1[0].x - 1][list1[0].y].inlist1 && !nodes[list1[0].x - 1][list1[0].y].inlist2)//down { list1.push_back(nodes[list1[0].x - 1][list1[0].y]); nodes[list1[0].x - 1][list1[0].y].inlist1 = true; } if (!nodes[list1[0].x][list1[0].y + 1].obstacle && !nodes[list1[0].x][list1[0].y + 1].inlist1 && !nodes[list1[0].x][list1[0].y + 1].inlist2)//down { list1.push_back(nodes[list1[0].x][list1[0].y + 1]); nodes[list1[0].x][list1[0].y + 1].inlist1 = true; } if (!nodes[list1[0].x][list1[0].y-1].obstacle && !nodes[list1[0].x][list1[0].y - 1].inlist1 && !nodes[list1[0].x][list1[0].y - 1].inlist2)//down { list1.push_back(nodes[list1[0].x][list1[0].y - 1]); nodes[list1[0].x][list1[0].y - 1].inlist1 = true; } list2.push_back(nodes[list1[0].x][list1[0].y]); nodes[list1[0].x][list1[0].y].inlist2 = true; list1.pop_front(); //sorting(list1, true); } sorting(list1, true); int a = xend; int b = yend; for (int i = list2.size() - 1; i > 0; i--) { if (canigo(nodes[a][b], list2[i])) { //nodes[list2[i].x][list2[i].y].symbol = '.'; a = list2[i].x; b = list2[i].y; nodes[a][b].symbol = '.'; } } for (int i = 0; i < N; i++) { for (int j = 0; j < M; j++) { cout << nodes[i][j].symbol; } cout << "\n"; } cout << "Info:\n" << "O=obstacle" << "\n" << ".=path node" << "\n" << "E=End node" << "\n" << "S=Start node"; /*for (int i = 0; i < list1.size(); i++) { cout << list1[i].id; }*/ }

Pathfinding C++