A *伪代码
Search( grid, initial_point, goal_point ) :
1.初始化一个空的打开节点列表 2.使用以下内容初始化起始节点：
- 由initial_point给出的x和y值
- g = 0，其中g是每一步的成本
- h由启发函数（当前坐标和目标的函数）给出
- 将新节点添加到打开的节点列表中
- while 打开节点的列表是非空的：
- 按f值对打开的列表进行排序
- 弹出最佳单元格（称为current单元格）。
- 在路径中将单元格的坐标标记在网格中。
- if current单元格是goal单元格：
- 返回单元格
- else将搜索范围扩展到current节点的邻居。这包括以下步骤：
- 检查网格中的每个相邻单元，以确保该单元为空：它尚未关闭且没有障碍。
- 如果单元格为空，请计算成本（g值）和启发式方法，然后将其添加到打开的节点列表中
- 将单元格标记为关闭。
- 如果由于打开的节点列表为空而退出while循环，则表明您用完了新节点以进行探索，但未找到路径。
#include <algorithm> // for sort
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
using std::cout;
using std::ifstream;
using std::istringstream;
using std::sort;
using std::string;
using std::vector;
using std::abs;
// State classes
enum class State { kEmpty, kObstacle, kClosed, kPath, kStart, kFinish };
// Directional deltas
const int delta[4][2]{ {-1, 0}, {0, -1}, {1, 0}, {0, 1} };
vector<State> ParseLine(string line) {
istringstream sline(line);
int n;
char c;
vector<State> row;
while (sline >> n >> c && c == ',') {
if (n == 0) {
row.push_back(State::kEmpty);
}
else {
row.push_back(State::kObstacle);
}
}
return row;
}
vector<vector<State>> ReadBoardFile(string path) {
ifstream myfile(path);
vector<vector<State>> board{};
if (myfile) {
string line;
while (getline(myfile, line)) {
vector<State> row = ParseLine(line);
board.push_back(row);
}
}
return board;
}
/**
* Compare the F values of two cells.
*/
bool Compare(const vector<int> a, const vector<int> b) {
int f1 = a[2] a[3]; // f1 = g1 h1
int f2 = b[2] b[3]; // f2 = g2 h2
return f1 > f2;
}
/**
* Sort the two-dimensional vector of ints in descending order.
*/
void CellSort(vector<vector<int>>* v) {
sort(v->begin(), v->end(), Compare);
}
// Calculate the manhattan distance
int Heuristic(int x1, int y1, int x2, int y2) {
return abs(x2 - x1) abs(y2 - y1);
}
/**
* Check that a cell is valid: on the grid, not an obstacle, and clear.
*/
bool CheckValidCell(int x, int y, vector<vector<State>>& grid) {
bool on_grid_x = (x >= 0 && x < grid.size());
bool on_grid_y = (y >= 0 && y < grid[0].size());
if (on_grid_x && on_grid_y)
return grid[x][y] == State::kEmpty;
return false;
}
/**
* Add a node to the open list and mark it as open.
*/
void AddToOpen(int x, int y, int g, int h, vector<vector<int>>& openlist, vector<vector<State>>& grid) {
// Add node to open vector, and mark grid cell as closed.
openlist.push_back(vector<int>{x, y, g, h});
grid[x][y] = State::kClosed;
}
/**
* Expand current nodes's neighbors and add them to the open list.
*/
void ExpandNeighbors(const vector<int>& current, int goal[2], vector<vector<int>>& openlist, vector<vector<State>>& grid) {
// Get current node's data.
int x = current[0];
int y = current[1];
int g = current[2];
// Loop through current node's potential neighbors.
for (int i = 0; i < 4; i ) {
int x2 = x delta[i][0];
int y2 = y delta[i][1];
// Check that the potential neighbor's x2 and y2 values are on the grid and not closed.
if (CheckValidCell(x2, y2, grid)) {
// Increment g value and add neighbor to open list.
int g2 = g 1;
int h2 = Heuristic(x2, y2, goal[0], goal[1]);
AddToOpen(x2, y2, g2, h2, openlist, grid);
}
}
}
/**
* Implementation of A* search algorithm
*/
vector<vector<State>> Search(vector<vector<State>> grid, int init[2], int goal[2]) {
// Create the vector of open nodes.
vector<vector<int>> open{};
// Initialize the starting node.
int x = init[0];
int y = init[1];
int g = 0;
int h = Heuristic(x, y, goal[0], goal[1]);
AddToOpen(x, y, g, h, open, grid);
while (open.size() > 0) {
// Get the next node
CellSort(&open);
auto current = open.back();
open.pop_back();
x = current[0];
y = current[1];
grid[x][y] = State::kPath;
// Check if we're done.
if (x == goal[0] && y == goal[1]) {
grid[init[0]][init[1]] = State::kStart;
grid[goal[0]][goal[1]] = State::kFinish;
return grid;
}
// If we're not done, expand search to current node's neighbors.
ExpandNeighbors(current, goal, open, grid);
}
// We've run out of new nodes to explore and haven't found a path.
cout << "No path found!" << "n";
return std::vector<vector<State>>{};
}
string CellString(State cell) {
switch (cell) {
case State::kObstacle: return "山 ";
case State::kPath: return "路径 ";
case State::kStart: return "起点 ";
case State::kFinish: return "终点 ";
default: return "0 ";
}
}
void PrintBoard(const vector<vector<State>> board) {
for (int i = 0; i < board.size(); i ) {
for (int j = 0; j < board[i].size(); j ) {
cout << CellString(board[i][j]);
}
cout << "n";
}
}
//#include "test.cpp"
int main() {
int init[2]{ 0, 0 };
int goal[2]{ 4, 5 };
auto board = ReadBoardFile("1.board");
auto solution = Search(board, init, goal);
PrintBoard(solution);
system("pause");
// Tests
/* TestHeuristic();
TestAddToOpen();
TestCompare();
TestSearch();
TestCheckValidCell();
TestExpandNeighbors();*/
}
