pathfinding.cpp (13329B)
1 #include <ncurses.h> 2 3 #include <algorithm> 4 #include <map> 5 #include <queue> 6 #include <stack> 7 #include <string> 8 #include <vector> 9 10 #include "colors.hpp" 11 #include "pathfinding.hpp" 12 #include "state.hpp" 13 #include "timer.hpp" 14 #include "vec2.hpp" 15 16 std::vector<vec2<int>> pathfinding::Breadth( 17 const bool* field, 18 const int& width, 19 const int& height, 20 const vec2<int>& start, 21 const std::vector<vec2<int>>& exits) 22 { 23 struct node { 24 vec2<int> pos; 25 node* parent = nullptr; 26 std::vector<node*> neighbors; 27 bool isVisited = false; 28 // adds valid neighbors 29 void AddNeighbors( 30 const bool* field, 31 node* grid, 32 const int& width, 33 const int& height) 34 { 35 neighbors.clear(); 36 // up 37 if (pos.y > 2 && field[(pos.y - 1) * width + pos.x]) 38 neighbors.push_back(&grid[(pos.y - 2) * width + pos.x]); 39 // down 40 if (pos.y < height - 2 && field[(pos.y + 1) * width + pos.x]) 41 neighbors.push_back(&grid[(pos.y + 2) * width + pos.x]); 42 // left 43 if (pos.x > 2 && field[(pos.y * width) + pos.x - 1]) 44 neighbors.push_back(&grid[(pos.y * width) + pos.x - 2]); 45 // right 46 if (pos.x < width - 2 && field[(pos.y * width) + pos.x + 1]) 47 neighbors.push_back(&grid[(pos.y * width) + pos.x + 2]); 48 } 49 }; 50 51 // initializing the grid of nodes used for pointer locations 52 node grid[width * height]; 53 for (int y = 0; y < height; ++y) { 54 for (int x = 0; x < width; ++x) { 55 grid[(y * width) + x].pos.x = x; 56 grid[(y * width) + x].pos.y = y; 57 } 58 } 59 60 // initializes the open set with the start pos 61 std::queue<node*> q; 62 q.push(&grid[start.y * width + start.x]); 63 64 node* current = q.front(); 65 node* parent = current; 66 node* middle = nullptr; 67 timer cycle; 68 69 while (!q.empty()) { 70 attron(COLOR_PAIR(green)); 71 s::draw.lock(); 72 mvprintw(start.y, start.x * 2, " "); 73 s::draw.unlock(); 74 attron(COLOR_PAIR(green)); 75 76 current = q.front(); 77 current->isVisited = true; 78 if (current->parent != nullptr) 79 parent = current->parent; 80 for (vec2<int> e : exits) { 81 if (current->pos == e) { 82 std::vector<vec2<int>> path; 83 node* previous = current; 84 do { 85 path.push_back(previous->pos); 86 previous = previous->parent; 87 } while (previous != nullptr); 88 return path; 89 } 90 } 91 s::draw.lock(); 92 attron(COLOR_PAIR(blue)); 93 mvprintw(current->pos.y, current->pos.x * 2, " "); 94 mvprintw(((current->pos + parent->pos) / 2).y, 95 ((current->pos + parent->pos) / 2).x * 2, " "); 96 attroff(COLOR_PAIR(blue)); 97 s::draw.unlock(); 98 q.pop(); 99 100 current->AddNeighbors(field, grid, width, height); 101 102 s::draw.lock(); 103 attron(COLOR_PAIR(cyan)); 104 for (node* n : current->neighbors) { 105 if (!n->isVisited) { 106 n->parent = current; 107 q.push(n); 108 middle = (n + (current - n) / 2); 109 mvprintw(middle->pos.y, middle->pos.x * 2, " "); 110 } 111 } 112 attroff(COLOR_PAIR(cyan)); 113 s::draw.unlock(); 114 if (s::render_pathfinding) { 115 s::draw.lock(); 116 refresh(); 117 s::draw.unlock(); 118 cycle.WaitInterval(s::time_hard_pathfinding); 119 } 120 } 121 return std::vector<vec2<int>>(); 122 } 123 124 std::vector<vec2<int>> pathfinding::Depth( 125 const bool* field, 126 const int& width, 127 const int& height, 128 const vec2<int>& start, 129 const std::vector<vec2<int>>& exits) 130 { 131 struct node { 132 vec2<int> pos; 133 node* parent = nullptr; 134 std::vector<node*> neighbors; 135 bool isVisited = false; 136 // adds valid neighbors 137 void AddNeighbors( 138 const bool* field, 139 node* grid, 140 const int& width, 141 const int& height) 142 { 143 neighbors.clear(); 144 // up 145 if (pos.y > 2 && field[(pos.y - 1) * width + pos.x]) 146 neighbors.push_back(&grid[(pos.y - 2) * width + pos.x]); 147 // down 148 if (pos.y < height - 2 && field[(pos.y + 1) * width + pos.x]) 149 neighbors.push_back(&grid[(pos.y + 2) * width + pos.x]); 150 // left 151 if (pos.x > 2 && field[(pos.y * width) + pos.x - 1]) 152 neighbors.push_back(&grid[(pos.y * width) + pos.x - 2]); 153 // right 154 if (pos.x < width - 2 && field[(pos.y * width) + pos.x + 1]) 155 neighbors.push_back(&grid[(pos.y * width) + pos.x + 2]); 156 } 157 }; 158 159 // initializing the grid of nodes used for pointer locations 160 node grid[width * height]; 161 for (int y = 0; y < height; ++y) { 162 for (int x = 0; x < width; ++x) { 163 grid[(y * width) + x].pos.x = x; 164 grid[(y * width) + x].pos.y = y; 165 } 166 } 167 168 // initializes the open set with the start pos 169 std::stack<node*> q; 170 q.push(&grid[start.y * width + start.x]); 171 172 node* current = q.top(); 173 node* parent; 174 node* wall; 175 node* middle = nullptr; 176 timer cycle; 177 178 while (!q.empty()) { 179 attron(COLOR_PAIR(green)); 180 s::draw.lock(); 181 mvprintw(start.y, start.x * 2, " "); 182 s::draw.unlock(); 183 attron(COLOR_PAIR(green)); 184 current = q.top(); 185 current->isVisited = true; 186 if (current->parent != nullptr) { 187 parent = current->parent; 188 } else { 189 parent = current; 190 } 191 // finding the node pointer containing the 192 // wall between the current and previous nodes 193 // so the path doesn't have gaps when drawn 194 wall = parent + (current - parent) / 2; 195 for (vec2<int> e : exits) { 196 if (current->pos == e) { 197 std::vector<vec2<int>> path; 198 node* previous = current; 199 do { 200 path.push_back(previous->pos); 201 previous = previous->parent; 202 } while (previous != nullptr); 203 return path; 204 } 205 } 206 attron(COLOR_PAIR(blue)); 207 s::draw.lock(); 208 mvprintw(current->pos.y, current->pos.x * 2, " "); 209 mvprintw(wall->pos.y, wall->pos.x * 2, " "); 210 s::draw.unlock(); 211 attroff(COLOR_PAIR(blue)); 212 q.pop(); 213 current->AddNeighbors(field, grid, width, height); 214 215 attron(COLOR_PAIR(cyan)); 216 s::draw.lock(); 217 for (node* n : current->neighbors) { 218 if (!n->isVisited) { 219 n->parent = current; 220 q.push(n); 221 middle = (n + (current - n) / 2); 222 mvprintw(middle->pos.y, middle->pos.x * 2, " "); 223 } 224 } 225 s::draw.unlock(); 226 attroff(COLOR_PAIR(cyan)); 227 if (s::render_pathfinding) { 228 s::draw.lock(); 229 refresh(); 230 s::draw.unlock(); 231 cycle.WaitInterval(s::time_hard_pathfinding); 232 } 233 } 234 return std::vector<vec2<int>>(); 235 } 236 237 std::vector<vec2<int>> pathfinding::AStar( 238 const bool* field, 239 const int& width, 240 const int& height, 241 const vec2<int>& start, 242 const std::vector<vec2<int>>& exits) 243 { 244 struct node { 245 vec2<int> pos; 246 node* parent = nullptr; 247 int g; 248 float h, f; 249 std::vector<node*> neighbors; 250 // adds valid neighbors 251 void AddNeighbors( 252 const bool* field, 253 node* grid, 254 const int& width, 255 const int& height) 256 { 257 neighbors.clear(); 258 // up 259 if (pos.y > 2 && field[(pos.y - 1) * width + pos.x]) 260 neighbors.push_back(&grid[(pos.y - 2) * width + pos.x]); 261 // down 262 if (pos.y < height - 2 && field[(pos.y + 1) * width + pos.x]) 263 neighbors.push_back(&grid[(pos.y + 2) * width + pos.x]); 264 // left 265 if (pos.x > 2 && field[(pos.y * width) + pos.x - 1]) 266 neighbors.push_back(&grid[(pos.y * width) + pos.x - 2]); 267 // right 268 if (pos.x < width - 2 && field[(pos.y * width) + pos.x + 1]) 269 neighbors.push_back(&grid[(pos.y * width) + pos.x + 2]); 270 } 271 }; 272 273 // initializing the grid of nodes used for pointer locations 274 node grid[width * height]; 275 for (int y = 0; y < height; ++y) { 276 for (int x = 0; x < width; ++x) { 277 grid[(y * width) + x].pos.x = x; 278 grid[(y * width) + x].pos.y = y; 279 } 280 } 281 282 // adding the neighbors of all the cells 283 for (int y = 0; y < height; ++y) { 284 for (int x = 0; x < width; ++x) { 285 grid[(y * width) + x].AddNeighbors(field, grid, width, height); 286 } 287 } 288 289 int index; 290 node* current = nullptr; 291 std::vector<vec2<int>> path; 292 std::vector<node*> neighbors; 293 int g_temp; 294 node* neighbor = nullptr; 295 node* middle = nullptr; 296 vec2<int> finish; 297 timer cycle; 298 299 std::vector<node*> openSet; 300 std::vector<node*> closedSet; 301 openSet.push_back(&grid[start.y * width + start.x]); 302 current = openSet.front(); 303 current->parent = nullptr; 304 current->g = 0.0f; 305 finish = exits[0]; 306 for (int i = 1; i < exits.size(); ++i) { 307 if ((exits[i] - current->pos).GetMagnitude() 308 < (finish - current->pos).GetMagnitude()) { 309 finish = exits[i]; 310 } 311 } 312 current->h = (current->pos - finish).GetMagnitude(); 313 current->f = current->g + current->h; 314 315 while (!openSet.empty()) { 316 index = 0; 317 for (int i = 1; i < openSet.size(); ++i) { 318 if (openSet[i]->f < openSet[index]->f) 319 index = i; 320 } 321 current = openSet[index]; 322 323 for (vec2<int> e : exits) { 324 if (current->pos == e) { 325 node* previous = current; 326 do { 327 path.push_back(previous->pos); 328 previous = previous->parent; 329 } while (previous != nullptr); 330 return path; 331 } 332 } 333 334 closedSet.push_back(current); 335 openSet.erase(openSet.begin() + index); 336 neighbors = current->neighbors; 337 if (current->parent != nullptr) { 338 middle = current + (current->parent - current) / 2; 339 } else { 340 middle = current; 341 } 342 s::draw.lock(); 343 attron(COLOR_PAIR(blue)); 344 mvprintw(current->pos.y, current->pos.x * 2, " "); 345 mvprintw(middle->pos.y, middle->pos.x * 2, " "); 346 attroff(COLOR_PAIR(blue)); 347 s::draw.unlock(); 348 if (current->pos == start) { 349 s::draw.lock(); 350 attron(COLOR_PAIR(green)); 351 mvprintw(start.y, start.x * 2, " "); 352 attroff(COLOR_PAIR(green)); 353 s::draw.unlock(); 354 } 355 356 for (int i = 0; i < neighbors.size(); ++i) { 357 neighbor = neighbors[i]; 358 // checks if neighbor is in the closed set 359 // if not, it processes it 360 if ((std::find(closedSet.begin(), closedSet.end(), neighbor) 361 == closedSet.end())) { 362 g_temp = current->g + 1; 363 364 // checks if the neighbor is already in the open set 365 // if not, it adds it 366 if ((std::find(openSet.begin(), openSet.end(), neighbor) 367 == openSet.end())) { 368 openSet.push_back(neighbor); 369 s::draw.lock(); 370 middle = (neighbor + (current - neighbor) / 2); 371 attron(COLOR_PAIR(cyan)); 372 mvprintw(middle->pos.y, middle->pos.x * 2, " "); 373 attroff(COLOR_PAIR(cyan)); 374 s::draw.unlock(); 375 } else if (g_temp >= neighbor->g) { 376 continue; 377 } 378 379 finish = exits[0]; 380 for (int i = 1; i < exits.size(); ++i) { 381 if ((exits[i] - neighbor->pos).GetMagnitude() 382 < (finish - neighbor->pos).GetMagnitude()) { 383 finish = exits[i]; 384 } 385 } 386 387 neighbor->g = g_temp; 388 neighbor->h = (neighbor->pos - finish).GetMagnitude(); 389 neighbor->f = neighbor->h + neighbor->g; 390 neighbor->parent = current; 391 } 392 if (s::render_pathfinding) { 393 s::draw.lock(); 394 refresh(); 395 s::draw.unlock(); 396 cycle.WaitInterval(s::time_hard_pathfinding); 397 } 398 } 399 } 400 return path; 401 } 402 403 std::map<std::string, 404 std::vector<vec2<int>> (*)( 405 const bool* field, 406 const int& width, 407 const int& height, 408 const vec2<int>& start, 409 const std::vector<vec2<int>>& exits)> 410 pathfinders = { 411 { "Breadth", pathfinding::Breadth }, 412 { "Dijkstra", pathfinding::Breadth }, 413 { "Depth", pathfinding::Depth }, 414 { "A*", pathfinding::AStar } 415 };