#include <iostream>
#include <algorithm>
#include <queue>
#include <vector>
#include <cmath>
#define N 6 // 棋盘/迷宫 的阶数
using namespace std;
class Node {
public:
int x, y; // 节点坐标
//f=g+h,总代价；
//g，上个点到该点的代价；
//h,该点到终点的估计值
int f, g, h;
Node(int a, int b) {
this->x = a;
this->y = b;
}
//重载'<'运算符，因为队列不知道怎么对Node排序
bool operator < (const Node& a)const {
//优先队列是大顶堆，因此这里反过来成为小顶堆。Set是小顶堆，如果用Set，这里的判定用'<'
return this->f > ;
}
};
bool visit[N][N]; //记录节点是否访问过
int path[N][N][2]; //记录父节点坐标，用于找路径
int realF[N][N]; //记录节点的实际f值
int addr[N][N]= { {0,0,0,0,0,0},//环境二维矩阵
{0,1,1,0,1,1},
{0,0,1,0,0,0},
{0,0,1,1,1,0},
{0,1,1,0,0,0},
{1,1,0,0,0,0} };
int att[8][2]= { {-1,-1}, {-1, 0}, {-1, 1}, {0, -1},//八个方向
{0, 1}, {1, -1}, {1, 0}, {1, 1} };
priority_queue<Node>que;
void PrintPath(int a, int b); //打印函数
void AStar(int x0, int y0, int x1, int y1); //A星算法部分
bool NodeIsLegal(int x, int y, int x0, int y0); //合理性检验
int Mahuattan(int x, int y, int x1, int y1); //启发性估值，曼哈顿函数
int main() {
//初始化填充
fill(visit[0],visit[0]+N*N,false);
fill(path[0][0], path[0][0] + N * N * 2, -1);
fill(realF[0], realF[0] + N * N, 0);
int x0, y0, x1, y1;
cout << "请输入起点：" << endl;
cin >> x0 >> y0;
cout << "请输入重点：" << endl;
cin >> x1 >> y1;
if (!NodeIsLegal(x0, y0, x0, y0)) {
cout << "输入有误！" << endl;
return 0;
}
AStar(x0, y0, x1, y1);
PrintPath(x1, y1);
return 0;
}
void AStar(int x0, int y0, int x1, int y1) {
//初始化起点
Node node(x0, y0);
= 0;
= Mahuattan(x0, y0, x1, y1);
= + ;
realF[x0][y0] = ;
(node); //加入队列
while (!()) {
//大循环，队列的Top节点即为代价最小点。
Node node_top = que.top();
visit[node_top.x][node_top.y] = true; //设置访问
if (node_top.x == x1 && node_top.y == y1) { //终止条件之一，已经到达
break;
}
();//从待处理队列中删掉
//循环处理八个方向相邻的节点
for (int i = 0; i < 8; i++) {
Node node_next(node_top.x + att[i][0], node_top.y + att[i][1]);
// 该节点坐标合法 且 未加入close表
if (NodeIsLegal(node_next.x, node_next.y, node_top.x, node_top.y)) {
// 计算从起点并经过node_top节点到达该节点所花费的代价
node_next.g = node_top.g + int(10 * (sqrt(pow(att[i][0], 2) + pow(att[i][i], 2))));
// 计算该节点到终点的曼哈顿距离
node_next.h = Mahuattan(node_next.x, node_next.y, x1, y1);
node_next.f = node_next.g + node_next.h;
// node_next.F < valF[node_next.x][node_next.y] 说明找到了更优的路径，则进行更新
// valF[node_next.x][node_next.y] == 0 说明该节点还未加入open表中，则加入
if (node_next.f < realF[node_next.x][node_next.y] || !visit[node_next.x][node_next.y]) {
realF[node_next.x][node_next.y] = node_next.f;
//设置父节点坐标信息
path[node_next.x][node_next.y][0] = node_top.x;
path[node_next.x][node_next.y][1] = node_top.y;
(node_next);//加入队列
}
}
}
}
}
void PrintPath(int x1, int y1) {
if (path[x1][y1][0] == -1 || path[x1][y1][1] == -1) {
cout << "没有可行路径！" << endl;
return;
}
int x = x1, y = y1;
int a, b;
while (x != -1 && y != -1) {
addr[x][y] = 2;// 将可行路径上的节点赋值为2
a = path[x][y][0];
b = path[x][y][1];
x = a;
y = b;
}
// □表示未经过的节点， █表示障碍物， ☆表示可行节点
string s[3] = { "□", "█", "☆" };
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
cout << s[addr[i][j]];
cout << endl;
}
}
bool NodeIsLegal(int x0, int y0, int x1, int y1) {
//边界判断
if (x0<0 || y0<0 || x0>N - 1 || y0>N - 1) return false;
//障碍物判断
if (addr[x0][y0] == 1) return false;
// 两节点成对角型且它们的公共相邻节点存在障碍物 ，就是移动不能穿模
if (x0 != x1 && y0 != y1 && (addr[x0][y1]==1||addr[x1][y0]==1))return false;
return true;
}
int Mahuattan(int x, int y, int x1, int y1) {
return (abs(x - x1) + abs(y - y1))*10;
}