A*算法

公式：F=G+H+W
F是代价函数，G代表从起点到当前点的距离，H代表从当前点到终点的距离，W是权值。
H不考虑障碍物，只计算直线距离（计算横纵距离差）
算法思想：从起始点出发计算周围8个点的F，选择最小的点进行拓展，如果F相等选择|G-H|小的结点，循环操作。若走进死胡同（周围所有点都被访问或者是障碍物）回溯。

#include<stdio.h>
#include<iostream>
#include<vector>
#include<windows.h>
using namespace std;

#define ROWS 9
#define CLOS 11
#define ZXDJ 10
#define XXDJ 14

//点的类型
struct MyPoint {
	int row;
	int col;
	int f;
	int g;
	int h;
	void setF() {
		f = g + h;
	}
	
	bool operator==(const MyPoint& p1) {
		if (this->row == p1.row && this->col == p1.col) {
			return true;
		}else
		return false;
	}
};

//树的节点类型
struct TreeNode {
	MyPoint pos;	//当前点坐标
	vector<TreeNode*> childs;//存储当前点的孩子节点指针数组
	TreeNode* pParent;//存储当前点父节点数组
};

enum direct{p_up,p_down,p_left,p_right,p_lup,p_ldown,p_rup,p_rdown};

//检查是否需要统计
bool needAdd(MyPoint pos, int map[ROWS][CLOS], bool pathMap[ROWS][CLOS]) {
	if (pos.row >= ROWS || pos.row < 0|| pos.col >= CLOS || pos.col < 0) {
		return false;
	}
	if (map[pos.row][pos.col]||pathMap[pos.row][pos.col])
		return false;
	return true;
}

void printMap(int map[ROWS][CLOS],TreeNode* pCurrent,MyPoint endPos) {
	for (int i = 0; i < ROWS; i++) {
		for (int j = 0; j < CLOS; j++) {
			if (i == pCurrent->pos.row && j == pCurrent->pos.col) {
				cout << "人";
			}
			else if (i == endPos.row && j == endPos.col) {
				cout << "终";
			}
			else if (map[i][j] == 0) {
				cout << "口";
			}
			else if(map[i][j]==1) {
				cout << "墙";
			}
		}
		cout << endl;
	}
 }
//计算h值：endPos:终点坐标  pos:当前点坐标
int getH(MyPoint endPos, MyPoint pos) {
	int x = abs(endPos.col-pos.col);
	int y = abs(endPos.row - pos.row);
	return (x + y) * ZXDJ;
}


