寻路算法c语言,A*寻路算法C 实现

分为开列表和关列表，

步骤如下：

1.开始搜索，将起始节点压入开队列中，队列不为空时，从开队列中取出一个节点，寻找开始列表里代价最小的一个作为当前节点。

判断该点是否是终点，如果是的话，结束，

2.将当前点压入关闭列表，同时移出开队列，

3.遍历当前节点的8领域内的节点，首先判断是否是墙或在关闭队列里面，

如果不在，重新计算代价G和H，判断是否在开列表，

如果在开队列中，且此次代价小于之前的，则更新代价值，并把父节点指向当前节点

如果不在，则新构造一个节点，包括父节点，代价值G，H，压入开列表。

下面看代码，

首先是数据的定义，点的定义：struct Point { int x, y; Point(int x_ = 0, int y_ = 0) { x = x_; y = y_; }; };

节点的定义：struct Node { int G, H; Point coordinates_; Node *parent_; vector direction = { {-1,0},{1,0},{0,-1},{0,1}, {-1,-1},{-1,1},{1,-1},{1,1} }; Node(Point coordinates, Node *parent = nullptr) { coordinates_.x = coordinates.x; coordinates_.y = coordinates.y; parent_ = parent; G = 0; H = 0; }; int Get_score() { return G H; }; };

图的定义：class Map { public: vector NodeSet; vector walls; Point size_; int directions; void Set_diag_move(bool enable) { directions = (enable ? 8 : 4); }; void Set_size(Point size) { size_ = size; }; void addCollision(Point coordinates); void removeCollision(Point coordinates); void clearCollisions(); void releaseNodes(vector nodes); Node* findNodeOnList(vector nodes_, Point coordinates_); Point getDelta(Point source, Point target); int manhattan(Point source, Point target); bool Detect_collision(Point coordinates); vector<:point> findPath(Point source, Point target); };

A_star.c如下：#include "A_star.h"#include #include using namespace std;void A_star::Map::addCollision(Point coordinates)//添加墙{ walls.push_back(coordinates);}void A_star::Map::removeCollision(Point coordinates)//删除墙{ vector::iterator it = walls.begin(); while(it != walls.end()) { if (coordinates.x == (*it).x && coordinates.y == (*it).y) { walls.erase(it); } it ; }}void A_star::Map::clearCollisions()//清除墙{ walls.clear();}A_star::Point A_star::Map::getDelta(Point source, Point target){ return{ abs(source.x - target.x), abs(source.y - target.y) };}int A_star::Map::manhattan(Point source, Point target)//曼哈顿距离{ //auto delta = std::move(getDelta(source, target));//以非常简单的方式将左值引用转换为右值引用，不懂为什么这么做 Point delta = getDelta(source, target); return static_cast(10 * (delta.x delta.y));}bool A_star::Map::Detect_collision(Point coordinates)//是否是墙{ if (coordinates.x >= 0 && coordinates.x <= size_.x && coordinates.y >= 0 && coordinates.y <= size_.y) { vector::iterator it = walls.begin(); while (it != walls.end()) { if (coordinates.x == (*it).x && coordinates.y == (*it).y) { return true; } it ; } return false; } else return true;}void A_star::Map::releaseNodes(vector nodes)//移除节点{ for (auto it = nodes.begin(); it != nodes.end();) { delete *it; it = nodes.erase(it); }}A_star::Node* A_star::Map::findNodeOnList(vector nodes, Point coordinates)//判断是否 在列表中{ for (auto node : nodes) { //Point s = node->coordinates_; if (node->coordinates_.x == coordinates.x && node->coordinates_.y == coordinates.y) { return node; } } return nullptr;}vector<:point> A_star::Map::findPath(Point source, Point target)//寻路{ vector path; if(Detect_collision(source) || Detect_collision(target))//判断起点和终点是否是墙 { cout << "please input orgin and target again" << endl; return path; } Node *current = nullptr; vector<:node> openSet, closedSet; openSet.push_back(new Node(source)); while (!openSet.empty()) { current = *openSet.begin(); for (auto node : openSet) { if (node->Get_score() <= current->Get_score()) { current = node; } } if (current->coordinates_.x == target.x && current->coordinates_.y == target.y) { break; } closedSet.push_back(current); openSet.erase(std::find(openSet.begin(), openSet.end(), current)); for (int i = 0; i < directions; i) { Point newCoordinates((current->coordinates_.x current->direction[i].x),(current->coordinates_.y current->direction[i].y)); if (Detect_collision(newCoordinates) || findNodeOnList(closedSet, newCoordinates)) {//判断墙和是否在关闭队列里面 continue; } int totalCost = current->G ((i < 4) ? 10 : 14); Node *successor = findNodeOnList(openSet, newCoordinates); if (successor == nullptr) {//不在开队列里面 successor = new Node(newCoordinates, current); successor->G = totalCost; successor->H = manhattan(successor->coordinates_, target); openSet.push_back(successor); } else if (totalCost < successor->G) {//已经在开队列里了 successor->parent_ = current; successor->G = totalCost; } } } while (current != nullptr) { path.push_back(current->coordinates_); current = current->parent_; } releaseNodes(openSet); releaseNodes(closedSet); return path;}

main.cpp#include "A_star.h"#include using namespace A_star;#define Max_X 10#define Max_Y 10void printMap(char map[Max_X][Max_Y], int width, int height){ for (int i = 0; i path; path = Map_.findPath({ 0, 0 }, { 4, 1}); for (auto& coordinate : path) { std::cout << coordinate.x << " " << coordinate.y << "\n"; } system("pause");}

来源：https://www.icode9.com/content-1-285201.html