Untitled
#include <iostream> #include <vector> #include <queue> #include <unordered_set> #include <cmath> #include <unordered_map> struct Node { int x, y; double g, h; Node* parent; Node(int x, int y, double g = 0.0, double h = 0.0, Node* parent = nullptr) : x(x), y(y), g(g), h(h), parent(parent) {} double f() const { return g + h; } }; struct Compare { bool operator()(const Node* a, const Node* b) const { return a->f() > b->f(); } }; double heuristic(int x1, int y1, int x2, int y2) { return std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)); } std::vector<Node*> getNeighbors(Node* node, const std::vector<std::vector<int>>& grid) { std::vector<Node*> neighbors; std::vector<std::pair<int, int>> directions = {{1, 0}, {-1, 0}, {0, 1}, {0, -1}}; int rows = grid.size(), cols = grid[0].size(); for (const auto& dir : directions) { int newX = node->x + dir.first, newY = node->y + dir.second; if (newX >= 0 && newX < rows && newY >= 0 && newY < cols && grid[newX][newY] == 0) { neighbors.push_back(new Node(newX, newY)); } } return neighbors; } std::vector<Node*> reconstructPath(Node* node) { std::vector<Node*> path; while (node) { path.push_back(node); node = node->parent; } std::reverse(path.begin(), path.end()); return path; } std::vector<Node*> aStar(const std::vector<std::vector<int>>& grid, const Node& start, const Node& goal) { std::priority_queue<Node*, std::vector<Node*>, Compare> openList; std::unordered_set<int> closedSet; std::unordered_map<int, Node*> allNodes; auto hashNode = [](const Node* node) { return node->x * 1000 + node->y; }; Node* startNode = new Node(start.x, start.y, 0.0, heuristic(start.x, start.y, goal.x, goal.y)); openList.push(startNode); allNodes[hashNode(startNode)] = startNode; while (!openList.empty()) { Node* current = openList.top(); openList.pop(); if (current->x == goal.x && current->y == goal.y) { return reconstructPath(current); } closedSet.insert(hashNode(current)); for (Node* neighbor : getNeighbors(current, grid)) { int neighborHash = hashNode(neighbor); if (closedSet.find(neighborHash) != closedSet.end()) { delete neighbor; continue; } double tentativeG = current->g + heuristic(current->x, current->y, neighbor->x, neighbor->y); if (allNodes.find(neighborHash) == allNodes.end() || tentativeG < neighbor->g) { neighbor->g = tentativeG; neighbor->h = heuristic(neighbor->x, neighbor->y, goal.x, goal.y); neighbor->parent = current; if (allNodes.find(neighborHash) == allNodes.end()) { openList.push(neighbor); allNodes[neighborHash] = neighbor; } } else { delete neighbor; } } } return {}; } int main() { std::vector<std::vector<int>> grid = { {0, 1, 0, 0, 0}, {0, 1, 0, 1, 0}, {0, 0, 0, 1, 0}, {0, 1, 1, 1, 0}, {0, 0, 0, 0, 0} }; Node start(0, 0); Node goal(4, 4); std::vector<Node*> path = aStar(grid, start, goal); if (!path.empty()) { std::cout << "Path found:\n"; for (const Node* node : path) { std::cout << "(" << node->x << ", " << node->y << ") "; } std::cout << std::endl; } else { std::cout << "No path found." << std::endl; } return 0; }
Leave a Comment