diff --git a/astar b/astar new file mode 100755 index 0000000..5e0a877 Binary files /dev/null and b/astar differ diff --git a/src/astar.cc b/src/astar.cc index 7a756b0..7307451 100644 --- a/src/astar.cc +++ b/src/astar.cc @@ -12,6 +12,7 @@ #include #include #include +#include class Grid { public: @@ -96,6 +97,32 @@ double calculateHValue(Grid grid, const Pair& src) { + pow((src.second - dest.second), 2.0)); } +void tracePath(cell *cellDetails, int num_r, int num_c) { + + cell **cellDetail; + cellDetail = &cellDetails; + std::stack Path; + int row = num_r - 1; + int col = num_c - 1; + std::cout << "row: " << num_r << " and col: " << num_c << std::endl; + Pair next_node = cellDetail[row][col].parent; + std::cout << "wehre we breaking" << std::endl; + Pair third_node = cellDetail[2][2].parent; + std::cout << "(2,2)'s parent: (" << third_node.first << "," << third_node.second << ")" << std::endl; + do { + Path.push(next_node); + row = next_node.first; + col = next_node.second; + } while (cellDetail[row][col].parent != next_node); + + Path.push(Pair(row, col)); + while (!Path.empty()) { + Pair p = Path.top(); + Path.pop(); + std::cout << "-> (" << p.first << "," << p.second << ") "; + } +} + void aSearchAlgorithm(Grid grid) { // 1) Initialize Open List // 2) Initialize the Closed List. Put the starting node @@ -145,6 +172,10 @@ void aSearchAlgorithm(Grid grid) { if (neighbor.first == grid.getRowSize() - 1 && neighbor.second == grid.getColSize() - 1) { std::cout << "Destination cell reached" << std::endl; + cellDetails[neighbor.first][neighbor.second].parent = { r, c }; + tracePath((cell *)cellDetails, + grid.getRowSize(), + grid.getColSize()); return; // ii) else, compute both g and h for successor // successor.g = q.g + distance between successor and q