用C++写类名 Y X坐标障碍,用A星寻路代码
时间: 2024-03-13 10:47:59 浏览: 172
以下是用C++写的障碍类和A*寻路算法的示例代码:
障碍类:
```c++
class Obstacle {
public:
Obstacle(int x, int y) : x_(x), y_(y) {}
int x() const { return x_; }
int y() const { return y_; }
private:
int x_;
int y_;
};
```
A*寻路算法:
```c++
#include <queue>
#include <vector>
#include <algorithm>
#include <iostream>
using namespace std;
class Node {
public:
Node(int x, int y, int g, int h, Node* parent) : x_(x), y_(y), g_(g), h_(h), parent_(parent) {}
int x() const { return x_; }
int y() const { return y_; }
int g() const { return g_; }
int h() const { return h_; }
int f() const { return g_ + h_; }
Node* parent() const { return parent_; }
private:
int x_;
int y_;
int g_;
int h_;
Node* parent_;
};
bool operator < (const Node& a, const Node& b) {
return a.f() > b.f();
}
vector<Node*> findPath(int startX, int startY, int endX, int endY, vector<Obstacle>& obstacles) {
priority_queue<Node> openList;
vector<Node*> closedList;
Node* startNode = new Node(startX, startY, 0, abs(endX-startX)+abs(endY-startY), NULL);
openList.push(*startNode);
while (!openList.empty()) {
Node* currentNode = new Node(openList.top().x(), openList.top().y(), openList.top().g(), openList.top().h(), openList.top().parent());
openList.pop();
if (currentNode->x() == endX && currentNode->y() == endY) {
vector<Node*> path;
while (currentNode) {
path.push_back(currentNode);
currentNode = currentNode->parent();
}
return path;
}
closedList.push_back(currentNode);
int x = currentNode->x();
int y = currentNode->y();
int g = currentNode->g();
int h = currentNode->h();
Node* parent = currentNode->parent();
for (int i = -1; i <= 1; ++i) {
for (int j = -1; j <= 1; ++j) {
if (i == 0 && j == 0) continue;
int newX = x + i;
int newY = y + j;
if (newX < 0 || newX >= 100 || newY < 0 || newY >= 100) continue;
bool found = false;
for (auto& obstacle : obstacles) {
if (obstacle.x() == newX && obstacle.y() == newY) {
found = true;
break;
}
}
if (found) continue;
bool inClosedList = false;
for (auto& node : closedList) {
if (node->x() == newX && node->y() == newY) {
inClosedList = true;
break;
}
}
if (inClosedList) continue;
int newG = g + abs(i)+abs(j);
int newH = abs(endX-newX)+abs(endY-newY);
bool inOpenList = false;
for (auto& node : openList) {
if (node.x() == newX && node.y() == newY) {
inOpenList = true;
if (newG < node.g()) {
node = Node(newX, newY, newG, newH, currentNode);
}
break;
}
}
if (!inOpenList) {
openList.push(Node(newX, newY, newG, newH, currentNode));
}
}
}
}
return vector<Node*>();
}
int main() {
vector<Obstacle> obstacles = { Obstacle(3, 3), Obstacle(3, 4), Obstacle(3, 5), Obstacle(4, 5), Obstacle(5, 5) };
vector<Node*> path = findPath(0, 0, 9, 9, obstacles);
if (path.empty()) {
cout << "No path found." << endl;
}
else {
reverse(path.begin(), path.end());
for (auto& node : path) {
cout << "(" << node->x() << "," << node->y() << ") ";
}
cout << endl;
}
return 0;
}
```
这段代码中,我们定义了一个Node类用于表示一个节点,包含了当前节点的x坐标、y坐标、g值(从起点到当前节点的实际代价)、h值(从当前节点到终点的估价),以及一个指向父节点的指针。我们还定义了一个小于号运算符,用于在优先队列中比较节点的f值(f值等于g值加上h值)。
在findPath函数中,我们首先将起点加入到openList中。然后,我们循环执行以下操作:取出openList中f值最小的节点,将其加入到closedList中,检查其周围的8个节点,计算它们的g值、h值和f值,如果它们不在障碍物中、不在closedList中、并且不在openList中或者在openList中但是新的g值更小,那么就将它们加入到openList中。如果openList为空了还没有找到终点,那么说明没有通路,返回空的路径。否则,我们从终点开始,依次沿着每个节点的父节点指针往回走,直到回到起点,就得到了一条路径。
在本例中,我们在100x100的网格上寻路,障碍物的坐标存储在一个vector中,起点为(0,0),终点为(9,9)。这个示例代码并没有考虑障碍物大小,也没有使用任何优化,只是简单的A*寻路算法的基本实现。
阅读全文