int main() {
	int map[ROWS][CLOS] = {
		{0,0,0,0,0,1,0,0,0,0,0},
		{0,0,0,1,0,1,0,1,1,1,0},
		{0,0,0,1,0,1,0,0,0,0,0},
		{0,0,0,1,0,1,0,0,0,0,0},
		{0,0,0,0,0,1,0,0,0,0,0},
		{0,1,1,0,0,1,0,0,0,0,0},
		{0,0,1,1,0,1,0,0,0,0,0},
		{0,0,0,0,1,1,0,0,0,0,0},
		{0,0,0,0,0,0,0,0,0,0,0},
	};
	//2 辅助地图 false 0：没有走过  true 1：走过
	bool pathMap[ROWS][CLOS] = { 0 };

	//3.起点 终点
	MyPoint begPos = { 8,8 };
	MyPoint endPos = { 0,8 };

	//4.标记起点走过
	pathMap[begPos.row][begPos.col] = true;

	//5.创建一棵树，起点是树的根节点

	//创建一个新节点
	TreeNode* pNew = new TreeNode;//创建一个节点
	//memset(pNew, 0, sizeof TreeNode);//给节点赋值
	//pNew={0};

	//新节点成为树根
	//pRoot一直指向树的根节点
	TreeNode* pRoot = pNew;

	//新节点是记录起点
	pRoot->pos = begPos;
	pRoot->pParent = NULL;

	//6.动态数组 存储用来比较的节点
	vector<TreeNode*> buff;

	TreeNode* pCurrent = pRoot;
	TreeNode* pChild = NULL;//中间节点

	vector<TreeNode*>::iterator it;
	vector<TreeNode*>::iterator itMin;

	//7.寻路
	while (1) {

		printMap(map, pCurrent, endPos);
		Sleep(300);
		system("cls");



		//7.1找到当前点周围能走的点
		for (int i = 0; i < 8; i++) {
			pChild = new TreeNode;
			//memset(pChild, 0, sizeof TreeNode);
			//pChild = { 0 };

			pChild->pos = pCurrent->pos;
			switch(i) {
			case p_up:
				pChild->pos.row--;
				pChild->pos.g += ZXDJ;
				break;
			case p_down:
				pChild->pos.row++;
				pChild->pos.g += ZXDJ;
				break;
			case p_left:
				pChild->pos.col--;
				pChild->pos.g += ZXDJ;
				break;
			case p_right:
				pChild->pos.col++;
				pChild->pos.g += ZXDJ;
				break;
			case p_lup:
				pChild->pos.row--;
				pChild->pos.col--;
				pChild->pos.g += XXDJ;
				break;
			case p_rup:
				pChild->pos.row--;
				pChild->pos.col++;
				pChild->pos.g += XXDJ;
				break;
			case p_ldown:
				pChild->pos.row++;
				pChild->pos.col--;
				pChild->pos.g += XXDJ;
				break;
			case p_rdown:
				pChild->pos.row++;
				pChild->pos.col++;
				pChild->pos.g += XXDJ;
				break;
			default:
				break;
			}

			/*printf("(%d,%d):%d\n", pChild->pos.row, pChild->pos.col, pChild->pos.g);*/
			
			//7.2计算 g h f值
			pChild->pos.h = getH(endPos, pChild->pos);
			pChild->pos.setF();
			//7.3入树，入buff数组
			if (needAdd(pChild->pos, map, pathMap)) {
				//入树
				pCurrent->childs.push_back(pChild);
				pChild->pParent = pCurrent;
				/*printf("(%d,%d):%d\n", pChild->pos.row, pChild->pos.col, pChild->pos.g);*/
				//入buff数组
				buff.push_back(pChild);
				//标记走过
				pathMap[pChild->pos.row][pChild->pos.col] = true;
			}
			else {
				delete pChild;
			}
		}

		
		//7.4从buff数组中找到f值最小的那个  回溯也是靠这个buff回溯的
		//TreeNode* tmin=*(buff.begin());//迭代器解引用，迭代器相当于一个指针
		itMin = buff.begin();
		int differ = abs((*buff.begin())->pos.g - (*buff.begin())->pos.h);
		for (it = buff.begin();it!=buff.end();it++) {
			itMin = (*itMin)->pos.f > (*it)->pos.f ? it : itMin;

			if ((*itMin)->pos.f > (*it)->pos.f) {
				itMin = it;
			}
			else if ((*itMin)->pos.f > (*it)->pos.f) {
				itMin = itMin;
			}
			else if ((*itMin)->pos.f == (*it)->pos.f) {
				if (abs((*it)->pos.g - (*it)->pos.h) < differ) {
					differ = abs((*it)->pos.g - (*it)->pos.h);
					itMin = it;
				}
			}
		}
		
		//7.5删掉，变化成当前点
		pCurrent = *itMin;
		buff.erase(itMin);
		//7.6 判断是否寻路结束
		if (pCurrent->pos == endPos) {
			cout << "我到终点了！"<<endl;
			while (pCurrent) {
				printf("(%d,%d)", pCurrent->pos.row, pCurrent->pos.col);
				pCurrent = pCurrent->pParent;
			}
			cout << endl;
			break;
		}
		if (buff.empty()) {
			cout << "无路径！" << endl;
			break;
		}

	}

	//end while
	

	//MyPoint p1;
	//p1.row = 1;
	//p1.col = 1;
	//MyPoint p2;
	//p2.row = 0;
	//p2.col = 1;
	//cout << (p1 == p2) << endl;
	return 0;
}