├── LICENSE ├── README.md ├── backups ├── pathfinding_copy.c ├── pathfinding_copy2.c ├── pathfinding_copy3.c ├── pathfinding_final.c └── testing.c ├── file.c ├── funcDefns.h ├── grid.c ├── node.c ├── path.c ├── release-v1.0 ├── c-pathfinding.exe ├── maze.txt └── sampleMaze2.txt └── structs.c /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # A* Pathfinding in C (command line) 2 | 3 | Recreation of my A* pathfinding algorithm except this time in C! Instead of a graphics interface this is meant to run on the command line and analyze a maze given by a .txt file. '#' characters are walls, 'O' characters are open, 'S' is the start and 'G' is the end. You can make any maze which is rectangular and this program will create a new file in the same directory called "nameSolution.txt". I included two test mazes named "maze.txt" and "sampleMaze2.txt". 4 | 5 | I also time the performance to show the efficiency of my algorithm :) 6 | 7 | ### How to run: 8 | - open command line 9 | - go to directory of executable (cd) 10 | - Make sure the maze file is in the same directory as the executable 11 | - type "c-pathfinding maze.txt" to run and analyze "maze.txt" file 12 | - you may analyze any file by typing name of executable SPACE name of mazefile.txt (or directory + name) 13 | 14 | ![capture](https://cloud.githubusercontent.com/assets/25334129/22401330/d81b8fee-e59c-11e6-921c-477ee9362644.PNG) 15 | 16 | Note: If you try to run executable without command line, it will close automatically because no file argument is given 17 | 18 | *a project by Devon Crawford.* 19 | -------------------------------------------------------------------------------- /backups/pathfinding_copy.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #define PI_2 3.14159/2 6 | 7 | // Creating GridSize struct 8 | typedef struct GridSize { 9 | int width; 10 | int height; 11 | } GridSize; 12 | 13 | // Creating Node struct 14 | typedef struct Node { 15 | struct Node* parent; 16 | int exists; 17 | int x; 18 | int y; 19 | int g; 20 | int h; 21 | int f; 22 | int id; 23 | } Node; 24 | 25 | GridSize size; 26 | GridSize getGridSize(FILE *file); 27 | int readFile(FILE *file, char grid[][size.width]); 28 | void initString(char string[]); 29 | void appendName(char origName[], char insert[], char newName[]); 30 | void writeFile(char grid[][size.width], char origName[]); 31 | void clearGrid(char grid[][size.width]); 32 | void printGrid(char grid[][size.width]); 33 | void copyGrid(char copy[][size.width], char grid[][size.width]); 34 | 35 | void clearNode(Node *node); 36 | void initNodes(Node node[][size.width]); 37 | void nodesExistence(Node node[][size.width]); 38 | Node getNodeChar(char grid[][size.width], char toFind); 39 | Node getNode(Node[][size.width], int openLength); 40 | void addListNode(Node list[][size.width], int* length, Node node); 41 | void removeListNode(Node list[][size.width], int *length, Node node); 42 | void arrangeID(Node arrange[], Node original[][size.width], int length); 43 | Node lowestF(Node open[][size.width], int length); 44 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength); 45 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength); 46 | void connectPath(Node path[][size.width], int *pathLength, Node parent); 47 | void reverseID(Node path[][size.width], int pathLength); 48 | void insertPath(char grid[][size.width], Node path[][size.width]); 49 | 50 | void info(Node node); 51 | void info1D(Node nodes[], int length); 52 | void info2D(Node nodes[][size.width], int length); 53 | 54 | int main(int argc, char* argv[]) { 55 | // Attempts to open file 56 | FILE *file = fopen(argv[1], "r"); 57 | int gridError = 0; 58 | 59 | // If file exists 60 | if(file != NULL) { 61 | // Getting the size of the grid from file 62 | size = getGridSize(file); 63 | 64 | // Declaring grid arrays based on pre calculated size 65 | char grid[size.height][size.width]; 66 | char solvedGrid[size.height][size.width]; 67 | 68 | // Initializes grid 69 | clearGrid(grid); 70 | 71 | // Reads file into grid and detects any errors 72 | gridError = readFile(file, grid); 73 | 74 | // If there is no error reading grid from file 75 | if(gridError == 0) { 76 | // Makes a copy of the grid for the solution 77 | copyGrid(solvedGrid, grid); 78 | 79 | // Prints the original map 80 | printf("=== Original Grid ===\n"); 81 | printGrid(grid); 82 | 83 | // Declaring Node arrays 84 | Node open[size.height][size.width]; 85 | Node closed[size.height][size.width]; 86 | Node path[size.height][size.width]; 87 | 88 | // Initializing Node arrays 89 | initNodes(open); 90 | initNodes(closed); 91 | initNodes(path); 92 | 93 | // Declaring and initializing lengths for Node arrays 94 | int openLength = 0; 95 | int closedLength = 0; 96 | int pathLength = 0; 97 | 98 | // Gets the node of start and goal 99 | Node start = getNodeChar(grid, 'S'); 100 | Node goal = getNodeChar(grid, 'G'); 101 | 102 | // If start node was not found 103 | if(start.exists == 0) { 104 | printf("Error: Start node was not found.\n"); 105 | } 106 | // If goal node was not found 107 | if(goal.exists == 0) { 108 | printf("Error: Goal node was not found.\n"); 109 | } 110 | 111 | // If start and goal nodes both exist 112 | if(start.exists == 1 && goal.exists == 1) { 113 | // Add start node to the closed list 114 | addListNode(closed, &closedLength, start); 115 | 116 | findPath(grid, open, closed, path, start, goal, start, &openLength, &closedLength, &pathLength); 117 | insertPath(solvedGrid, path); 118 | 119 | printf("\nSOLVED:..\n"); 120 | printGrid(solvedGrid); 121 | 122 | writeFile(solvedGrid, argv[1]); 123 | } 124 | } 125 | } 126 | // If file has any errors 127 | else { 128 | if(argc == 1) { 129 | printf("Error: No file was entered in command line\n"); 130 | } 131 | else { 132 | printf("Error: \"%s\" does not exist.\n", argv[1]); 133 | } 134 | } 135 | return 0; 136 | } 137 | 138 | GridSize getGridSize(FILE *file) { 139 | char c = '\0'; 140 | int x, y = 0; 141 | GridSize size; 142 | 143 | if(file != NULL) { 144 | // Gets the length of the first line 145 | for(x = 0; (c = fgetc(file)) && (c != '\n' && !feof(file)); x++); 146 | 147 | // Resets file pointer to start of file 148 | rewind(file); 149 | 150 | // Gets the number of lines in the file 151 | while((c = fgetc(file)) && !feof(file)) { 152 | if(c == '\n') { 153 | y++; 154 | } 155 | } 156 | 157 | // Adds one to the line count 158 | if(x > 0) { 159 | y++; 160 | } 161 | } 162 | 163 | size.width = x; 164 | size.height = y; 165 | return size; 166 | } 167 | 168 | // Reads grid from file 169 | // Returns 0 if correct and 1 if grid is incomplete 170 | int readFile(FILE *file, char grid[][size.width]) { 171 | char c = '\0'; 172 | int h, w; 173 | 174 | rewind(file); 175 | for(h = 0; h < size.height; h++) { 176 | for(w = 0; (c = fgetc(file)) && w < size.width && !feof(file); w++) { 177 | // Detects if the grid is incomplete, prints location and returns 1 (error) 178 | if(c == '\n' || (c != '#' && c != 'O' && c != 'S' && c != 'G')) { 179 | printf("Error: Grid data is incomplete at x: %d, y: %d\n", w + 1, h + 1); 180 | return 1; 181 | } 182 | // Saves file character to grid 183 | grid[h][w] = c; 184 | } 185 | } 186 | return 0; 187 | } 188 | 189 | // Initializes each character in string to null terminator 190 | void initString(char string[]) { 191 | int w; 192 | int length = strlen(string); 193 | for(w = 0; w < length; w++) { 194 | string[w] = '\0'; 195 | } 196 | } 197 | 198 | // Adds string to the end of origName, while keeping extension intact 199 | void appendName(char origName[], char insert[], char newName[]) { 200 | // Declaring lengths of strings 201 | int nameLen = strlen(origName); 202 | int insertLen = strlen(insert); 203 | int newNameLen = nameLen + insertLen; 204 | 205 | // Declaring index counters 206 | int ri = -1; 207 | int ei = -1; 208 | int c; 209 | 210 | // Initializing every character in newName string to '\0' 211 | initString(newName); 212 | 213 | // Gets the location to replace (finds start of extension) 214 | for(ri = nameLen; ri >= 0 && origName[ri] != '.'; ri--); 215 | 216 | // If file does not have an extension 217 | if(ri == 0) { 218 | // Replace index is very end of string 219 | ri = nameLen - 1; 220 | } 221 | // extension index starts at replace index 222 | ei = ri; 223 | 224 | // Pieces newName together 225 | for(c = 0; c < newNameLen; c++) { 226 | // Saves newName as original from start up till replace index 227 | if(c < ri) { 228 | newName[c] = origName[c]; 229 | } 230 | // Appends insert to newName 231 | else if(c < ri + insertLen) { 232 | newName[c] = insert[c - ri]; 233 | } 234 | // Appends extension from origName to newName 235 | else if(c < newNameLen) { 236 | newName[c] = origName[ei]; 237 | ei++; 238 | } 239 | } 240 | // Adds null terminator to end of string 241 | newName[c] = '\0'; 242 | } 243 | 244 | void writeFile(char grid[][size.width], char origName[]) { 245 | int h, w; 246 | FILE *file; 247 | 248 | // Declaring strings for new file name 249 | char insert[] = "solution"; 250 | char newName[strlen(origName) + strlen(insert)]; 251 | 252 | // Adds solution onto origName, keeping extension intact 253 | appendName(origName, insert, newName); 254 | 255 | // Opens file with new name for writing 256 | file = fopen(newName, "w"); 257 | 258 | // Writes each character of solved grid to new file 259 | for(h = 0; h < size.height; h++) { 260 | for(w = 0; w < size.width; w++) { 261 | fputc(grid[h][w], file); 262 | } 263 | fputc('\n', file); 264 | } 265 | // Close file 266 | fclose(file); 267 | } 268 | 269 | void clearGrid(char grid[][size.width]) { 270 | int h, w; 271 | 272 | for(h = 0; h < size.height; h++) { 273 | for(w = 0; w < size.width; w++) { 274 | grid[h][w] = '0'; 275 | } 276 | } 277 | } 278 | 279 | void printGrid(char grid[][size.width]) { 280 | int h, w; 281 | 282 | for(h = 0; h < size.height; h++) { 283 | for(w = 0; w < size.width; w++) { 284 | printf("%c", grid[h][w]); 285 | } 286 | printf("\n"); 287 | } 288 | } 289 | 290 | void copyGrid(char copy[][size.width], char grid[][size.width]) { 291 | int h, w; 292 | 293 | for(h = 0; h < size.height; h++) { 294 | for(w = 0; w < size.width; w++) { 295 | copy[h][w] = grid[h][w]; 296 | } 297 | } 298 | } 299 | 300 | // Clears the node to default values 301 | void clearNode(Node *node) { 302 | node->parent = NULL; 303 | node->exists = 0; 304 | node->id = -1; 305 | node->f = -1; 306 | node->g = -1; 307 | node->h = -1; 308 | } 309 | 310 | // Initializes all nodes to default values 311 | void initNodes(Node node[][size.width]) { 312 | int h, w; 313 | for(h = 0; h < size.height; h++) { 314 | for(w = 0; w < size.width; w++) { 315 | clearNode(&node[h][w]); 316 | node[h][w].x = w; 317 | node[h][w].y = h; 318 | } 319 | } 320 | } 321 | 322 | // Prints 1 if Node at that position exists 323 | // Prints 0 if Node at that position does not exist 324 | void nodesExistence(Node node[][size.width]) { 325 | int h, w; 326 | 327 | for(h = 0; h < size.height; h++) { 328 | for(w = 0; w < size.width; w++) { 329 | printf("%d", node[h][w].exists); 330 | } 331 | printf("\n"); 332 | } 333 | } 334 | 335 | // Gets a node (with position) based upon the character on the grid 336 | // Used for finding the start Node and goal Node 337 | Node getNodeChar(char grid[][size.width], char toFind) { 338 | int h, w; 339 | Node node; 340 | clearNode(&node); 341 | 342 | for(h = 0; h < size.width; h++) { 343 | for(w = 0; w < size.width; w++) { 344 | // If char 'toFind' is found, save the position and return node 345 | if(grid[h][w] == toFind) { 346 | node.x = w; 347 | node.y = h; 348 | node.exists = 1; 349 | return node; 350 | } 351 | } 352 | } 353 | return node; 354 | } 355 | 356 | // Finds node with specific id number index and returns it 357 | Node getNode(Node list[][size.width], int findIndex) { 358 | int h, w; 359 | 360 | for(h = 0; h < size.height; h++) { 361 | for(w = 0; w < size.width; w++) { 362 | if(list[h][w].id == findIndex) { 363 | return list[h][w]; 364 | } 365 | } 366 | } 367 | Node fail; 368 | fail.exists = 0; 369 | fail.id = -1; 370 | return fail; 371 | } 372 | 373 | // Copies data from Node orig into parent of Node copy 374 | void setParent(Node *copy, Node orig) { 375 | if(copy->parent == NULL) { 376 | copy->parent = (Node*) malloc(sizeof(Node)); 377 | } 378 | copy->parent->parent = orig.parent; 379 | copy->parent->exists = orig.exists; 380 | copy->parent->x = orig.x; 381 | copy->parent->y = orig.y; 382 | copy->parent->g = orig.g; 383 | copy->parent->h = orig.h; 384 | copy->parent->f = orig.f; 385 | copy->parent->id = orig.id; 386 | } 387 | 388 | void addListNode(Node list[][size.width], int* length, Node node) { 389 | list[node.y][node.x] = node; 390 | list[node.y][node.x].exists = 1; 391 | list[node.y][node.x].id = (*length); 392 | (*length)++; 393 | } 394 | 395 | void removeListNode(Node list[][size.width], int *length, Node node) { 396 | int h, w; 397 | 398 | // If node exists 399 | if(node.exists == 1) { 400 | // If node is not the last in list 401 | if(node.id != (*length)-1) { 402 | // Iterates through every node in list 403 | for(h = 0; h < size.height; h++) { 404 | for(w = 0; w < size.width; w++) { 405 | // If node has id greater than one being removed 406 | if(list[h][w].id > node.id) { 407 | // Decrement id 408 | list[h][w].id--; 409 | } 410 | } 411 | } 412 | } 413 | // Clear values on the node 414 | clearNode(&list[node.y][node.x]); 415 | 416 | // Free parent from memory 417 | free(list[node.y][node.x].parent); 418 | 419 | // Decrement length 420 | (*length)--; 421 | } 422 | } 423 | 424 | // Arranges original nodes with a valid id number 425 | // from low to high, into 1D array to be sorted 426 | void arrangeID(Node arrange[], Node original[][size.width], int length) { 427 | int a, h, w; 428 | 429 | for(a = 0; a < length; a++) { 430 | for(h = 0; h < size.height; h++) { 431 | for(w = 0; w < size.width; w++) { 432 | if(original[h][w].id == a) { 433 | arrange[a] = original[h][w]; 434 | break; 435 | } 436 | } 437 | } 438 | } 439 | } 440 | 441 | // Returns node with lowest F value 442 | Node lowestF(Node open[][size.width], int length) { 443 | // Copies open nodes to new 1D array for sorting 444 | Node sort[length]; 445 | arrangeID(sort, open, length); 446 | 447 | int Switch = -1; 448 | Node curr, next; 449 | int i; 450 | 451 | // Sorts open array from low to high 452 | while(Switch != 0) { 453 | Switch = 0; 454 | 455 | // Iterates each existing open node (length) 456 | for(i = 0; i < length - 1; i++) { 457 | curr = sort[i]; 458 | next = sort[i + 1]; 459 | 460 | // SWAP: if curr Node has greater f than next Node 461 | if(curr.f > next.f) { 462 | sort[i] = next; 463 | sort[i + 1] = curr; 464 | Switch = 1; 465 | } 466 | } 467 | } 468 | return sort[0]; 469 | } 470 | 471 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength) { 472 | int i; 473 | // Checks all directions (90 degrees) 474 | for(i = 0; i < 4; i++) { 475 | // Uses cosine and sine functions to get circle of points (at 90 degree intervals) around parent 476 | int possibleX = parent.x + round(-1 * cos(PI_2 * i)); 477 | int possibleY = parent.y + round(-1 * sin(PI_2 * i)); 478 | 479 | // If coordinates are outside of boundaries 480 | if(possibleX < 0 || possibleY < 0 || possibleX > (size.width - 1) || possibleY > (size.height - 1)) { 481 | continue; 482 | } 483 | 484 | // Does not make a new open node IF 485 | // Node is a border, node is already in the open list 486 | // Or the node is already in the closed list 487 | if(grid[possibleY][possibleX] == '#' || closed[possibleY][possibleX].exists == 1 || open[possibleY][possibleX].exists == 1) { 488 | continue; 489 | } 490 | // Create an open node, calculate the values and add it to the open list 491 | calculateNodeValues(parent, goal, possibleX, possibleY, grid, open, openLength); 492 | } 493 | // Sets new parent as openNode with the lowest F cost 494 | parent = lowestF(open, *openLength); 495 | 496 | // If chosen parent does not exist.. 497 | // Must be no path, exhausted entire map 498 | if(parent.exists != 1) { 499 | // Inform user that no path was found 500 | printf("No path found..\n"); 501 | 502 | // Breaks out of entire pathfinding functions 503 | return; 504 | } 505 | // If parent node is the goal! 506 | else if(grid[parent.y][parent.x] == 'G') { 507 | // Get the last node in the closed list 508 | Node lastClosed = getNode(closed, (*closedLength)-1); 509 | 510 | // Connect path by tracing back parents, starting at lastClosed Node 511 | connectPath(path, pathLength, lastClosed); 512 | return; 513 | } 514 | 515 | // Remove parent from open list 516 | removeListNode(open, openLength, parent); 517 | 518 | // Adds parent to closed list 519 | addListNode(closed, closedLength, parent); 520 | 521 | // Recursive call to function, replacing parent as the open node with lowest fCost 522 | findPath(grid, open, closed, path, start, goal, parent, openLength, closedLength, pathLength); 523 | } 524 | 525 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength) { 526 | // Creates a new open node (to be looked at) 527 | Node openNode; 528 | clearNode(&openNode); 529 | 530 | // Declaring and initalizing variables used for calculations 531 | int gCost = 0; 532 | int hxDiff = 0; 533 | int hyDiff = 0; 534 | int hCost = 0; 535 | int fCost = 0; 536 | 537 | // Sets coordinates of the open node 538 | openNode.x = possibleX; 539 | openNode.y = possibleY; 540 | 541 | // Sets the parent of the open node 542 | setParent(&openNode, parent); 543 | 544 | // Calculating G cost 545 | // Cost to move from parent node to one open node 546 | gCost = parent.g + 1; 547 | openNode.g = gCost; 548 | 549 | // Calculating H cost 550 | // Estimated cost to move from the open node to the goal node 551 | hxDiff = fabs(goal.x - openNode.x); 552 | hyDiff = fabs(goal.y - openNode.y); 553 | hCost = hxDiff + hyDiff; 554 | openNode.h = hCost; 555 | 556 | // Calculating F cost (combined G and H) 557 | fCost = gCost + hCost; 558 | openNode.f = fCost; 559 | 560 | // Add the open node to the open list 561 | addListNode(open, openLength, openNode); 562 | } 563 | 564 | void connectPath(Node path[][size.width], int *pathLength, Node parent) { 565 | // Parent pointers are followed from goal until it reaches start 566 | while(parent.parent != NULL) { 567 | // Adds parent to the path list 568 | addListNode(path, pathLength, parent); 569 | 570 | // Parent Node is overwritten by parent Node of itself 571 | parent = *(parent.parent); 572 | } 573 | // Flips id's in path because they were recorded in reverse 574 | reverseID(path, *pathLength); 575 | } 576 | 577 | void reverseID(Node path[][size.width], int pathLength) { 578 | // s starts at 0, e starts at last index in path 579 | int s, e = pathLength - 1; 580 | Node start, end; 581 | 582 | // s increments while e decrements 583 | for(s = 0; s < e; s++) { 584 | // Gets node at id s and e 585 | start = getNode(path, s); 586 | end = getNode(path, e); 587 | 588 | // Swaps id's of nodes start and end 589 | path[start.y][start.x].id = e; 590 | path[end.y][end.x].id = s; 591 | e--; 592 | } 593 | } 594 | 595 | void insertPath(char grid[][size.width], Node path[][size.width]) { 596 | int h, w; 597 | 598 | for(h = 0; h < size.height; h++) { 599 | for(w = 0; w < size.width; w++) { 600 | if(path[h][w].exists == 1) { 601 | // Replaces a path location with a space 602 | grid[h][w] = ' '; 603 | } 604 | } 605 | } 606 | } 607 | 608 | // Prints information about one node 609 | void info(Node node) { 610 | if(node.parent != NULL) { 611 | printf("info.. %d, %d. f: %d. id: %d. E: %d ", node.x, node.y, node.f, node.id, node.exists); 612 | printf("//// Parent: %d, %d. f: %d. id: %d. E: %d\n", node.parent->x, node.parent->y, node.parent->f, node.parent->id, node.parent->exists); 613 | } 614 | else { 615 | printf("info.. %d, %d. f: %d. id: %d. E: %d //// Parent = NULL\n", node.x, node.y, node.f, node.id, node.exists); 616 | } 617 | } 618 | 619 | // Prints information of a one-dimensional array of nodes 620 | void info1D(Node nodes[], int length) { 621 | int h; 622 | for(h = 0; h < length; h++) { 623 | if(nodes[h].exists == 1) { 624 | info(nodes[h]); 625 | } 626 | } 627 | } 628 | 629 | // Prints information of a two-dimensional (grid) array of nodes 630 | void info2D(Node nodes[][size.width], int length) { 631 | int a, h, w; 632 | 633 | for(a = 0; a < length; a++) { 634 | for(h = 0; h < size.height; h++) { 635 | for(w = 0; w < size.width; w++) { 636 | if(nodes[h][w].id == a) { 637 | info(nodes[h][w]); 638 | break; 639 | } 640 | } 641 | } 642 | } 643 | } 644 | -------------------------------------------------------------------------------- /backups/pathfinding_copy2.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #define PI_2 3.14159/2 6 | 7 | // Creating GridSize struct 8 | typedef struct GridSize { 9 | int width; 10 | int height; 11 | } GridSize; 12 | 13 | typedef struct Node { 14 | struct Node* parent; 15 | int exists; 16 | int x; 17 | int y; 18 | int g; 19 | int h; 20 | int f; 21 | int id; 22 | } Node; 23 | 24 | GridSize size; 25 | GridSize getGridSize(FILE *file); 26 | int readFile(FILE *file, char grid[][size.width]); 27 | void writeFile(char grid[][size.width], char origName[]); 28 | 29 | void addListNode(Node list[][size.width], int* length, Node node); 30 | void removeListNode(Node list[][size.width], int *length, Node node); 31 | void clearGrid(char grid[][size.width]); 32 | void printGrid(char grid[][size.width]); 33 | void copyGrid(char copy[][size.width], char grid[][size.width]); 34 | void initNodes(Node node[][size.width]); 35 | void clearNode(Node *node); 36 | void nodesExistence(Node node[][size.width]); 37 | Node getNodeChar(char grid[][size.width], char toFind); 38 | Node getNode(Node[][size.width], int openLength); 39 | void arrangeID(Node arrange[], Node original[][size.width], int length); 40 | void insertPath(char grid[][size.width], Node path[][size.width]); 41 | 42 | void info(Node node); 43 | void info1D(Node nodes[], int length); 44 | void info2D(Node nodes[][size.width], int length); 45 | 46 | Node lowestF(Node open[][size.width], int length); 47 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength); 48 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength); 49 | void connectPath(Node path[][size.width], int *pathLength, Node parent); 50 | void reverseID(Node path[][size.width], int pathLength); 51 | 52 | int main(int argc, char* argv[]) { 53 | 54 | FILE *file = fopen(argv[1], "r"); 55 | int gridError = 0; 56 | 57 | if(file != NULL) { 58 | size = getGridSize(file); 59 | 60 | char grid[size.height][size.width]; 61 | char solvedGrid[size.height][size.width]; 62 | 63 | clearGrid(grid); 64 | 65 | gridError = readFile(file, grid); 66 | 67 | // If there is no error reading grid from file 68 | if(gridError == 0) { 69 | // Makes a copy of the grid for the solution 70 | copyGrid(solvedGrid, grid); 71 | 72 | // Prints the original map 73 | printGrid(grid); 74 | 75 | // Declaring Node arrays 76 | Node open[size.height][size.width]; 77 | Node closed[size.height][size.width]; 78 | Node path[size.height][size.width]; 79 | 80 | // Initializing Node arrays 81 | initNodes(open); 82 | initNodes(closed); 83 | initNodes(path); 84 | 85 | int openLength = 0; 86 | int closedLength = 0; 87 | int pathLength = 0; 88 | 89 | Node start = getNodeChar(grid, 'S'); 90 | Node goal = getNodeChar(grid, 'G'); 91 | 92 | // If start node was not found 93 | if(start.exists == 0) { 94 | printf("Error: Start node was not found.\n"); 95 | } 96 | // If goal node was not found 97 | if(goal.exists == 0) { 98 | printf("Error: Goal node was not found.\n"); 99 | } 100 | 101 | // If start and goal nodes both exist 102 | if(start.exists == 1 && goal.exists == 1) { 103 | // Add start node to the closed list 104 | addListNode(closed, &closedLength, start); 105 | 106 | findPath(grid, open, closed, path, start, goal, start, &openLength, &closedLength, &pathLength); 107 | insertPath(solvedGrid, path); 108 | 109 | nodesExistence(path); 110 | info2D(path, pathLength); 111 | printf("SOLVED:..\n"); 112 | printGrid(solvedGrid); 113 | 114 | writeFile(solvedGrid, argv[1]); 115 | } 116 | } 117 | } 118 | else { 119 | printf("Error: \"%s\" does not exist.\n", argv[1]); 120 | } 121 | 122 | return 0; 123 | } 124 | 125 | void addListNode(Node list[][size.width], int* length, Node node) { 126 | list[node.y][node.x] = node; 127 | list[node.y][node.x].exists = 1; 128 | list[node.y][node.x].id = (*length); 129 | (*length)++; 130 | } 131 | 132 | void removeListNode(Node list[][size.width], int *length, Node node) { 133 | int h, w; 134 | 135 | // If node exists 136 | if(node.exists == 1) { 137 | // If node is not the last in list 138 | if(node.id != (*length)-1) { 139 | // Iterates through every node in list 140 | for(h = 0; h < size.height; h++) { 141 | for(w = 0; w < size.width; w++) { 142 | // If node has id greater than one being removed 143 | if(list[h][w].id > node.id) { 144 | // Decrement id 145 | list[h][w].id--; 146 | } 147 | } 148 | } 149 | } 150 | // Clear values on the node 151 | clearNode(&list[node.y][node.x]); 152 | 153 | // Free parent from memory 154 | free(list[node.y][node.x].parent); 155 | 156 | // Decrement length 157 | (*length)--; 158 | } 159 | } 160 | 161 | void clearGrid(char grid[][size.width]) { 162 | int h, w; 163 | 164 | for(h = 0; h < size.height; h++) { 165 | for(w = 0; w < size.width; w++) { 166 | grid[h][w] = '0'; 167 | } 168 | } 169 | } 170 | 171 | void printGrid(char grid[][size.width]) { 172 | int h, w; 173 | 174 | for(h = 0; h < size.height; h++) { 175 | for(w = 0; w < size.width; w++) { 176 | printf("%c", grid[h][w]); 177 | } 178 | printf("\n"); 179 | } 180 | } 181 | 182 | void copyGrid(char copy[][size.width], char grid[][size.width]) { 183 | int h, w; 184 | 185 | for(h = 0; h < size.height; h++) { 186 | for(w = 0; w < size.width; w++) { 187 | copy[h][w] = grid[h][w]; 188 | } 189 | } 190 | } 191 | 192 | void insertPath(char grid[][size.width], Node path[][size.width]) { 193 | int h, w; 194 | 195 | for(h = 0; h < size.height; h++) { 196 | for(w = 0; w < size.width; w++) { 197 | if(path[h][w].exists == 1) { 198 | // Replaces a path location with a space 199 | grid[h][w] = ' '; 200 | } 201 | } 202 | } 203 | } 204 | 205 | // Initializes all nodes to default values 206 | void initNodes(Node node[][size.width]) { 207 | int h, w; 208 | for(h = 0; h < size.height; h++) { 209 | for(w = 0; w < size.width; w++) { 210 | clearNode(&node[h][w]); 211 | node[h][w].x = w; 212 | node[h][w].y = h; 213 | } 214 | } 215 | } 216 | 217 | // Clears the node to default values 218 | void clearNode(Node *node) { 219 | node->parent = NULL; 220 | node->exists = 0; 221 | node->id = -1; 222 | node->f = -1; 223 | node->g = -1; 224 | node->h = -1; 225 | } 226 | 227 | // Prints 1 if Node at that position exists 228 | // Prints 0 if Node at that position does not exist 229 | void nodesExistence(Node node[][size.width]) { 230 | int h, w; 231 | 232 | for(h = 0; h < size.height; h++) { 233 | for(w = 0; w < size.width; w++) { 234 | printf("%d", node[h][w].exists); 235 | } 236 | printf("\n"); 237 | } 238 | } 239 | 240 | // Gets a node (with position) based upon the character on the grid 241 | // Used for finding the start Node and goal Node 242 | Node getNodeChar(char grid[][size.width], char toFind) { 243 | int h, w; 244 | Node node; 245 | clearNode(&node); 246 | 247 | for(h = 0; h < size.width; h++) { 248 | for(w = 0; w < size.width; w++) { 249 | // If char 'toFind' is found, save the position and return node 250 | if(grid[h][w] == toFind) { 251 | node.x = w; 252 | node.y = h; 253 | node.exists = 1; 254 | return node; 255 | } 256 | } 257 | } 258 | return node; 259 | } 260 | 261 | // Finds node with specific id number index and returns it 262 | Node getNode(Node list[][size.width], int findIndex) { 263 | int h, w; 264 | 265 | for(h = 0; h < size.height; h++) { 266 | for(w = 0; w < size.width; w++) { 267 | if(list[h][w].id == findIndex) { 268 | return list[h][w]; 269 | } 270 | } 271 | } 272 | Node fail; 273 | fail.exists = 0; 274 | fail.id = -1; 275 | return fail; 276 | } 277 | 278 | // Arranges original nodes with a valid id number 279 | // from low to high, into 1D array to be sorted 280 | void arrangeID(Node arrange[], Node original[][size.width], int length) { 281 | int a, h, w; 282 | 283 | for(a = 0; a < length; a++) { 284 | for(h = 0; h < size.height; h++) { 285 | for(w = 0; w < size.width; w++) { 286 | if(original[h][w].id == a) { 287 | arrange[a] = original[h][w]; 288 | break; 289 | } 290 | } 291 | } 292 | } 293 | } 294 | 295 | // Prints information about one node 296 | void info(Node node) { 297 | if(node.parent != NULL) { 298 | printf("info.. %d, %d. f: %d. id: %d. E: %d ", node.x, node.y, node.f, node.id, node.exists); 299 | printf("//// Parent: %d, %d. f: %d. id: %d. E: %d\n", node.parent->x, node.parent->y, node.parent->f, node.parent->id, node.parent->exists); 300 | } 301 | else { 302 | printf("info.. %d, %d. f: %d. id: %d. E: %d //// Parent = NULL\n", node.x, node.y, node.f, node.id, node.exists); 303 | } 304 | } 305 | 306 | // Prints information of a one-dimensional array of nodes 307 | void info1D(Node nodes[], int length) { 308 | int h; 309 | for(h = 0; h < length; h++) { 310 | if(nodes[h].exists == 1) { 311 | info(nodes[h]); 312 | } 313 | } 314 | } 315 | 316 | // Prints information of a two-dimensional (grid) array of nodes 317 | void info2D(Node nodes[][size.width], int length) { 318 | int a, h, w; 319 | 320 | for(a = 0; a < length; a++) { 321 | for(h = 0; h < size.height; h++) { 322 | for(w = 0; w < size.width; w++) { 323 | if(nodes[h][w].id == a) { 324 | info(nodes[h][w]); 325 | break; 326 | } 327 | } 328 | } 329 | } 330 | } 331 | 332 | // Copies data from Node orig into parent of Node copy 333 | void setParent(Node *copy, Node orig) { 334 | if(copy->parent == NULL) { 335 | copy->parent = (Node*) malloc(sizeof(Node)); 336 | } 337 | copy->parent->parent = orig.parent; 338 | copy->parent->exists = orig.exists; 339 | copy->parent->x = orig.x; 340 | copy->parent->y = orig.y; 341 | copy->parent->g = orig.g; 342 | copy->parent->h = orig.h; 343 | copy->parent->f = orig.f; 344 | copy->parent->id = orig.id; 345 | } 346 | 347 | Node lowestF(Node open[][size.width], int length) { 348 | // Copies open nodes to new array for sorting 349 | Node sort[length]; 350 | arrangeID(sort, open, length); 351 | 352 | int Switch = -1; 353 | Node curr, next; 354 | int i; 355 | 356 | while(Switch != 0) { 357 | Switch = 0; 358 | 359 | for(i = 0; i < length - 1; i++) { 360 | curr = sort[i]; 361 | next = sort[i + 1]; 362 | 363 | if(curr.f > next.f) { 364 | sort[i] = next; 365 | sort[i + 1] = curr; 366 | Switch = 1; 367 | } 368 | } 369 | } 370 | return sort[0]; 371 | } 372 | 373 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength) { 374 | int i; 375 | for(i = 0; i < 4; i++) { 376 | // Uses cosine and sine functions to get circle of points (at 90 degree intervals) around parent 377 | int possibleX = parent.x + round(-1 * cos(PI_2 * i)); 378 | int possibleY = parent.y + round(-1 * sin(PI_2 * i)); 379 | 380 | // If coordinates are outside of boundaries 381 | if(possibleX < 0 || possibleY < 0 || possibleX > (size.width - 1) || possibleY > (size.height - 1)) { 382 | continue; 383 | } 384 | 385 | // Does not make a new open node IF 386 | // Node is a border, node is already in the open list 387 | // Or the node is already in the closed list 388 | if(grid[possibleY][possibleX] == '#' || closed[possibleY][possibleX].exists == 1 || open[possibleY][possibleX].exists == 1) { 389 | continue; 390 | } 391 | // Create an open node, calculate the values and add it to the open list 392 | calculateNodeValues(parent, goal, possibleX, possibleY, grid, open, openLength); 393 | } 394 | parent = lowestF(open, *openLength); 395 | 396 | // If chosen parent does not exist.. 397 | // Must be no path, exhausted entire map 398 | if(parent.exists != 1) { 399 | printf("NO PATH U BETCH\n"); 400 | return; 401 | } 402 | // If parent node is the goal! 403 | else if(grid[parent.y][parent.x] == 'G') { 404 | printf("\nfound path!!\n"); 405 | 406 | Node lastClosed = getNode(closed, (*closedLength)-1); 407 | connectPath(path, pathLength, lastClosed); 408 | return; 409 | } 410 | 411 | // Remove parent from open list 412 | removeListNode(open, openLength, parent); 413 | 414 | // Adds parent to closed list 415 | addListNode(closed, closedLength, parent); 416 | 417 | // Recursive call to function, replacing parent as the open node with lowest fCost 418 | findPath(grid, open, closed, path, start, goal, parent, openLength, closedLength, pathLength); 419 | } 420 | 421 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength) { 422 | // Creates a new open node (to be looked at) 423 | Node openNode; 424 | clearNode(&openNode); 425 | 426 | // Declaring and initalizing variables used for calculations 427 | int gCost = 0; 428 | int hxDiff = 0; 429 | int hyDiff = 0; 430 | int hCost = 0; 431 | int fCost = 0; 432 | 433 | // Sets coordinates of the open node 434 | openNode.x = possibleX; 435 | openNode.y = possibleY; 436 | 437 | // Sets the parent of the open node 438 | setParent(&openNode, parent); 439 | 440 | // Calculating G cost 441 | // Cost to move from parent node to one open node 442 | gCost = parent.g + 1; 443 | openNode.g = gCost; 444 | 445 | // Calculating H cost 446 | // Estimated cost to move from the open node to the goal node 447 | hxDiff = fabs(goal.x - openNode.x); 448 | hyDiff = fabs(goal.y - openNode.y); 449 | hCost = hxDiff + hyDiff; 450 | openNode.h = hCost; 451 | 452 | // Calculating F cost (combined G and H) 453 | fCost = gCost + hCost; 454 | openNode.f = fCost; 455 | 456 | // Add the open node to the open list 457 | addListNode(open, openLength, openNode); 458 | } 459 | 460 | void connectPath(Node path[][size.width], int *pathLength, Node parent) { 461 | // Parent pointers are followed from goal until it reaches start 462 | while(parent.parent != NULL) { 463 | // Adds parent to the path list 464 | addListNode(path, pathLength, parent); 465 | 466 | // Parent of parent is saved 467 | parent = *(parent.parent); 468 | } 469 | // Flips id's in path because they were recorded in reverse 470 | reverseID(path, *pathLength); 471 | } 472 | 473 | void reverseID(Node path[][size.width], int pathLength) { 474 | int s, e = pathLength - 1; 475 | Node start, end; 476 | 477 | for(s = 0; s < e; s++) { 478 | start = getNode(path, s); 479 | end = getNode(path, e); 480 | 481 | path[start.y][start.x].id = e; 482 | path[end.y][end.x].id = s; 483 | e--; 484 | } 485 | } 486 | 487 | // Reads grid from file 488 | // Returns 0 if correct and 1 if grid is incomplete 489 | int readFile(FILE *file, char grid[][size.width]) { 490 | char c = '\0'; 491 | int h, w; 492 | 493 | rewind(file); 494 | for(h = 0; h < size.height; h++) { 495 | for(w = 0; (c = fgetc(file)) && w < size.width && !feof(file); w++) { 496 | if(c == '\n') { 497 | printf("Error: Grid data is incomplete at x: %d, y: %d\n", w + 1, h + 1); 498 | return 1; 499 | } 500 | grid[h][w] = c; 501 | } 502 | } 503 | return 0; 504 | } 505 | 506 | void initString(char string[]) { 507 | int length = strlen(string); 508 | int w; 509 | for(w = 0; w < length; w++) { 510 | string[w] = '\0'; 511 | } 512 | } 513 | 514 | void writeFile(char grid[][size.width], char origName[]) { 515 | int nameLen = strlen(origName); 516 | int newNameLen = nameLen + 8; 517 | 518 | char name[newNameLen]; 519 | initString(name); 520 | char addon[] = "solution"; 521 | int el = -1; 522 | int a = -1; 523 | int ri = -1; 524 | int ari = -1; 525 | FILE *file; 526 | 527 | // Gets the location to replace 528 | for(ri = nameLen; origName[ri] != '.'; ri--); 529 | 530 | ari = ri + 8; 531 | el = nameLen - ri + 1; 532 | char extension[el]; 533 | initString(extension); 534 | 535 | for(a = ri; a < nameLen; a++) { 536 | extension[a - ri] = origName[a]; 537 | } 538 | extension[a - ri] = '\0'; 539 | 540 | int w; 541 | for(w = 0; w < newNameLen; w++) { 542 | if(w < ri) { 543 | name[w] = origName[w]; 544 | } 545 | else if(w < ari){ 546 | name[w] = addon[w - nameLen + el - 1]; 547 | } 548 | else if (w < ari + el){ 549 | name[w] = extension[w - ari]; 550 | } 551 | } 552 | name[w] = '\0'; 553 | 554 | printf("Filename: %s\n", name); 555 | file = fopen(name, "w"); 556 | 557 | int h; 558 | for(h = 0; h < size.height; h++) { 559 | for(w = 0; w < size.width; w++) { 560 | fputc(grid[h][w], file); 561 | } 562 | fputc('\n', file); 563 | } 564 | fclose(file); 565 | } 566 | 567 | GridSize getGridSize(FILE *file) { 568 | char c = '\0'; 569 | int x, y = 0; 570 | GridSize size; 571 | 572 | if(file != NULL) { 573 | // Gets the length of the first line 574 | for(x = 0; (c = fgetc(file)) && (c != '\n' && !feof(file)); x++); 575 | 576 | // Resets file pointer to start of file 577 | rewind(file); 578 | 579 | // Gets the number of lines in the file 580 | while((c = fgetc(file)) && !feof(file)) { 581 | if(c == '\n') { 582 | y++; 583 | } 584 | } 585 | 586 | // Adds one to the line count 587 | if(x > 0) { 588 | y++; 589 | } 590 | } 591 | 592 | size.width = x; 593 | size.height = y; 594 | return size; 595 | } 596 | -------------------------------------------------------------------------------- /backups/pathfinding_copy3.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #define PI_2 3.14159/2 5 | 6 | // Creating GridSize struct 7 | typedef struct GridSize { 8 | int width; 9 | int height; 10 | } GridSize; 11 | 12 | typedef struct Node { 13 | struct Node* parent; 14 | int exists; 15 | int x; 16 | int y; 17 | int g; 18 | int h; 19 | int f; 20 | int id; 21 | } Node; 22 | 23 | GridSize size; 24 | GridSize getGridSize(FILE *file); 25 | int readFile(FILE *file, char grid[][size.width]); 26 | void clearGrid(char grid[][size.width]); 27 | void printGrid(char grid[][size.width]); 28 | void clearNode(Node *node); 29 | void initNodes(Node node[][size.width]); 30 | void printNodes(Node node[][size.width]); 31 | void findNodes(char grid[][size.width], Node node[][size.width], char toFind); 32 | 33 | void printSort(Node sort[], int length); 34 | void printNodeInfo(Node nodes[][size.width]); 35 | 36 | Node getNodeChar(char grid[][size.width], char toFind); 37 | Node getNode(Node[][size.width], int openLength); 38 | void addListNode(Node list[][size.width], int* length, Node node, char debug[]); 39 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength); 40 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength); 41 | void connectPath(char grid[][size.width], Node path[][size.width], Node parent, int *pathLength); 42 | 43 | int main(int argc, char* argv[]) { 44 | 45 | FILE *file = fopen(argv[1], "r"); 46 | int gridError = 0; 47 | 48 | if(file != NULL) { 49 | size = getGridSize(file); 50 | 51 | char grid[size.height][size.width]; 52 | 53 | clearGrid(grid); 54 | 55 | gridError = readFile(file, grid); 56 | 57 | // If there is no error reading grid from file 58 | if(gridError == 0) { 59 | printGrid(grid); 60 | 61 | // Declaring Node arrays 62 | Node open[size.height][size.width]; 63 | Node closed[size.height][size.width]; 64 | Node path[size.height][size.width]; 65 | 66 | // Initializing Node arrays 67 | initNodes(open); 68 | initNodes(closed); 69 | initNodes(path); 70 | 71 | int openLength = 0; 72 | int closedLength = 0; 73 | int pathLength = 0; 74 | 75 | /* Parents */ 76 | // Node parent, parentParent; 77 | // Node test; 78 | // parentParent.g = 20; 79 | // parent.g = 10; 80 | // 81 | // open[0][0].parent = &parent; 82 | // open[0][0].parent->parent = &parentParent; 83 | // // PROBLEM:: HERE IT WORKS.. 84 | // // In printParents it doesnt. setting parent 85 | // // Nodes must be off in open Node creation 86 | // // Or while switching to closed Nodes 87 | // test = *(open[0][0].parent); 88 | // 89 | // printf("Parent: %d\n", open[0][0].parent->g); 90 | // printf("Parent(x2): %d\n", open[0][0].parent->parent->g); 91 | // printf("ParentTONode: %d\n", test.g); 92 | 93 | Node start = getNodeChar(grid, 'S'); 94 | Node goal = getNodeChar(grid, 'G'); 95 | 96 | // If start node was not found 97 | if(start.exists == 0) { 98 | printf("Error: Start node was not found.\n"); 99 | } 100 | // If goal node was not found 101 | if(goal.exists == 0) { 102 | printf("Error: Goal node was not found.\n"); 103 | } 104 | 105 | // If start and goal nodes both exist 106 | if(start.exists == 1 && goal.exists == 1) { 107 | // Adds the start node to the closed list 108 | addListNode(closed, &closedLength, start, "START"); 109 | 110 | findPath(grid, open, closed, path, start, goal, start, &openLength, &closedLength, &pathLength); 111 | printNodes(closed); 112 | } 113 | } 114 | } 115 | else { 116 | printf("Error: \"%s\" does not exist.\n", argv[1]); 117 | } 118 | 119 | return 0; 120 | } 121 | 122 | void addListNode(Node list[][size.width], int* length, Node node, char debug[]) { 123 | // Crashes program 124 | // Node parent = *(node.parent); 125 | 126 | if(node.parent != NULL) { 127 | printf("ALN %s\n%d %d\n", debug, node.parent->x, node.parent->y); 128 | } 129 | 130 | list[node.y][node.x] = node; 131 | list[node.y][node.x].exists = 1; 132 | list[node.y][node.x].id = (*length); 133 | (*length)++; 134 | 135 | //printf("alN x: %d, y: %d, E: %d. Px: %d, Py: %d, E: %d\n", list[node]); 136 | // printNodeInfo(list); 137 | // printf("\n"); 138 | } 139 | 140 | void removeListNode(Node list[][size.width], int *length, Node node) { 141 | int h, w; 142 | 143 | // If node exists 144 | if(node.exists == 1) { 145 | // If node is not the last in list 146 | if(node.id != (*length)-1) { 147 | // Iterates through every node in list 148 | for(h = 0; h < size.height; h++) { 149 | for(w = 0; w < size.width; w++) { 150 | // If node has id greater than one being removed 151 | if(list[h][w].id > node.id) { 152 | // Decrement id 153 | list[h][w].id--; 154 | } 155 | } 156 | } 157 | } 158 | // Clear values on the node 159 | clearNode(&list[node.y][node.x]); 160 | 161 | // Decrement length 162 | (*length)--; 163 | } 164 | } 165 | 166 | void clearGrid(char grid[][size.width]) { 167 | int h, w; 168 | 169 | for(h = 0; h < size.height; h++) { 170 | for(w = 0; w < size.width; w++) { 171 | grid[h][w] = '0'; 172 | } 173 | } 174 | } 175 | 176 | void printGrid(char grid[][size.width]) { 177 | int h, w; 178 | 179 | for(h = 0; h < size.height; h++) { 180 | for(w = 0; w < size.width; w++) { 181 | printf("%c", grid[h][w]); 182 | } 183 | printf("\n"); 184 | } 185 | } 186 | 187 | // Initializes all nodes to default values 188 | void initNodes(Node node[][size.width]) { 189 | int h, w; 190 | for(h = 0; h < size.height; h++) { 191 | for(w = 0; w < size.width; w++) { 192 | clearNode(&node[h][w]); 193 | node[h][w].x = w; 194 | node[h][w].y = h; 195 | } 196 | } 197 | } 198 | 199 | // Clears the node to default values 200 | void clearNode(Node *node) { 201 | node->exists = 0; 202 | node->id = -1; 203 | node->f = -1; 204 | node->g = -1; 205 | node->h = -1; 206 | } 207 | 208 | void findNodes(char grid[][size.width], Node node[][size.width], char toFind) { 209 | int h, w; 210 | 211 | for(h = 0; h < size.height; h++) { 212 | for(w = 0; w < size.width; w++) { 213 | if(grid[h][w] == toFind) { 214 | node[h][w].exists = 1; 215 | } 216 | } 217 | } 218 | } 219 | 220 | // Prints 1 if Node at that position exists 221 | // Prints 0 if Node at that position does not exist 222 | void printNodes(Node node[][size.width]) { 223 | int h, w; 224 | 225 | for(h = 0; h < size.height; h++) { 226 | for(w = 0; w < size.width; w++) { 227 | printf("%d", node[h][w].exists); 228 | } 229 | printf("\n"); 230 | } 231 | } 232 | 233 | // Gets a node (with position) based upon the character on the grid 234 | // Used for finding the start Node and goal Node 235 | Node getNodeChar(char grid[][size.width], char toFind) { 236 | int h, w; 237 | Node start; 238 | clearNode(&start); 239 | 240 | for(h = 0; h < size.width; h++) { 241 | for(w = 0; w < size.width; w++) { 242 | // If start is found, save the position and return Node start 243 | if(grid[h][w] == toFind) { 244 | start.x = w; 245 | start.y = h; 246 | start.exists = 1; 247 | return start; 248 | } 249 | } 250 | } 251 | return start; 252 | } 253 | 254 | // Finds node with specific id number index and returns it 255 | Node getNode(Node list[][size.width], int findIndex) { 256 | int h, w; 257 | 258 | for(h = 0; h < size.height; h++) { 259 | for(w = 0; w < size.width; w++) { 260 | if(list[h][w].id == findIndex) { 261 | //printf("n.. %d, %d, E: %d //// nParent: %d, %d, E: %d\n" ,list[h][w].x, list[h][w].y, list[h][w].exists, list[h][w].parent->x, list[h][w].parent->y, list[h][w].parent->exists); 262 | return list[h][w]; 263 | } 264 | } 265 | } 266 | Node fail; 267 | fail.exists = 0; 268 | fail.id = -1; 269 | return fail; 270 | } 271 | 272 | // Arranges original nodes with a valid id number 273 | // from low to high, into 1D array to be sorted 274 | void arrangeID(Node arrange[], Node original[][size.width], int length) { 275 | int a, h, w; 276 | 277 | for(a = 0; a < length; a++) { 278 | for(h = 0; h < size.height; h++) { 279 | for(w = 0; w < size.width; w++) { 280 | if(original[h][w].id == a) { 281 | arrange[a] = original[h][w]; 282 | //printf("a.. %d, %d, E: %d //// aParent: %d, %d, E: %d\n" ,original[h][w].x, original[h][w].y, original[h][w].exists, original[h][w].parent->x, original[h][w].parent->y, original[h][w].parent->exists); 283 | //arrange[a].parent = original[h][w].parent; 284 | } 285 | } 286 | } 287 | } 288 | } 289 | 290 | void printNodeInfo(Node nodes[][size.width]) { 291 | int h, w; 292 | 293 | for(h = 0; h < size.height; h++) { 294 | for(w = 0; w < size.width; w++) { 295 | if(nodes[h][w].exists == 1) { 296 | printf("info.. %d, %d, E: %d //// Parent: %d, %d, E: %d\n" , nodes[h][w].x, nodes[h][w].y, nodes[h][w].exists, nodes[h][w].parent->x, nodes[h][w].parent->y, nodes[h][w].parent->exists); 297 | } 298 | } 299 | } 300 | } 301 | 302 | void printSort(Node sort[], int length) { 303 | int i; 304 | 305 | for(i = 0; i < length; i++) { 306 | printf("s.. %d, %d, E: %d //// sParent: %d, %d, E: %d\n", sort[i].x, sort[i].y, sort[i].exists, sort[i].parent->x, sort[i].parent->y, sort[i].parent->exists); 307 | } 308 | printf("\n"); 309 | } 310 | 311 | Node lowestFCost(Node open[][size.width], int length) { 312 | // If there is only one open Node, return it 313 | if(length == 1) { 314 | return getNode(open, 0); 315 | } 316 | // Copies open nodes to new array for sorting 317 | Node sort[length]; 318 | arrangeID(sort, open, length); 319 | 320 | // int w; 321 | // for(w = 0; w < length; w++) { 322 | // Node p = getNode(open, w); 323 | // printf("o.. %d, %d, E: %d //// oParent: %d, %d, E: %d\n", p.x, p.y, p.exists, p.parent->x, p.parent->y, p.parent->exists); 324 | // } 325 | 326 | int Switch = -1; 327 | Node curr, next; 328 | int i; 329 | 330 | while(Switch != 0) { 331 | Switch = 0; 332 | 333 | for(i = 0; i < length - 1; i++) { 334 | curr = sort[i]; 335 | next = sort[i + 1]; 336 | 337 | if(curr.f > next.f) { 338 | sort[curr.id] = next; 339 | sort[next.id] = curr; 340 | Switch = 1; 341 | } 342 | } 343 | /*int w = 0; 344 | for(w = 0; w < length; w++) { 345 | Node p = getNode(open, w); 346 | printf("o.. %d, %d, E: %d //// oParent: %d, %d, E: %d\n", p.x, p.y, p.exists, p.parent->x, p.parent->y, p.parent->exists); 347 | } 348 | //printSort(sort, length); */ 349 | } 350 | //printf("PS: %d %d\n", sort[0].parent->x, sort[0].parent->y); 351 | // Returns lowest F cost node 352 | //printf("s.. %d, %d, E: %d //// sParent: %d, %d, E: %d\n", sort[0].x, sort[0].y, sort[0].exists, sort[0].parent->x, sort[0].parent->y, sort[0].parent->exists); 353 | return sort[0]; 354 | } 355 | 356 | 357 | void printParents(Node nodes[][size.width], int length) { 358 | //int a; 359 | Node parent = getNode(nodes, length-1); 360 | 361 | //printf("P: %d %d\n", parent.x, parent.y); 362 | printf("LEN: %d\n", length); 363 | 364 | //for(a = 0; a < length-1; a++) { 365 | printf("Px: %d, Py: %d.Pe: %d\n", parent.x, parent.y, parent.exists); 366 | parent = *(parent.parent); 367 | printf("PPx: %d, PPy: %d.. PPe: %d\n", parent.x, parent.y, parent.exists); 368 | //*(struct_inst->pointer) = var; 369 | //} 370 | } 371 | 372 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength) { 373 | int i; 374 | for(i = 0; i < 4; i++) { 375 | // Uses cosine and sine functions to get circle of points (at 90 degree intervals) around parent 376 | int possibleX = parent.x + round(-1 * cos(PI_2 * i)); 377 | int possibleY = parent.y + round(-1 * sin(PI_2 * i)); 378 | 379 | // If coordinates are outside of boundaries 380 | if(possibleX < 0 || possibleY < 0 || possibleX > (size.width - 1) || possibleY > (size.height - 1)) { 381 | continue; 382 | } 383 | 384 | // Does not make a new open node IF 385 | // Node is a border, node is already in the open list 386 | // Or the node is already in the closed list 387 | if(grid[possibleY][possibleX] == '#' || closed[possibleY][possibleX].exists == 1 || open[possibleY][possibleX].exists == 1) { 388 | continue; 389 | } 390 | // Create an open node, calculate the values and add it to the open list 391 | calculateNodeValues(parent, goal, possibleX, possibleY, grid, open, openLength); 392 | } 393 | // printNodeInfo(open); 394 | // printf("\n"); 395 | // int w; 396 | // for(w = 0; w < *openLength; w++) { 397 | // Node p = getNode(open, w); 398 | // printf("o.. %d, %d, E: %d //// oParent: %d, %d, E: %d\n", p.x, p.y, p.exists, p.parent->x, p.parent->y, p.parent->exists); 399 | // } 400 | // Chooses open node with lowest F cost as new parent node 401 | 402 | Node copy[*openLength]; 403 | arrangeID(copy, open, *openLength); 404 | 405 | parent = lowestFCost(open, *openLength); 406 | printf("\nSORT\n%d %d\n\n", parent.x, parent.y); 407 | 408 | // Remove parent from open list 409 | removeListNode(open, openLength, parent); 410 | 411 | // Adds parent to closed list 412 | addListNode(closed, closedLength, parent, "CLOSED"); 413 | 414 | // If parent node is the goal! 415 | if(grid[parent.y][parent.x] == 'G') { 416 | //goal.parent = parent.parent; 417 | //printParents(open, *openLength); 418 | //Node p = *(goal.parent); 419 | printf("\nfound path: %d %d\n", parent.parent->x, parent.parent->y); 420 | //connectPath(grid, path, goal.parent, pathLength); 421 | return; 422 | } 423 | 424 | // Recursive call replacing original parent with new parent 425 | // if(*openLength + *closedLength < 10) { 426 | findPath(grid, open, closed, path, start, goal, parent, openLength, closedLength, pathLength); 427 | // } 428 | } 429 | 430 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength) { 431 | // Creates a new open node (to be looked at) 432 | Node openNode; 433 | clearNode(&openNode); 434 | 435 | // Declaring and initalizing variables used for calculations 436 | int gCost = 0; 437 | int hxDiff = 0; 438 | int hyDiff = 0; 439 | int hCost = 0; 440 | int fCost = 0; 441 | 442 | // Sets coordinates of the open node 443 | openNode.x = possibleX; 444 | openNode.y = possibleY; 445 | 446 | // Sets the parent of the open node 447 | openNode.parent = &parent; 448 | 449 | // Node p = *(openNode.parent); 450 | // printf("PoX: %d, PoY: %d, E: %d\n", p.x, p.y, p.exists); 451 | 452 | // Calculating G cost 453 | // Cost to move from parent node to one open node 454 | gCost = parent.g + 1; 455 | openNode.g = gCost; 456 | 457 | // Calculating H cost 458 | // Estimated cost to move from the open node to the goal node 459 | hxDiff = fabs(goal.x - openNode.x); 460 | hyDiff = fabs(goal.y - openNode.y); 461 | hCost = hxDiff + hyDiff; 462 | openNode.h = hCost; 463 | 464 | // Calculating F cost (combined G and H) 465 | fCost = gCost + hCost; 466 | openNode.f = fCost; 467 | 468 | // Prints information about the openNode 469 | //printf("OPENINFO.. x: %d, y: %d. Px: %d, Py: %d...g: %d, h: %d, f: %d\n", openNode.x, openNode.y, openNode.parent->x, openNode.parent->y, openNode.g, openNode.h, openNode.f); 470 | 471 | printf("\nOPEN\n%d %d\n\n", openNode.parent->x, openNode.parent->y); 472 | 473 | // Add the open node to the open list 474 | addListNode(open, openLength, openNode, "OPEN"); 475 | 476 | //printNodeInfo(open); 477 | //printf("\n"); 478 | } 479 | 480 | 481 | /* 482 | void connectPath(char grid[][size.width], Node path[][size.width], Node parent, int *pathLength) { 483 | //while(grid[parent.y][parent.x] != 'S') { 484 | addListNode(path, pathLength, parent); 485 | //parent = *(parent.parent); 486 | //} 487 | printf("PATH\n"); 488 | printNodes(path); 489 | } */ 490 | 491 | // Reads grid from file 492 | // Returns 0 if correct and 1 if grid is incomplete 493 | int readFile(FILE *file, char grid[][size.width]) { 494 | char c = '\0'; 495 | int h, w; 496 | 497 | rewind(file); 498 | for(h = 0; h < size.height; h++) { 499 | for(w = 0; (c = fgetc(file)) && w < size.width && !feof(file); w++) { 500 | if(c == '\n') { 501 | printf("Error: Grid data is incomplete at x: %d, y: %d\n", w + 1, h + 1); 502 | return 1; 503 | } 504 | grid[h][w] = c; 505 | } 506 | } 507 | return 0; 508 | } 509 | 510 | GridSize getGridSize(FILE *file) { 511 | char c = '\0'; 512 | int x, y = 0; 513 | GridSize size; 514 | 515 | if(file != NULL) { 516 | // Gets the length of the first line 517 | for(x = 0; (c = fgetc(file)) && (c != '\n' && !feof(file)); x++); 518 | 519 | // Resets file pointer to start of file 520 | rewind(file); 521 | 522 | // Gets the number of lines in the file 523 | while((c = fgetc(file)) && !feof(file)) { 524 | if(c == '\n') { 525 | y++; 526 | } 527 | } 528 | 529 | // Adds one to the line count 530 | if(x > 0) { 531 | y++; 532 | } 533 | } 534 | 535 | size.width = x; 536 | size.height = y; 537 | return size; 538 | } 539 | -------------------------------------------------------------------------------- /backups/pathfinding_final.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #define PI_2 3.14159/2 6 | 7 | // Creating GridSize struct 8 | typedef struct GridSize { 9 | int width; 10 | int height; 11 | } GridSize; 12 | 13 | // Creating Node struct 14 | typedef struct Node { 15 | struct Node* parent; 16 | int exists; 17 | int x; 18 | int y; 19 | int g; 20 | int h; 21 | int f; 22 | int id; 23 | } Node; 24 | 25 | GridSize size; 26 | GridSize getGridSize(FILE *file); 27 | int readFile(FILE *file, char grid[][size.width]); 28 | void initString(char string[]); 29 | void appendName(char origName[], char insert[], char newName[]); 30 | void writeFile(char grid[][size.width], char origName[]); 31 | void clearGrid(char grid[][size.width]); 32 | void printGrid(char grid[][size.width]); 33 | void copyGrid(char copy[][size.width], char grid[][size.width]); 34 | 35 | void clearNode(Node *node); 36 | void initNodes(Node node[][size.width]); 37 | void nodesExistence(Node node[][size.width]); 38 | Node getNodeChar(char grid[][size.width], char toFind); 39 | Node getNode(Node[][size.width], int openLength); 40 | void addListNode(Node list[][size.width], int* length, Node node); 41 | void removeListNode(Node list[][size.width], int *length, Node node); 42 | void arrangeID(Node arrange[], Node original[][size.width], int length); 43 | Node lowestF(Node open[][size.width], int length); 44 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength); 45 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength); 46 | void connectPath(Node path[][size.width], int *pathLength, Node parent); 47 | void reverseID(Node path[][size.width], int pathLength); 48 | void insertPath(char grid[][size.width], Node path[][size.width]); 49 | 50 | void info(Node node); 51 | void info1D(Node nodes[], int length); 52 | void info2D(Node nodes[][size.width], int length); 53 | 54 | int main(int argc, char* argv[]) { 55 | // Attempts to open file 56 | FILE *file = fopen(argv[1], "r"); 57 | int gridError = 0; 58 | 59 | // If file exists 60 | if(file != NULL) { 61 | // Getting the size of the grid from file 62 | size = getGridSize(file); 63 | 64 | // Declaring grid arrays based on pre calculated size 65 | char grid[size.height][size.width]; 66 | char solvedGrid[size.height][size.width]; 67 | 68 | // Initializes grid 69 | clearGrid(grid); 70 | 71 | // Reads file into grid and detects any errors 72 | gridError = readFile(file, grid); 73 | 74 | // If there is no error reading grid from file 75 | if(gridError == 0) { 76 | // Makes a copy of the grid for the solution 77 | copyGrid(solvedGrid, grid); 78 | 79 | // Prints the original map 80 | printf("=== Original Grid ===\n"); 81 | printGrid(grid); 82 | 83 | // Declaring Node arrays 84 | Node open[size.height][size.width]; 85 | Node closed[size.height][size.width]; 86 | Node path[size.height][size.width]; 87 | 88 | // Initializing Node arrays 89 | initNodes(open); 90 | initNodes(closed); 91 | initNodes(path); 92 | 93 | // Declaring and initializing lengths for Node arrays 94 | int openLength = 0; 95 | int closedLength = 0; 96 | int pathLength = 0; 97 | 98 | // Gets the node of start and goal 99 | Node start = getNodeChar(grid, 'S'); 100 | Node goal = getNodeChar(grid, 'G'); 101 | 102 | // If start node was not found 103 | if(start.exists == 0) { 104 | printf("Error: Start node was not found.\n"); 105 | } 106 | // If goal node was not found 107 | if(goal.exists == 0) { 108 | printf("Error: Goal node was not found.\n"); 109 | } 110 | 111 | // If start and goal nodes both exist 112 | if(start.exists == 1 && goal.exists == 1) { 113 | // Add start node to the closed list 114 | addListNode(closed, &closedLength, start); 115 | 116 | findPath(grid, open, closed, path, start, goal, start, &openLength, &closedLength, &pathLength); 117 | 118 | // If path was found.. 119 | if(pathLength > 0) { 120 | // Inserts spaces into solvedGrid 121 | insertPath(solvedGrid, path); 122 | 123 | // Prints solvedGrid 124 | printf("\nSOLVED:..\n"); 125 | printGrid(solvedGrid); 126 | 127 | // Writes solution file 128 | writeFile(solvedGrid, argv[1]); 129 | } 130 | } 131 | } 132 | } 133 | // If file has any errors 134 | else { 135 | if(argc == 1) { 136 | printf("Error: No file was entered in command line\n"); 137 | } 138 | else { 139 | printf("Error: \"%s\" does not exist.\n", argv[1]); 140 | } 141 | } 142 | return 0; 143 | } 144 | 145 | // Calculates the size of the grid 146 | GridSize getGridSize(FILE *file) { 147 | char c = '\0'; 148 | int x, y = 0; 149 | GridSize size; 150 | 151 | if(file != NULL) { 152 | // Gets the length of the first line 153 | for(x = 0; (c = fgetc(file)) && (c != '\n' && !feof(file)); x++); 154 | 155 | // Resets file pointer to start of file 156 | rewind(file); 157 | 158 | // Gets the number of lines in the file 159 | while((c = fgetc(file)) && !feof(file)) { 160 | if(c == '\n') { 161 | y++; 162 | } 163 | } 164 | 165 | // Adds one to the line count 166 | if(x > 0) { 167 | y++; 168 | } 169 | } 170 | 171 | size.width = x; 172 | size.height = y; 173 | return size; 174 | } 175 | 176 | // Reads grid from file 177 | // Returns 0 if correct and 1 if grid is incomplete 178 | int readFile(FILE *file, char grid[][size.width]) { 179 | char c = '\0'; 180 | int h, w; 181 | 182 | rewind(file); 183 | for(h = 0; h < size.height; h++) { 184 | for(w = 0; (c = fgetc(file)) && w < size.width && !feof(file); w++) { 185 | // Detects if the grid is incomplete, prints location and returns 1 (error) 186 | if(c == '\n' || (c != '#' && c != 'O' && c != 'S' && c != 'G')) { 187 | printf("Error: Grid data is incomplete at x: %d, y: %d\n", w + 1, h + 1); 188 | return 1; 189 | } 190 | // Saves file character to grid 191 | grid[h][w] = c; 192 | } 193 | } 194 | return 0; 195 | } 196 | 197 | // Initializes each character in string to null terminator 198 | void initString(char string[]) { 199 | int w; 200 | int length = strlen(string); 201 | for(w = 0; w < length; w++) { 202 | string[w] = '\0'; 203 | } 204 | } 205 | 206 | // Adds string to the end of origName, while keeping extension intact 207 | void appendName(char origName[], char insert[], char newName[]) { 208 | // Declaring lengths of strings 209 | int nameLen = strlen(origName); 210 | int insertLen = strlen(insert); 211 | int newNameLen = nameLen + insertLen; 212 | 213 | // Declaring index counters 214 | int ri = -1; 215 | int ei = -1; 216 | int c; 217 | 218 | // Initializing every character in newName string to '\0' 219 | initString(newName); 220 | 221 | // Gets the location to replace (finds start of extension) 222 | for(ri = nameLen; ri >= 0 && origName[ri] != '.'; ri--); 223 | 224 | // If file does not have an extension 225 | if(ri == 0) { 226 | // Replace index is very end of string 227 | ri = nameLen - 1; 228 | } 229 | // extension index starts at replace index 230 | ei = ri; 231 | 232 | // Pieces newName together 233 | for(c = 0; c < newNameLen; c++) { 234 | // Saves newName as original from start up till replace index 235 | if(c < ri) { 236 | newName[c] = origName[c]; 237 | } 238 | // Appends insert to newName 239 | else if(c < ri + insertLen) { 240 | newName[c] = insert[c - ri]; 241 | } 242 | // Appends extension from origName to newName 243 | else if(c < newNameLen) { 244 | newName[c] = origName[ei]; 245 | ei++; 246 | } 247 | } 248 | // Adds null terminator to end of string 249 | newName[c] = '\0'; 250 | } 251 | 252 | // Writes the solution file 253 | void writeFile(char grid[][size.width], char origName[]) { 254 | int h, w; 255 | FILE *file; 256 | 257 | // Declaring strings for new file name 258 | char insert[] = "solution"; 259 | char newName[strlen(origName) + strlen(insert)]; 260 | 261 | // Adds solution onto origName, keeping extension intact 262 | appendName(origName, insert, newName); 263 | 264 | // Opens file with new name for writing 265 | file = fopen(newName, "w"); 266 | 267 | // Writes each character of solved grid to new file 268 | for(h = 0; h < size.height; h++) { 269 | for(w = 0; w < size.width; w++) { 270 | fputc(grid[h][w], file); 271 | } 272 | fputc('\n', file); 273 | } 274 | // Close file 275 | fclose(file); 276 | } 277 | 278 | // Clears each character in grid to '0' 279 | void clearGrid(char grid[][size.width]) { 280 | int h, w; 281 | 282 | for(h = 0; h < size.height; h++) { 283 | for(w = 0; w < size.width; w++) { 284 | grid[h][w] = '0'; 285 | } 286 | } 287 | } 288 | 289 | // Prints grid characters as they appear 290 | void printGrid(char grid[][size.width]) { 291 | int h, w; 292 | 293 | for(h = 0; h < size.height; h++) { 294 | for(w = 0; w < size.width; w++) { 295 | printf("%c", grid[h][w]); 296 | } 297 | printf("\n"); 298 | } 299 | } 300 | 301 | // Copies one grid to another, used for solvedGrid 302 | void copyGrid(char copy[][size.width], char grid[][size.width]) { 303 | int h, w; 304 | 305 | for(h = 0; h < size.height; h++) { 306 | for(w = 0; w < size.width; w++) { 307 | copy[h][w] = grid[h][w]; 308 | } 309 | } 310 | } 311 | 312 | // Clears the node to default values 313 | void clearNode(Node *node) { 314 | node->parent = NULL; 315 | node->exists = 0; 316 | node->id = -1; 317 | node->f = -1; 318 | node->g = -1; 319 | node->h = -1; 320 | } 321 | 322 | // Initializes all nodes to default values 323 | void initNodes(Node node[][size.width]) { 324 | int h, w; 325 | for(h = 0; h < size.height; h++) { 326 | for(w = 0; w < size.width; w++) { 327 | clearNode(&node[h][w]); 328 | node[h][w].x = w; 329 | node[h][w].y = h; 330 | } 331 | } 332 | } 333 | 334 | // Prints 1 if Node at that position exists 335 | // Prints 0 if Node at that position does not exist 336 | void nodesExistence(Node node[][size.width]) { 337 | int h, w; 338 | 339 | for(h = 0; h < size.height; h++) { 340 | for(w = 0; w < size.width; w++) { 341 | printf("%d", node[h][w].exists); 342 | } 343 | printf("\n"); 344 | } 345 | } 346 | 347 | // Gets a node (with position) based upon the character on the grid 348 | // Used for finding the start Node and goal Node 349 | Node getNodeChar(char grid[][size.width], char toFind) { 350 | int h, w; 351 | Node node; 352 | clearNode(&node); 353 | 354 | for(h = 0; h < size.width; h++) { 355 | for(w = 0; w < size.width; w++) { 356 | // If char 'toFind' is found, save the position and return node 357 | if(grid[h][w] == toFind) { 358 | node.x = w; 359 | node.y = h; 360 | node.exists = 1; 361 | return node; 362 | } 363 | } 364 | } 365 | return node; 366 | } 367 | 368 | // Finds node with specific id number index and returns it 369 | Node getNode(Node list[][size.width], int findIndex) { 370 | int h, w; 371 | 372 | for(h = 0; h < size.height; h++) { 373 | for(w = 0; w < size.width; w++) { 374 | if(list[h][w].id == findIndex) { 375 | return list[h][w]; 376 | } 377 | } 378 | } 379 | Node fail; 380 | fail.exists = 0; 381 | fail.id = -1; 382 | return fail; 383 | } 384 | 385 | // Copies data from Node orig into parent of Node copy 386 | void setParent(Node *copy, Node orig) { 387 | if(copy->parent == NULL) { 388 | copy->parent = (Node*) malloc(sizeof(Node)); 389 | } 390 | copy->parent->parent = orig.parent; 391 | copy->parent->exists = orig.exists; 392 | copy->parent->x = orig.x; 393 | copy->parent->y = orig.y; 394 | copy->parent->g = orig.g; 395 | copy->parent->h = orig.h; 396 | copy->parent->f = orig.f; 397 | copy->parent->id = orig.id; 398 | } 399 | 400 | // Adds a node to a list by saving it's id, index in the list 401 | void addListNode(Node list[][size.width], int* length, Node node) { 402 | list[node.y][node.x] = node; 403 | list[node.y][node.x].exists = 1; 404 | 405 | // Saves id as a representative of index in list 406 | list[node.y][node.x].id = (*length); 407 | (*length)++; 408 | } 409 | 410 | // Removes a node from a list 411 | void removeListNode(Node list[][size.width], int *length, Node node) { 412 | int h, w; 413 | 414 | // If node exists 415 | if(node.exists == 1) { 416 | // If node is not the last in list 417 | if(node.id != (*length)-1) { 418 | // Iterates through every node in list 419 | for(h = 0; h < size.height; h++) { 420 | for(w = 0; w < size.width; w++) { 421 | // If node has id greater than one being removed 422 | if(list[h][w].id > node.id) { 423 | // Decrement id 424 | list[h][w].id--; 425 | } 426 | } 427 | } 428 | } 429 | // Clear values on the node 430 | clearNode(&list[node.y][node.x]); 431 | 432 | // Free parent from memory 433 | free(list[node.y][node.x].parent); 434 | 435 | // Decrement length 436 | (*length)--; 437 | } 438 | } 439 | 440 | // Arranges original nodes with a valid id number 441 | // from low to high, into 1D array to be sorted 442 | void arrangeID(Node arrange[], Node original[][size.width], int length) { 443 | int a, h, w; 444 | 445 | for(a = 0; a < length; a++) { 446 | for(h = 0; h < size.height; h++) { 447 | for(w = 0; w < size.width; w++) { 448 | // If node id matches the search criteria 449 | if(original[h][w].id == a) { 450 | // Saves node in arrange at index of node id 451 | arrange[a] = original[h][w]; 452 | break; 453 | } 454 | } 455 | } 456 | } 457 | } 458 | 459 | // Returns node with lowest F value 460 | Node lowestF(Node open[][size.width], int length) { 461 | // Copies open nodes to new 1D array for sorting 462 | Node sort[length]; 463 | arrangeID(sort, open, length); 464 | 465 | int Switch = -1; 466 | Node curr, next; 467 | int i; 468 | 469 | // Sorts open array from low to high 470 | while(Switch != 0) { 471 | Switch = 0; 472 | 473 | // Iterates each existing open node (length) 474 | for(i = 0; i < length - 1; i++) { 475 | curr = sort[i]; 476 | next = sort[i + 1]; 477 | 478 | // SWAP: if curr Node has greater f than next Node 479 | if(curr.f > next.f) { 480 | sort[i] = next; 481 | sort[i + 1] = curr; 482 | Switch = 1; 483 | } 484 | } 485 | } 486 | // Returns node with lowest f 487 | return sort[0]; 488 | } 489 | 490 | // Main recursive function to find open nodes, new parent, and repeat 491 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength) { 492 | int i; 493 | // Checks all 4 directions 494 | for(i = 0; i < 4; i++) { 495 | // Uses cosine and sine functions to get circle of points (at 90 degree intervals) around parent 496 | int possibleX = parent.x + round(-1 * cos(PI_2 * i)); 497 | int possibleY = parent.y + round(-1 * sin(PI_2 * i)); 498 | 499 | // If coordinates are outside of boundaries 500 | if(possibleX < 0 || possibleY < 0 || possibleX > (size.width - 1) || possibleY > (size.height - 1)) { 501 | continue; 502 | } 503 | 504 | // Does not make a new open node IF 505 | // Node is a border, node is already in the open list 506 | // Or the node is already in the closed list 507 | if(grid[possibleY][possibleX] == '#' || closed[possibleY][possibleX].exists == 1 || open[possibleY][possibleX].exists == 1) { 508 | continue; 509 | } 510 | // Create an open node, calculate the values and add it to the open list 511 | calculateNodeValues(parent, goal, possibleX, possibleY, grid, open, openLength); 512 | } 513 | // Sets new parent as openNode with the lowest F cost 514 | parent = lowestF(open, *openLength); 515 | 516 | // If chosen parent does not exist.. 517 | // Must be no path, exhausted entire map 518 | if(parent.exists != 1) { 519 | // Inform user that no path was found 520 | printf("\n!! No path exists !!\n"); 521 | 522 | // Breaks out of entire pathfinding functions 523 | return; 524 | } 525 | // If parent node is the goal! 526 | else if(grid[parent.y][parent.x] == 'G') { 527 | // Get the last node in the closed list 528 | Node lastClosed = getNode(closed, (*closedLength)-1); 529 | 530 | // Connect path by tracing back parents, starting at lastClosed Node 531 | connectPath(path, pathLength, lastClosed); 532 | return; 533 | } 534 | 535 | // Remove parent from open list 536 | removeListNode(open, openLength, parent); 537 | 538 | // Adds parent to closed list 539 | addListNode(closed, closedLength, parent); 540 | 541 | // Recursive call to function, replacing parent as the open node with lowest fCost 542 | findPath(grid, open, closed, path, start, goal, parent, openLength, closedLength, pathLength); 543 | } 544 | 545 | // Calculates mathematical values on a single open node 546 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength) { 547 | // Creates a new open node (to be looked at) 548 | Node openNode; 549 | clearNode(&openNode); 550 | 551 | // Declaring and initalizing variables used for calculations 552 | int gCost = 0; 553 | int hxDiff = 0; 554 | int hyDiff = 0; 555 | int hCost = 0; 556 | int fCost = 0; 557 | 558 | // Sets coordinates of the open node 559 | openNode.x = possibleX; 560 | openNode.y = possibleY; 561 | 562 | // Sets the parent of the open node 563 | setParent(&openNode, parent); 564 | 565 | // Calculating G cost 566 | // Cost to move from parent node to one open node 567 | gCost = parent.g + 1; 568 | openNode.g = gCost; 569 | 570 | // Calculating H cost 571 | // Estimated cost to move from the open node to the goal node 572 | hxDiff = fabs(goal.x - openNode.x); 573 | hyDiff = fabs(goal.y - openNode.y); 574 | hCost = hxDiff + hyDiff; 575 | openNode.h = hCost; 576 | 577 | // Calculating F cost (combined G and H) 578 | fCost = gCost + hCost; 579 | openNode.f = fCost; 580 | 581 | // Add the open node to the open list 582 | addListNode(open, openLength, openNode); 583 | } 584 | 585 | // Parent pointers are followed from goal until it reaches start 586 | void connectPath(Node path[][size.width], int *pathLength, Node parent) { 587 | while(parent.parent != NULL) { 588 | // Adds parent to the path list 589 | addListNode(path, pathLength, parent); 590 | 591 | // Parent Node is overwritten by parent Node of itself 592 | parent = *(parent.parent); 593 | } 594 | // Flips id's in path because they were recorded in reverse 595 | reverseID(path, *pathLength); 596 | } 597 | 598 | // Reverses Node id's in a grid 599 | void reverseID(Node path[][size.width], int pathLength) { 600 | // s starts at 0, e starts at last index in path 601 | int s, e = pathLength - 1; 602 | Node start, end; 603 | 604 | // s increments while e decrements 605 | for(s = 0; s < e; s++) { 606 | // Gets node at id s and e 607 | start = getNode(path, s); 608 | end = getNode(path, e); 609 | 610 | // Swaps id's of nodes start and end 611 | path[start.y][start.x].id = e; 612 | path[end.y][end.x].id = s; 613 | e--; 614 | } 615 | } 616 | 617 | // Replaces path locations with spaces in solvedGrid 618 | void insertPath(char grid[][size.width], Node path[][size.width]) { 619 | int h, w; 620 | 621 | for(h = 0; h < size.height; h++) { 622 | for(w = 0; w < size.width; w++) { 623 | if(path[h][w].exists == 1) { 624 | // Replaces a path location with a space 625 | grid[h][w] = ' '; 626 | } 627 | } 628 | } 629 | } 630 | 631 | // Prints information about one node 632 | void info(Node node) { 633 | if(node.parent != NULL) { 634 | printf("info.. %d, %d. f: %d. id: %d. E: %d ", node.x, node.y, node.f, node.id, node.exists); 635 | printf("//// Parent: %d, %d. f: %d. id: %d. E: %d\n", node.parent->x, node.parent->y, node.parent->f, node.parent->id, node.parent->exists); 636 | } 637 | else { 638 | printf("info.. %d, %d. f: %d. id: %d. E: %d //// Parent = NULL\n", node.x, node.y, node.f, node.id, node.exists); 639 | } 640 | } 641 | 642 | // Prints information of a one-dimensional array of nodes 643 | void info1D(Node nodes[], int length) { 644 | int h; 645 | for(h = 0; h < length; h++) { 646 | if(nodes[h].exists == 1) { 647 | info(nodes[h]); 648 | } 649 | } 650 | } 651 | 652 | // Prints information of a two-dimensional (grid) array of nodes 653 | void info2D(Node nodes[][size.width], int length) { 654 | int a, h, w; 655 | 656 | for(a = 0; a < length; a++) { 657 | for(h = 0; h < size.height; h++) { 658 | for(w = 0; w < size.width; w++) { 659 | if(nodes[h][w].id == a) { 660 | info(nodes[h][w]); 661 | break; 662 | } 663 | } 664 | } 665 | } 666 | } 667 | -------------------------------------------------------------------------------- /backups/testing.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #define size 6 4 | 5 | typedef struct Node { 6 | struct Node* parent; 7 | int exists; 8 | int x; 9 | int y; 10 | int g; 11 | int h; 12 | int f; 13 | int id; 14 | } Node; 15 | 16 | void initNodes(Node node[]); 17 | void clearNode(Node *node); 18 | void info(Node node); 19 | void nodesInfo(Node nodes[]); 20 | Node getNode(Node list[], int findIndex); 21 | Node lowestF(Node open[], int length); 22 | void arrangeID(Node arrange[], Node original[], int length); 23 | void addListNode(Node list[], int* length, Node node); 24 | void removeListNode(Node list[], int *length, Node node); 25 | void setParent(Node *parent, Node orig); 26 | 27 | int main(void) { 28 | Node open[size]; 29 | int openLength = 0; 30 | //int closedLength = 0; 31 | 32 | initNodes(open); 33 | 34 | open[0].f = 31; 35 | open[1].f = 8; 36 | open[2].f = 1; 37 | open[3].f = 32; 38 | open[4].f = 16; 39 | open[5].f = 2; 40 | 41 | int i; 42 | // Adds all nodes to open list 43 | for(i = 0; i < size; i++) { 44 | addListNode(open, &openLength, open[i]); 45 | } 46 | setParent(&open[1], open[0]); 47 | setParent(&open[2], open[1]); 48 | setParent(&open[3], open[2]); 49 | setParent(&open[4], open[3]); 50 | setParent(&open[5], open[4]); 51 | 52 | nodesInfo(open); 53 | 54 | Node find = getNode(open, 2); 55 | printf("\nFind.. id 2\n"); 56 | info(find); 57 | printf("\n"); 58 | 59 | Node lowF = lowestF(open, size); 60 | printf("\nLOW\n"); 61 | info(lowF); 62 | 63 | removeListNode(open, &openLength, lowF); 64 | printf("\nRemoved lowF from open list.\n"); 65 | nodesInfo(open); 66 | 67 | return 0; 68 | } 69 | 70 | // Copies data from Node orig into parent of Node copy 71 | void setParent(Node *copy, Node orig) { 72 | if(copy->parent == NULL) { 73 | copy->parent = (Node*) malloc(sizeof(Node)); 74 | } 75 | copy->parent->exists = orig.exists; 76 | copy->parent->x = orig.x; 77 | copy->parent->y = orig.y; 78 | copy->parent->g = orig.g; 79 | copy->parent->h = orig.h; 80 | copy->parent->f = orig.f; 81 | copy->parent->id = orig.id; 82 | } 83 | 84 | void addListNode(Node list[], int* length, Node node) { 85 | list[node.x] = node; 86 | list[node.x].exists = 1; 87 | list[node.x].id = (*length); 88 | (*length)++; 89 | } 90 | 91 | void removeListNode(Node list[], int *length, Node node) { 92 | int h; 93 | 94 | // If node exists 95 | if(node.exists == 1) { 96 | // If node is not the last in list 97 | if(node.id != (*length)-1) { 98 | // Iterates through every node in list 99 | for(h = 0; h < size; h++) { 100 | // If node has id greater than one being removed 101 | if(list[h].id > node.id) { 102 | // Decrement id 103 | list[h].id--; 104 | } 105 | } 106 | } 107 | // Clear values on the node 108 | clearNode(&list[node.x]); 109 | 110 | // Free parent from memory 111 | free(list[node.x].parent); 112 | 113 | // Decrement length 114 | (*length)--; 115 | } 116 | } 117 | 118 | // Arranges original opens with a valid id number 119 | // from low to high, into 1D array to be sorted 120 | void arrangeID(Node arrange[], Node original[], int length) { 121 | int a, h; 122 | 123 | for(a = 0; a < length; a++) { 124 | for(h = 0; h < size; h++) { 125 | if(original[h].id == a) { 126 | arrange[a] = original[h]; 127 | break; 128 | } 129 | } 130 | } 131 | } 132 | 133 | Node lowestF(Node open[], int length) { 134 | // Copies open opens to new array for sorting 135 | Node sort[length]; 136 | arrangeID(sort, open, length); 137 | 138 | int Switch = -1; 139 | Node curr, next; 140 | int i; 141 | 142 | while(Switch != 0) { 143 | Switch = 0; 144 | 145 | for(i = 0; i < length - 1; i++) { 146 | curr = sort[i]; 147 | next = sort[i + 1]; 148 | 149 | if(curr.f > next.f) { 150 | sort[i] = next; 151 | sort[i + 1] = curr; 152 | Switch = 1; 153 | } 154 | } 155 | } 156 | printf("SORT\n"); 157 | nodesInfo(sort); 158 | return sort[0]; 159 | } 160 | 161 | // Finds open with specific id number index and returns it 162 | Node getNode(Node list[], int findIndex) { 163 | int h; 164 | 165 | for(h = 0; h < size; h++) { 166 | if(list[h].id == findIndex) { 167 | return list[h]; 168 | } 169 | } 170 | Node fail; 171 | fail.exists = 0; 172 | fail.id = -1; 173 | return fail; 174 | } 175 | 176 | // Initializes all opens to default values 177 | void initNodes(Node node[]) { 178 | int h; 179 | for(h = 0; h < size; h++) { 180 | clearNode(&node[h]); 181 | node[h].x = h; 182 | node[h].y = 0; 183 | } 184 | } 185 | 186 | // Clears the node to default values 187 | void clearNode(Node *node) { 188 | node->parent = NULL; 189 | node->exists = 0; 190 | node->id = -1; 191 | node->f = -1; 192 | node->g = -1; 193 | node->h = -1; 194 | } 195 | 196 | void info(Node node) { 197 | if(node.parent != NULL) { 198 | printf("info.. %d, %d. f: %d. id: %d. E: %d ", node.x, node.y, node.f, node.id, node.exists); 199 | printf("//// Parent: %d, %d. f: %d. id: %d. E: %d\n", node.parent->x, node.parent->y, node.parent->f, node.parent->id, node.parent->exists); 200 | } 201 | else { 202 | printf("info.. %d, %d. f: %d. id: %d. E: %d //// Parent = NULL\n", node.x, node.y, node.f, node.id, node.exists); 203 | } 204 | } 205 | 206 | void nodesInfo(Node nodes[]) { 207 | int h; 208 | for(h = 0; h < size; h++) { 209 | if(nodes[h].exists == 1) { 210 | info(nodes[h]); 211 | } 212 | } 213 | } 214 | -------------------------------------------------------------------------------- /file.c: -------------------------------------------------------------------------------- 1 | // Reads grid from file 2 | // Returns 0 if correct and 1 if grid is incomplete 3 | int readFile(FILE *file, char grid[][size.width]) { 4 | char c = '\0'; 5 | int h, w; 6 | 7 | rewind(file); 8 | for(h = 0; h < size.height; h++) { 9 | for(w = 0; (c = fgetc(file)) && w < size.width && !feof(file); w++) { 10 | // Detects if the grid is incomplete, prints location and returns 1 (error) 11 | if(c == '\n' || (c != '#' && c != 'O' && c != 'S' && c != 'G')) { 12 | printf("Error: Grid data is incomplete at x: %d, y: %d\n", w + 1, h + 1); 13 | return 1; 14 | } 15 | // Saves file character to grid 16 | grid[h][w] = c; 17 | } 18 | } 19 | return 0; 20 | } 21 | 22 | // Writes the solution file 23 | void writeFile(char grid[][size.width], char origName[]) { 24 | int h, w; 25 | FILE *file; 26 | 27 | // Declaring strings for new file name 28 | char insert[] = "solution"; 29 | char newName[strlen(origName) + strlen(insert)]; 30 | 31 | // Adds solution onto origName, keeping extension intact 32 | appendName(origName, insert, newName); 33 | 34 | // Opens file with new name for writing 35 | file = fopen(newName, "w"); 36 | 37 | // Writes each character of solved grid to new file 38 | for(h = 0; h < size.height; h++) { 39 | for(w = 0; w < size.width; w++) { 40 | fputc(grid[h][w], file); 41 | } 42 | fputc('\n', file); 43 | } 44 | // Close file 45 | fclose(file); 46 | } 47 | 48 | // Adds string to the end of origName, while keeping extension intact 49 | void appendName(char origName[], char insert[], char newName[]) { 50 | // Declaring lengths of strings 51 | int nameLen = strlen(origName); 52 | int insertLen = strlen(insert); 53 | int newNameLen = nameLen + insertLen; 54 | 55 | // Declaring index counters 56 | int ri = -1; 57 | int ei = -1; 58 | int c; 59 | 60 | // Initializing every character in newName string to '\0' 61 | initString(newName); 62 | 63 | // Gets the location to replace (finds start of extension) 64 | for(ri = nameLen; ri >= 0 && origName[ri] != '.'; ri--); 65 | 66 | // If file does not have an extension 67 | if(ri == 0) { 68 | // Replace index is very end of string 69 | ri = nameLen - 1; 70 | } 71 | // extension index starts at replace index 72 | ei = ri; 73 | 74 | // Pieces newName together 75 | for(c = 0; c < newNameLen; c++) { 76 | // Saves newName as original from start up till replace index 77 | if(c < ri) { 78 | newName[c] = origName[c]; 79 | } 80 | // Appends insert to newName 81 | else if(c < ri + insertLen) { 82 | newName[c] = insert[c - ri]; 83 | } 84 | // Appends extension from origName to newName 85 | else if(c < newNameLen) { 86 | newName[c] = origName[ei]; 87 | ei++; 88 | } 89 | } 90 | // Adds null terminator to end of string 91 | newName[c] = '\0'; 92 | } 93 | 94 | // Initializes each character in string to null terminator 95 | void initString(char string[]) { 96 | int w; 97 | int length = strlen(string); 98 | for(w = 0; w < length; w++) { 99 | string[w] = '\0'; 100 | } 101 | } 102 | -------------------------------------------------------------------------------- /funcDefns.h: -------------------------------------------------------------------------------- 1 | int readFile(FILE *file, char grid[][size.width]); 2 | void initString(char string[]); 3 | void appendName(char origName[], char insert[], char newName[]); 4 | void writeFile(char grid[][size.width], char origName[]); 5 | void clearGrid(char grid[][size.width]); 6 | void printGrid(char grid[][size.width]); 7 | void copyGrid(char copy[][size.width], char grid[][size.width]); 8 | 9 | void clearNode(Node *node); 10 | void initNodes(Node node[][size.width]); 11 | void nodesExistence(Node node[][size.width]); 12 | Node getNodeChar(char grid[][size.width], char toFind); 13 | Node getNode(Node[][size.width], int openLength); 14 | void addListNode(Node list[][size.width], int* length, Node node); 15 | void removeListNode(Node list[][size.width], int *length, Node node); 16 | void arrangeID(Node arrange[], Node original[][size.width], int length); 17 | Node lowestF(Node open[][size.width], int length); 18 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength); 19 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength); 20 | void connectPath(Node path[][size.width], int *pathLength, Node parent); 21 | void reverseID(Node path[][size.width], int pathLength); 22 | void insertPath(char grid[][size.width], Node path[][size.width]); 23 | 24 | void info(Node node); 25 | void info1D(Node nodes[], int length); 26 | void info2D(Node nodes[][size.width], int length); 27 | -------------------------------------------------------------------------------- /grid.c: -------------------------------------------------------------------------------- 1 | // Calculates the size of the grid 2 | GridSize getGridSize(FILE *file) { 3 | char c = '\0'; 4 | char pc = '\0'; 5 | int x, y = 0; 6 | GridSize size; 7 | 8 | if(file != NULL) { 9 | // Gets the length of the first line 10 | for(x = 0; (c = fgetc(file)) && (c != '\n' && !feof(file)); x++); 11 | 12 | // Resets file pointer to start of file 13 | rewind(file); 14 | 15 | // Gets the number of lines in the file 16 | while((c = fgetc(file)) && !feof(file)) { 17 | // Save previous character, check for blank newline bug 18 | pc = c; 19 | 20 | // If character is newline, add newline 21 | if(c == '\n') { 22 | y++; 23 | } 24 | } 25 | 26 | // Adds one to the line count 27 | if(x > 0 && pc != '\n') { 28 | y++; 29 | } 30 | } 31 | 32 | size.width = x; 33 | size.height = y; 34 | return size; 35 | } 36 | 37 | // Clears each character in grid to '0' 38 | void clearGrid(char grid[][size.width]) { 39 | int h, w; 40 | 41 | for(h = 0; h < size.height; h++) { 42 | for(w = 0; w < size.width; w++) { 43 | grid[h][w] = '0'; 44 | } 45 | } 46 | } 47 | 48 | // Prints grid characters as they appear 49 | void printGrid(char grid[][size.width]) { 50 | int h, w; 51 | 52 | for(h = 0; h < size.height; h++) { 53 | for(w = 0; w < size.width; w++) { 54 | printf("%c", grid[h][w]); 55 | } 56 | printf("\n"); 57 | } 58 | } 59 | 60 | // Copies one grid to another, used for solvedGrid 61 | void copyGrid(char copy[][size.width], char grid[][size.width]) { 62 | int h, w; 63 | 64 | for(h = 0; h < size.height; h++) { 65 | for(w = 0; w < size.width; w++) { 66 | copy[h][w] = grid[h][w]; 67 | } 68 | } 69 | } 70 | 71 | // Replaces path locations with spaces in solvedGrid 72 | void insertPath(char grid[][size.width], Node path[][size.width]) { 73 | int h, w; 74 | 75 | for(h = 0; h < size.height; h++) { 76 | for(w = 0; w < size.width; w++) { 77 | if(path[h][w].exists == 1) { 78 | // Replaces a path location with a space 79 | grid[h][w] = ' '; 80 | } 81 | } 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /node.c: -------------------------------------------------------------------------------- 1 | // Clears the node to default values 2 | void clearNode(Node *node) { 3 | node->parent = NULL; 4 | node->exists = 0; 5 | node->id = -1; 6 | node->f = -1; 7 | node->g = -1; 8 | node->h = -1; 9 | } 10 | 11 | // Initializes all nodes to default values 12 | void initNodes(Node node[][size.width]) { 13 | int h, w; 14 | for(h = 0; h < size.height; h++) { 15 | for(w = 0; w < size.width; w++) { 16 | clearNode(&node[h][w]); 17 | node[h][w].x = w; 18 | node[h][w].y = h; 19 | } 20 | } 21 | } 22 | 23 | // Prints 1 if Node at that position exists 24 | // Prints 0 if Node at that position does not exist 25 | void nodesExistence(Node node[][size.width]) { 26 | int h, w; 27 | 28 | for(h = 0; h < size.height; h++) { 29 | for(w = 0; w < size.width; w++) { 30 | printf("%d", node[h][w].exists); 31 | } 32 | printf("\n"); 33 | } 34 | } 35 | 36 | // Gets a node (with position) based upon the character on the grid 37 | // Used for finding the start Node and goal Node 38 | Node getNodeChar(char grid[][size.width], char toFind) { 39 | int h, w; 40 | Node node; 41 | clearNode(&node); 42 | 43 | for(h = 0; h < size.width; h++) { 44 | for(w = 0; w < size.width; w++) { 45 | // If char 'toFind' is found, save the position and return node 46 | if(grid[h][w] == toFind) { 47 | node.x = w; 48 | node.y = h; 49 | node.exists = 1; 50 | return node; 51 | } 52 | } 53 | } 54 | return node; 55 | } 56 | 57 | // Finds node with specific id number index and returns it 58 | Node getNode(Node list[][size.width], int findIndex) { 59 | int h, w; 60 | 61 | for(h = 0; h < size.height; h++) { 62 | for(w = 0; w < size.width; w++) { 63 | if(list[h][w].id == findIndex) { 64 | return list[h][w]; 65 | } 66 | } 67 | } 68 | Node fail; 69 | fail.exists = 0; 70 | fail.id = -1; 71 | return fail; 72 | } 73 | 74 | // Copies data from Node orig into parent of Node copy 75 | void setParent(Node *copy, Node orig) { 76 | if(copy->parent == NULL) { 77 | copy->parent = (Node*) malloc(sizeof(Node)); 78 | } 79 | copy->parent->parent = orig.parent; 80 | copy->parent->exists = orig.exists; 81 | copy->parent->x = orig.x; 82 | copy->parent->y = orig.y; 83 | copy->parent->g = orig.g; 84 | copy->parent->h = orig.h; 85 | copy->parent->f = orig.f; 86 | copy->parent->id = orig.id; 87 | } 88 | 89 | // Adds a node to a list by saving it's id, index in the list 90 | void addListNode(Node list[][size.width], int* length, Node node) { 91 | list[node.y][node.x] = node; 92 | list[node.y][node.x].exists = 1; 93 | 94 | // Saves id as a representative of index in list 95 | list[node.y][node.x].id = (*length); 96 | (*length)++; 97 | } 98 | 99 | // Removes a node from a list 100 | void removeListNode(Node list[][size.width], int *length, Node node) { 101 | int h, w; 102 | 103 | // If node exists 104 | if(node.exists == 1) { 105 | // If node is not the last in list 106 | if(node.id != (*length)-1) { 107 | // Iterates through every node in list 108 | for(h = 0; h < size.height; h++) { 109 | for(w = 0; w < size.width; w++) { 110 | // If node has id greater than one being removed 111 | if(list[h][w].id > node.id) { 112 | // Decrement id 113 | list[h][w].id--; 114 | } 115 | } 116 | } 117 | } 118 | // Clear values on the node 119 | clearNode(&list[node.y][node.x]); 120 | 121 | // Free parent from memory 122 | free(list[node.y][node.x].parent); 123 | 124 | // Decrement length 125 | (*length)--; 126 | } 127 | } 128 | 129 | // Arranges original nodes with a valid id number 130 | // from low to high, into 1D array to be sorted 131 | void arrangeID(Node arrange[], Node original[][size.width], int length) { 132 | int a, h, w; 133 | 134 | for(a = 0; a < length; a++) { 135 | for(h = 0; h < size.height; h++) { 136 | for(w = 0; w < size.width; w++) { 137 | // If node id matches the search criteria 138 | if(original[h][w].id == a) { 139 | // Saves node in arrange at index of node id 140 | arrange[a] = original[h][w]; 141 | break; 142 | } 143 | } 144 | } 145 | } 146 | } 147 | 148 | // Reverses Node id's in a grid 149 | void reverseID(Node path[][size.width], int pathLength) { 150 | // s starts at 0, e starts at last index in path 151 | int s, e = pathLength - 1; 152 | Node start, end; 153 | 154 | // s increments while e decrements 155 | for(s = 0; s < e; s++) { 156 | // Gets node at id s and e 157 | start = getNode(path, s); 158 | end = getNode(path, e); 159 | 160 | // Swaps id's of nodes start and end 161 | path[start.y][start.x].id = e; 162 | path[end.y][end.x].id = s; 163 | e--; 164 | } 165 | } 166 | 167 | // Calculates mathematical values on a single open node 168 | void calculateNodeValues(Node parent, Node goal, int possibleX, int possibleY, char grid[][size.width], Node open[][size.width], int* openLength) { 169 | // Creates a new open node (to be looked at) 170 | Node openNode; 171 | clearNode(&openNode); 172 | 173 | // Declaring and initalizing variables used for calculations 174 | int gCost = 0; 175 | int hxDiff = 0; 176 | int hyDiff = 0; 177 | int hCost = 0; 178 | int fCost = 0; 179 | 180 | // Sets coordinates of the open node 181 | openNode.x = possibleX; 182 | openNode.y = possibleY; 183 | 184 | // Sets the parent of the open node 185 | setParent(&openNode, parent); 186 | 187 | // Calculating G cost 188 | // Cost to move from parent node to one open node 189 | gCost = parent.g + 1; 190 | openNode.g = gCost; 191 | 192 | // Calculating H cost 193 | // Estimated cost to move from the open node to the goal node 194 | hxDiff = fabs(goal.x - openNode.x); 195 | hyDiff = fabs(goal.y - openNode.y); 196 | hCost = hxDiff + hyDiff; 197 | openNode.h = hCost; 198 | 199 | // Calculating F cost (combined G and H) 200 | fCost = gCost + hCost; 201 | openNode.f = fCost; 202 | 203 | // Add the open node to the open list 204 | addListNode(open, openLength, openNode); 205 | } 206 | 207 | // Prints information about one node 208 | void info(Node node) { 209 | if(node.parent != NULL) { 210 | printf("info.. %d, %d. f: %d. id: %d. E: %d ", node.x, node.y, node.f, node.id, node.exists); 211 | printf("//// Parent: %d, %d. f: %d. id: %d. E: %d\n", node.parent->x, node.parent->y, node.parent->f, node.parent->id, node.parent->exists); 212 | } 213 | else { 214 | printf("info.. %d, %d. f: %d. id: %d. E: %d //// Parent = NULL\n", node.x, node.y, node.f, node.id, node.exists); 215 | } 216 | } 217 | 218 | // Prints information of a one-dimensional array of nodes 219 | void info1D(Node nodes[], int length) { 220 | int h; 221 | for(h = 0; h < length; h++) { 222 | if(nodes[h].exists == 1) { 223 | info(nodes[h]); 224 | } 225 | } 226 | } 227 | 228 | // Prints information of a two-dimensional (grid) array of nodes 229 | void info2D(Node nodes[][size.width], int length) { 230 | int a, h, w; 231 | 232 | for(a = 0; a < length; a++) { 233 | for(h = 0; h < size.height; h++) { 234 | for(w = 0; w < size.width; w++) { 235 | if(nodes[h][w].id == a) { 236 | info(nodes[h][w]); 237 | break; 238 | } 239 | } 240 | } 241 | } 242 | } 243 | -------------------------------------------------------------------------------- /path.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include"structs.c" 7 | #include"funcDefns.h" 8 | #include"grid.c" 9 | #include"file.c" 10 | #include"node.c" 11 | #define PI_2 3.14159/2 12 | 13 | /* path.c 14 | This program reads maze data from a file, solves 15 | the shortest path, and outputs the solution to a new file. 16 | Currently the algorithm uses four directions (90 degrees) 17 | but can be easily modified to support any angles since 18 | the node generation is based upon sin and cos functions 19 | by Devon Crawford (dcrawf04@mail.uoguelph.ca) 20 | 0973673 21 | Dec 2. 2016 22 | */ 23 | 24 | int main(int argc, char* argv[]) { 25 | // Attempts to open file 26 | FILE *file = fopen(argv[1], "r"); 27 | int gridError = 0; 28 | 29 | // If file exists 30 | if(file != NULL) { 31 | // Getting the size of the grid from file 32 | size = getGridSize(file); 33 | 34 | // Declaring grid arrays based on pre calculated size 35 | char grid[size.height][size.width]; 36 | char solvedGrid[size.height][size.width]; 37 | 38 | // Initializes grid 39 | clearGrid(grid); 40 | 41 | // Reads file into grid and detects any errors 42 | gridError = readFile(file, grid); 43 | 44 | // If there is no error reading grid from file 45 | if(gridError == 0) { 46 | struct timeb startTime, endTime; 47 | int diff; 48 | ftime(&startTime); 49 | 50 | // Makes a copy of the grid for the solution 51 | copyGrid(solvedGrid, grid); 52 | 53 | // Prints the original map 54 | printf("=== Original Grid ===\n"); 55 | printGrid(grid); 56 | 57 | // Declaring Node arrays 58 | Node open[size.height][size.width]; 59 | Node closed[size.height][size.width]; 60 | Node path[size.height][size.width]; 61 | 62 | // Initializing Node arrays 63 | initNodes(open); 64 | initNodes(closed); 65 | initNodes(path); 66 | 67 | // Declaring and initializing lengths for Node arrays 68 | int openLength = 0; 69 | int closedLength = 0; 70 | int pathLength = 0; 71 | 72 | // Gets the node of start and goal 73 | Node start = getNodeChar(grid, 'S'); 74 | Node goal = getNodeChar(grid, 'G'); 75 | 76 | // If start node was not found 77 | if(start.exists == 0) { 78 | printf("Error: Start node was not found.\n"); 79 | } 80 | // If goal node was not found 81 | if(goal.exists == 0) { 82 | printf("Error: Goal node was not found.\n"); 83 | } 84 | 85 | // If start and goal nodes both exist 86 | if(start.exists == 1 && goal.exists == 1) { 87 | // Add start node to the closed list 88 | addListNode(closed, &closedLength, start); 89 | 90 | findPath(grid, open, closed, path, start, goal, start, &openLength, &closedLength, &pathLength); 91 | 92 | // If path was found.. 93 | if(pathLength > 0) { 94 | // Inserts spaces into solvedGrid 95 | insertPath(solvedGrid, path); 96 | 97 | // Recording endtime, Calculating runtime (diff) 98 | ftime(&endTime); 99 | diff = (int) (1000.0 * (endTime.time - startTime.time) + 100 | (endTime.millitm - startTime.millitm)); 101 | 102 | // Prints completion time 103 | printf("\nSOLVED.. in %u milliseconds\n", diff); 104 | 105 | // Prints solved grid 106 | printGrid(solvedGrid); 107 | 108 | // Writes solution file 109 | writeFile(solvedGrid, argv[1]); 110 | } 111 | } 112 | } 113 | } 114 | // If file has any errors 115 | else { 116 | if(argc == 1) { 117 | printf("Error: No maze file was entered in command line. Correct input: \"programName mazeName.txt\"\n"); 118 | } 119 | else { 120 | printf("Error: \"%s\" does not exist.\n", argv[1]); 121 | } 122 | } 123 | return 0; 124 | } 125 | 126 | // Returns node with lowest F value 127 | Node lowestF(Node open[][size.width], int length) { 128 | // Copies open nodes to new 1D array for sorting 129 | Node sort[length]; 130 | arrangeID(sort, open, length); 131 | 132 | int Switch = -1; 133 | Node curr, next; 134 | int i; 135 | 136 | // Sorts open array from low to high 137 | while(Switch != 0) { 138 | Switch = 0; 139 | 140 | // Iterates each existing open node (length) 141 | for(i = 0; i < length - 1; i++) { 142 | curr = sort[i]; 143 | next = sort[i + 1]; 144 | 145 | // SWAP: if curr Node has greater f than next Node 146 | if(curr.f > next.f) { 147 | sort[i] = next; 148 | sort[i + 1] = curr; 149 | Switch = 1; 150 | } 151 | } 152 | } 153 | // Returns node with lowest f 154 | return sort[0]; 155 | } 156 | 157 | // Main recursive function to find open nodes, new parent, and repeat 158 | void findPath(char grid[][size.width], Node open[][size.width], Node closed[][size.width], Node path[][size.width], Node start, Node goal, Node parent, int* openLength, int* closedLength, int* pathLength) { 159 | int i; 160 | // Checks all 4 directions 161 | for(i = 0; i < 4; i++) { 162 | // Uses cosine and sine functions to get circle of points (at 90 degree intervals) around parent 163 | int possibleX = parent.x + round(-1 * cos(PI_2 * i)); 164 | int possibleY = parent.y + round(-1 * sin(PI_2 * i)); 165 | 166 | // If coordinates are outside of boundaries 167 | if(possibleX < 0 || possibleY < 0 || possibleX > (size.width - 1) || possibleY > (size.height - 1)) { 168 | continue; 169 | } 170 | 171 | // Does not make a new open node IF 172 | // Node is a border, node is already in the open list 173 | // Or the node is already in the closed list 174 | if(grid[possibleY][possibleX] == '#' || closed[possibleY][possibleX].exists == 1 || open[possibleY][possibleX].exists == 1) { 175 | continue; 176 | } 177 | // Create an open node, calculate the values and add it to the open list 178 | calculateNodeValues(parent, goal, possibleX, possibleY, grid, open, openLength); 179 | } 180 | // Sets new parent as openNode with the lowest F cost 181 | parent = lowestF(open, *openLength); 182 | 183 | // If chosen parent does not exist.. 184 | // Must be no path, exhausted entire map 185 | if(parent.exists != 1) { 186 | // Inform user that no path was found 187 | printf("\n!! No path exists !!\n"); 188 | 189 | // Breaks out of entire pathfinding functions 190 | return; 191 | } 192 | // If parent node is the goal! 193 | else if(grid[parent.y][parent.x] == 'G') { 194 | // Get the last node in the closed list 195 | Node lastClosed = getNode(closed, (*closedLength)-1); 196 | 197 | // Connect path by tracing back parents, starting at lastClosed Node 198 | connectPath(path, pathLength, lastClosed); 199 | return; 200 | } 201 | 202 | // Remove parent from open list 203 | removeListNode(open, openLength, parent); 204 | 205 | // Adds parent to closed list 206 | addListNode(closed, closedLength, parent); 207 | 208 | // Recursive call to function, replacing parent as the open node with lowest fCost 209 | findPath(grid, open, closed, path, start, goal, parent, openLength, closedLength, pathLength); 210 | } 211 | 212 | // Parent pointers are followed from goal until it reaches start 213 | void connectPath(Node path[][size.width], int *pathLength, Node parent) { 214 | while(parent.parent != NULL) { 215 | // Adds parent to the path list 216 | addListNode(path, pathLength, parent); 217 | 218 | // Parent Node is overwritten by parent Node of itself 219 | parent = *(parent.parent); 220 | } 221 | // Flips id's in path because they were recorded in reverse 222 | reverseID(path, *pathLength); 223 | } 224 | -------------------------------------------------------------------------------- /release-v1.0/c-pathfinding.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DevonCrawford/A-Pathfinding-in-C-command-line/5f80075108b8dd20852286a4e176bc527b491473/release-v1.0/c-pathfinding.exe -------------------------------------------------------------------------------- /release-v1.0/maze.txt: -------------------------------------------------------------------------------- 1 | #S############### 2 | #OOO#O#OOOOO#OOO# 3 | #O#O#O#O###O###O# 4 | #O#OOO#O#O#O#OOO# 5 | #O#######O#O#O### 6 | #OOO#OOO#OOO#OOO# 7 | #O#O#O#O#O#####O# 8 | #O#OOO#OOOOOOOOOG 9 | ################# 10 | -------------------------------------------------------------------------------- /release-v1.0/sampleMaze2.txt: -------------------------------------------------------------------------------- 1 | ###S################# 2 | #O#OOOOOOOOO#OOOOO#O# 3 | #O#O###O#####O#O#O#O# 4 | #OOO#O#OOOOO#O#O#OOO# 5 | #O###O#####O#O#O###O# 6 | #O#OOOOO#OOO#O#O#OOO# 7 | #O###O#O#O###O#O##### 8 | #OOO#O#O#O#OOO#OOOOO# 9 | ###O###O#O#O#####O#O# 10 | #O#OOO#O#O#O#OOO#O#O# 11 | #O###O#O#O#O###O#O#O# 12 | #OOO#OOO#OOOOO#O#O#O# 13 | #O#####O#######O###O# 14 | #OOO#OOO#OOO#OOO#OOO# 15 | #O#O#O###O#O#O###O### 16 | #O#OOO#OOO#O#O#OOOOO# 17 | #O#######O###O#####O# 18 | #OOO#OOOOO#OOO#OOO#O# 19 | ###O#O###O#O###O#O#O# 20 | #OOOOO#OOO#OOOOO#OOO# 21 | ###############G##### -------------------------------------------------------------------------------- /structs.c: -------------------------------------------------------------------------------- 1 | // Creating GridSize struct 2 | typedef struct GridSize { 3 | int width; 4 | int height; 5 | } GridSize; 6 | 7 | // Creating Node struct 8 | typedef struct Node { 9 | struct Node* parent; 10 | int exists; 11 | int x; 12 | int y; 13 | int g; 14 | int h; 15 | int f; 16 | int id; 17 | } Node; 18 | 19 | GridSize size; 20 | GridSize getGridSize(FILE *file); 21 | --------------------------------------------------------------------------------