C++之A*算法的简单实现_c++ a*算法

​
​

目录

何为A*算法

伪代码

代码

活动地址：CSDN21天学习挑战赛

何为A*算法

A*算法（启发式搜索）的首要条件是在静态路网中，相对于广度优先遍历（BFS）求最短路径的最有效算法（BFS缺点是效率低耗时久，而A*更高效）

启发式搜索，高薪开发者必会的人工智能寻路算法

　路径规划是指的是机器人的最优路径规划问题，即依据某个或某些优化准则（如工作代价最小、行走路径最短、行走时间最短等），在工作空间中找到一个从起始状态到目标状态能避开障碍物的最优路径。机器人的路径规划应用场景极丰富，最常见如游戏中NPC及控制角色的位置移动，百度地图等导航问题，小到家庭扫地机器人、无人机大到各公司正争相开拓的无人驾驶汽车等。

A*算法原理：

　在计算机科学中，A*算法作为Dijkstra算法的扩展，因其高效性而被广泛应用于寻路及图的遍历，如星际争霸等游戏中就大量使用。在理解算法前，我们需要知道几个概念：

• 搜索区域（The Search Area）：图中的搜索区域被划分为了简单的二维数组，数组每个元素对应一个小方格，当然我们也可以将区域等分成是五角星，矩形等，通常将一个单位的中心点称之为搜索区域节点（Node）。　　
• 开放列表(Open List)：我们将路径规划过程中待检测的节点存放于Open List中，而已检测过的格子则存放于Close List中。
• 父节点（parent）：在路径规划中用于回溯的节点，开发时可考虑为双向链表结构中的父结点指针。
• 路径排序（Path Sorting）：具体往哪个节点移动由以下公式确定：F(n) = G + H 。G代表的是从初始位置A沿着已生成的路径到指定待检测格子的移动开销。H指定待测格子到目标节点B的估计移动开销。
• 启发函数（Heuristics Function）：H为启发函数，也被认为是一种试探，由于在找到唯一路径前，我们不确定在前面会出现什么障碍物，因此用了一种计算H的算法，具体根据实际场景决定。在我们简化的模型中，H采用的是传统的曼哈顿距离（Manhattan Distance），也就是横纵向走的距离之和。

A*算法公式：

F= G + H + W

f:代价
g：已经付出的代价
h：预估代价
w：权重
   就是不讲人话

g：已经付出的代价
    直接累加
h：预估代价
    只算直线距离 并且 无视障碍 即为曼哈顿距离

每走一步 都计算 周围能走的点的F值  并存到数组中 并入树
找出数组中f值最小的点  走上去
循环继续 遇到终点 循环结束   

伪代码

int open_list;//一个记录下所有被考虑来寻找最短路径的格子
int close_list; //一个记录下不会再被考虑的格子
typedef struct point{
	bool Is_Wall;
	struct point* father;//父节点
	int G;// 表示从起点 A 移动到网格上指定方格的移动耗费 (上下左右，还可沿斜方向移动)
	int old_G;//旧G 第一次：从起点 A 直接移动到 A 四周方格的移动耗费 ；上次更新得到的G 
	int new_G; //新G  从起点 A 经过当前搜索中心点到其四周指定点的移动耗费 
	int H;//表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动)
	int F=G+H;//表示该点的总耗费
}Point;
point* start_point;
point* end_point;
point* min_point; 
point* now_point; 

代码

#include <stdio.h>
#include <vector>//动态数组
using namespace std;
 
#define ROWS  10
#define COLS  10
 
//直线代价
#define ZXDJ 10
//斜线代价
#define XXDJ 14
 
enum direct{ p_up, p_down, p_left, p_right,
	p_lup, p_ldown, p_rup, p_rdown };
 
//存下标
struct MyPoint{
	int row;
	int col;
 
	int f;
	int g;
	int h;
};
//树的节点类型
struct treeNode
{
	MyPoint				pos;
	vector<treeNode*>	child;
	treeNode*			pParent;
};
 
//创建树节点
treeNode* createTreeNode(int row, int col);
//计算h值并返回
int getH(MyPoint pos, MyPoint endPos);
 
int main(){
	int map[ROWS][COLS] = {
		{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
		{ 0, 0, 1, 1, 1, 0, 0, 0, 0, 0 },
		{ 0, 1, 0, 1, 1, 0, 0, 0, 0, 0 },
		{ 0, 1, 1, 0, 1, 0, 0, 0, 0, 0 },
		{ 0, 0, 1, 1, 1, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
		{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }
	};
	//标记每个点是否走过
	bool pathMap[ROWS][COLS] = { 0 };//0 没有走过 false     1 true 走过
 
 
	MyPoint begPos = { 1, 1 };
	MyPoint endPos = { 6, 7 };
	//标记起点走过
	pathMap[begPos.row][begPos.col] = true;
 
	//准备一颗树
	treeNode* pRoot = NULL;
 
	//起点入树
	pRoot = createTreeNode(begPos.row, begPos.col);
 
	vector<treeNode*> buff;//准备一个数组
	bool isFindEnd = false;
 
	treeNode* current = pRoot;
 
	vector<treeNode*>::iterator it;
	vector<treeNode*>::iterator itMin;
 
	while (1){
		//1 把当前点周围能走的点找出来
		for (int i = 0; i < 8; i++){
			treeNode* pChild = createTreeNode(current->pos.row,
				current->pos.col);
			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.col--;
				pChild->pos.row--;
				pChild->pos.g += XXDJ;
				break;
			case p_ldown:
				pChild->pos.col--;
				pChild->pos.row++;
				pChild->pos.g += XXDJ;
				break;
			case p_rup:
				pChild->pos.col++;
				pChild->pos.row--;
				pChild->pos.g += XXDJ;
				break;
			case p_rdown:
				pChild->pos.col++;
				pChild->pos.row++;
				pChild->pos.g += XXDJ;
				break;
			}
 
			//2 判断能不能走
			if (map[pChild->pos.row][pChild->pos.col] != 1 && //不是障碍物
				pathMap[pChild->pos.row][pChild->pos.col] == false
				){//能走
				//的计算出f值 入树，存入数组
				pChild->pos.h = getH(pChild->pos, endPos);
				pChild->pos.f = pChild->pos.g + pChild->pos.h;
 
				//入树
				current->child.push_back(pChild);
				pChild->pParent = current;
 
				//存入数组
				buff.push_back(pChild);
			}
			else{//不能走
				//delete pChild;
			}
 
		}
		
		//3 找出数组中f值最小的点，走上去
		itMin = buff.begin();//假设第一个的f值最小
		for (it = buff.begin(); it != buff.end(); it++){
			itMin =( ( (*it)->pos.f < (*itMin)->pos.f ) ? it : itMin );
		}
 
		//走上去
		current = *itMin;
		//标记走过
		pathMap[current->pos.row][current->pos.col] = true;
		//从数组中删掉
		buff.erase(itMin);
 
		//4 判断是否找到终点了
		if (current->pos.row == endPos.row &&
			current->pos.col == endPos.col){
			isFindEnd = true;
			break;
		}
		//5 判断数组是否为空 为空说明都试过了找不到终点
		if (0 == buff.size()) break;
	}
 
	if (isFindEnd){
		printf("找到终点了!\n");
 
		while (current){
			printf("(%d,%d)", current->pos.row, current->pos.col);
			current = current->pParent;
		}
		printf("\n");
	}
 
 
 
 
 
	while (1);
	return 0;
}
 
//创建树节点
treeNode* createTreeNode(int row,int col){
	treeNode* pNew = new treeNode;//开内存
	memset(pNew, 0, sizeof(treeNode));//全部赋值为NULL
	pNew->pos.row = row;
	pNew->pos.col = col;
	return pNew;
}
 
//计算h值并返回
int getH(MyPoint pos, MyPoint endPos){
	int x = ((pos.col>endPos.col)?(pos.col-endPos.col):(endPos.col-pos.col));
	int y = ((pos.row>endPos.row) ? (pos.row - endPos.row) : (endPos.row - pos.row));
 
	return ZXDJ*(x + y);
}

//转载 https://blog.csdn.net/dujuancao11/article/details/109749219?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166081157616780366520908%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=166081157616780366520908&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-109749219-null-null.142^v41^pc_rank_v36,185^v2^control&utm_term=A*%E7%AE%97%E6%B3%95&spm=1018.2226.3001.4187
 
 
 
 
 
import time,sys
from PyQt5.QtWidgets import QDialogButtonBox,QDialog,QMainWindow,QGridLayout,QTextEdit,QLineEdit,QWidget, QMessageBox, QApplication,QLabel,QPushButton,QHBoxLayout,QVBoxLayout
from PyQt5.QtCore import Qt,QTimer,QObject,pyqtSignal,QBasicTimer
from PyQt5.QtGui import QPainter, QColor, QFont,QPen
import json
class config:
	WIDTH=20#地图列数
	HEIGHT=20#地图行数
	blockLength=30#绘制画面时每一个节点方块的边长
class point:#点类（每一个唯一坐标只有对应的一个实例）
	_list=[]#储存所有的point类实例
	_tag=True#标记最新创建的实例是否为_list中的已有的实例，True表示不是已有实例
	def __new__(cls,x,y):#重写new方法实现对于同样的坐标只有唯一的一个实例
		for i in point._list:
			if i.x==x and i.y==y:
				point._tag=False
				return i
		nt=super(point,cls).__new__(cls)
		point._list.append(nt)
		return nt
	def __init__(self,x,y):
		if point._tag:
			self.x=x
			self.y=y
			self.father=None
			self.F=0#当前点的评分  F=G+H
			self.G=0#起点到当前节点所花费的消耗
			self.cost=0#父节点到此节点的消耗
		else:
			point._tag=True
	@classmethod
	def clear(cls):#clear方法，每次搜索结束后，将所有点数据清除，以便进行下一次搜索的时候点数据不会冲突。
		point._list=[]
	def __eq__(self,T):#重写==运算以便实现point类的in运算
		if type(self)==type(T):
			return (self.x,self.y)==(T.x,T.y)
		else:
			return False
	def __str__(self):
		return'(%d,%d)[F=%d,G=%d,cost=%d][father:(%s)]'%(self.x,self.y,self.F,self.G,self.cost,str((self.father.x,self.father.y)) if self.father!=None else 'null')
class A_Search:#核心部分，寻路类
	def __init__(self,arg_start,arg_end,arg_map):
		self.start=arg_start#储存此次搜索的开始点
		self.end=arg_end#储存此次搜索的目的点
		self.Map=arg_map#一个二维数组，为此次搜索的地图引用
		self.open=[]#开放列表：储存即将被搜索的节点
		self.close=[]#关闭列表：储存已经搜索过的节点
		self.result=[]#当计算完成后，将最终得到的路径写入到此属性中
		self.count=0#记录此次搜索所搜索过的节点数
		self.useTime=0#记录此次搜索花费的时间--在此演示中无意义，因为process方法变成了一个逐步处理的生成器，统计时间无意义。
		#开始进行初始数据处理
		self.open.append(arg_start)
	def cal_F(self,loc):
		print('计算值：',loc)
		G=loc.father.G+loc.cost
		H=self.getEstimate(loc)
		F=G+H
		print("F=%d G=%d H=%d"%(F,G,H))
		return {'G':G,'H':H,'F':F}
	def F_Min(self):#搜索open列表中F值最小的点并将其返回，同时判断open列表是否为空，为空则代表搜索失败
		if len(self.open)<=0:
			return None
		t=self.open[0]
		for i in self.open:
			if i.F<t.F:
				t=i
		return t
	def getAroundPoint(self,loc):#获取指定点周围所有可通行的点，并将其对应的移动消耗进行赋值。
		l=[(loc.x,loc.y+1,10),(loc.x+1,loc.y+1,14),(loc.x+1,loc.y,10),(loc.x+1,loc.y-1,14),(loc.x,loc.y-1,10),(loc.x-1,loc.y-1,14),(loc.x-1,loc.y,10),(loc.x-1,loc.y+1,14)]
		for i in l[::-1]:
			if i[0]<0 or i[0]>=config.HEIGHT or i[1]<0 or i[1]>=config.WIDTH:
				l.remove(i)
		nl=[]
		for i in l:
			if self.Map[i[0]][i[1]]==0:
				nt=point(i[0],i[1])
				nt.cost=i[2]
				nl.append(nt)
		return nl
 
	def addToOpen(self,l,father):#此次判断的点周围的可通行点加入到open列表中，如此点已经在open列表中则对其进行判断，如果此次路径得到的F值较之之前的F值更小，则将其父节点更新为此次判断的点，同时更新F、G值。
		for i in l:
			if i not in self.open:
				if i not in self.close:
					i.father=father
					self.open.append(i)
					r=self.cal_F(i)
					i.G=r['G']
					i.F=r['F']
			else:
				tf=i.father
				i.father=father
				r=self.cal_F(i)
				if i.F>r['F']:
					i.G=r['G']
					i.F=r['F']
					# i.father=father
				else:
					i.father=tf
	def getEstimate(self,loc):#H :从点loc移动到终点的预估花费
		return (abs(loc.x-self.end.x)+abs(loc.y-self.end.y))*10
	def DisplayPath(self):#在此演示中无意义
		print('搜索花费的时间:%.2fs.迭代次数%d,路径长度:%d'%(self.useTime,self.count,len(self.result)))
		if self.result!=None:
			for i in self.result:
				self.Map[i.x][i.y]=8
			for i in self.Map:
				for j in i:
					if j==0:
						print('%s'%'□',end='')
					elif j==1:
						print('%s'%'▽',end='')
					elif j==8:
						print('%s'%'★',end='')
				print('')
		else:
			print('搜索失败，无可通行路径')
	def process(self):#使用yield将process方法变成一个生成器，可以逐步的对搜索过程进行处理并返回关键数据
		while True:
			self.count+=1
			tar=self.F_Min()#先获取open列表中F值最低的点tar
			if tar==None:
				self.result=None
				self.count=-1
				break
			else:
				aroundP=self.getAroundPoint(tar)#获取tar周围的可用点列表aroundP
				self.addToOpen(aroundP,tar)#把aroundP加入到open列表中并更新F值以及设定父节点
				self.open.remove(tar)#将tar从open列表中移除
				self.close.append(tar)#已经迭代过的节点tar放入close列表中
				if self.end in self.open:#判断终点是否已经处于open列表中
					e=self.end
					self.result.append(e)
					while True:
						e=e.father
						if e==None:
							break
						self.result.append(e)
					yield (tar,self.open,self.close)
					break
 
			# self.repaint()
			# print('返回')
			yield (tar,self.open,self.close)
			time.sleep(5)#暂停
		self.useTime=time2-time1
class GameBoard(QMainWindow):#可视化类，pyqt5进行编写。
	def __init__(self):
		print('初始化地图...')
		self.Map=[]
		for i in range(config.HEIGHT):
			col=[]
			for j in range(config.WIDTH):
				col.append(0)
			self.Map.append(col)
		self.startPoint=None
		self.endPoint=None
		self.search=None
		self.centerTimer=None
		self.yi=None
		self.special=None
		self.displayFlush=False
		super().__init__()
		print('初始化UI...')
		self.initUI()
	def initUI(self):
		#开始初始化UI部分
			#创建UI控件
		self.label_tips=QLabel("<p style='color:green'>使用说明：</p>右键单击格子选定起始点,左键格子选定格子为墙壁或删除墙壁。\n<p style='color:green'>颜色说明：</p>\n黄色代表起点，绿色代表终点，黑色代表墙壁，红色代表待搜索的open列表，灰色代表已搜索过的close列表，蓝色代表当前搜索到的路径",self)
		self.label_display=QLabel("",self)
		self.button_start=QPushButton("开始搜索",self)
		self.button_clearSE=QPushButton("重选起始点",self)
		self.button_clearWall=QPushButton("清空地图墙壁",self)
		self.button_saveMap=QPushButton("保存地图",self)
		self.button_loadMap=QPushButton("加载地图",self)
 
 
			#设置控件属性
		self.label_tips.setWordWrap(True)
		self.label_display.setWordWrap(True)
			#设置控件样式
		self.label_display.setStyleSheet("border:1px solid black")
		self.label_display.setAlignment(Qt.AlignLeft)
		self.label_display.setAlignment(Qt.AlignTop)
			#设置控件的尺寸和位置
		self.label_tips.resize(200,150)
		self.button_saveMap.resize(80,30)
		self.button_loadMap.resize(80,30)
		self.label_display.resize(200,300)
 
		self.label_tips.move(100+(config.WIDTH-1)*config.blockLength,0)
		self.label_display.move(100+(config.WIDTH-1)*config.blockLength,400)
		self.button_start.move(100+(config.WIDTH-1)*config.blockLength,200)
		self.button_clearSE.move(100+(config.WIDTH-1)*config.blockLength,250)
		self.button_clearWall.move(100+(config.WIDTH-1)*config.blockLength,300)
		self.button_saveMap.move(100+(config.WIDTH-1)*config.blockLength,350)
		self.button_loadMap.move(200+(config.WIDTH-1)*config.blockLength,350)
			#给控件绑定事件
		self.button_start.clicked.connect(self.button_StartEvent)
		self.button_clearSE.clicked.connect(self.button_Clear)
		self.button_clearWall.clicked.connect(self.button_Clear)
		self.button_saveMap.clicked.connect(self.button_SaveMap)
		self.button_loadMap.clicked.connect(self.button_LoadMap)
		#UI初始化完成
		self.setGeometry(0, 0, 150+(config.WIDTH*config.blockLength-config.blockLength)+200, 150+(config.HEIGHT*config.blockLength-config.blockLength))
		self.setMinimumSize(150+(config.WIDTH*config.blockLength-config.blockLength)+200, 150+(config.HEIGHT*config.blockLength-config.blockLength))
		self.setMaximumSize(150+(config.WIDTH*config.blockLength-config.blockLength)+200, 150+(config.HEIGHT*config.blockLength-config.blockLength))
		self.setWindowTitle('A*搜索')
		self.show()
	def addDisplayText(self,text):
		if self.displayFlush:
			self.label_display.setText(text+'\n')
			self.displayFlush=False
		else:
			self.label_display.setText(self.label_display.text()+text+'\n')
	def mousePressEvent(self,event):
		x,y=event.x()-50,event.y()-50
		x=x//config.blockLength
		y=y//config.blockLength
		if x>=0 and x<config.WIDTH and y>=0 and y<config.HEIGHT:
			if event.button()==Qt.LeftButton:
				if (x,y)!=self.startPoint and (x,y)!=self.endPoint:
					self.Map[y][x]=(1 if self.Map[y][x]==0 else 0)
			if event.button()==Qt.RightButton:
				if self.Map[y][x]==0:
					if self.startPoint==None:
						self.startPoint=(x,y)
						self.addDisplayText('添加了一个起点:(%d,%d)'%(x,y))
					elif self.endPoint==None and self.startPoint!=(x,y):
						self.endPoint=(x,y)
						self.addDisplayText('添加了一个终点:(%d,%d)'%(x,y))
			self.repaint()
	def button_StartEvent(self):
		sender=self.sender()
		print(sender)
		if self.startPoint!=None and self.endPoint!=None:
			if self.centerTimer==None:
				self.centerTimer=QBasicTimer()
			self.button_start.setEnabled(False)
			self.button_clearSE.setEnabled(False)
			self.button_clearWall.setEnabled(False)
			self.centerTimer.start(200,self)
			self.search=A_Search(point(self.startPoint[1],self.startPoint[0]),point(self.endPoint[1],self.endPoint[0]),self.Map)
			self.yi=self.search.process()
			self.addDisplayText('开始进行搜索')
	def button_SaveMap(self):
		with open('map.txt','w') as f:
			f.write(json.dumps(self.Map))
			self.addDisplayText('地图保存成功-->map.txt')
		# else:
			# self.addDisplayText('地图保存失败')
	def button_LoadMap(self):
		try:
			with open('map.txt','r') as f:
				self.Map=json.loads(f.read())
				config.HEIGHT=len(self.Map)
				config.WIDTH=len(self.Map[0])
				self.addDisplayText('地图加载成功')
				self.repaint()
		except Exception as e:
			print('失败',e,type(e))
			if type(e)==FileNotFoundError:
				self.addDisplayText('地图加载失败:地图文件不存在')
			elif type(e)==json.decoder.JSONDecodeError:
				self.addDisplayText('地图加载失败:错误的地图文件')
	def button_Clear(self):
		sender=self.sender()
		print(self.button_clearSE,type(self.button_clearSE))
		if sender==self.button_clearSE:
			self.startPoint=None
			self.endPoint=None
			self.repaint()
			self.addDisplayText('清空起始点')
		elif sender==self.button_clearWall:
			for i in range(len(self.Map)):
				for j in range(len(self.Map[i])):
					self.Map[i][j]=0
			self.repaint()
			self.addDisplayText('清空所有墙壁')
	def paintEvent(self, event):
		qp = QPainter()
		qp.begin(self)
		self.drawBoard(event,qp)
		qp.end()
	def drawBoard(self, event, qp):
	    self.drawMap(qp)
	def drawMap(self,qp):#画面绘制方法，每次地图有所改动都将重绘
		time1=time.time()
		if self.search!=None:
			if self.special!=None:
				e=self.special[0]
				path=[e]
				while True:
					e=e.father
					if e!=None:
						path.append(e)
					else:
						break
			else:
				path=None
			pen=QPen(QColor(0,0,0),1,Qt.SolidLine)
			qp.setPen(pen)
			for i in range(len(self.Map)):
				for j in range(len(self.Map[i])):
					wordTag=False
					if i==self.search.start.x and j==self.search.start.y:
						qp.setBrush(QColor(255,255,0))
					elif i==self.search.end.x and j==self.search.end.y:
						qp.setBrush(QColor(100,200,50))
					else:
						if self.Map[i][j]==0:
							tagx=True
							if path:
								for k in path:
									if k.x==i and k.y==j:
										tagx=False
										qp.setBrush(QColor(0,100,255))
							if tagx:
								if self.special!=None:
									if i==self.special[0].x and j==self.special[0].y:
										qp.setBrush(QColor(0,255,0))
									else:
										tag=True
										for k in self.special[1]:
											if k.x==i and k.y==j:
												tag=False
												wordTag=True
												word=str(k.F)
 
												qp.setBrush(QColor(150,0,0))
												break
											else:
												qp.setBrush(QColor(220,220,220))
										if tag:
											for k in self.special[2]:
												if k.x==i and k.y==j:
													qp.setBrush(QColor(150,150,150))
													break
												else:
													qp.setBrush(QColor(220,220,220))
								else:
									qp.setBrush(QColor(220,220,220))
						elif self.Map[i][j]==1:
							qp.setBrush(QColor(0,0,0))
						else:
							qp.setBrush(QColor(255,0,0))
					qp.drawRect(50+j*config.blockLength,50+i*config.blockLength,config.blockLength,config.blockLength)
					if wordTag:
						qp.setFont(QFont('楷体',5,QFont.Light))
						qp.drawText(50+10+j*config.blockLength,50+10+i*config.blockLength,word)
						wordTag=False
		#time.sleep(20)
		else:
			for i in range(len(self.Map)):
				for j in range(len(self.Map[i])):
					if (j,i)==self.startPoint:
						qp.setBrush(QColor(255,255,0))
					elif (j,i)==self.endPoint:
						qp.setBrush(QColor(100,200,50))
					else:
						if self.Map[i][j]==0:
							qp.setBrush(QColor(220,220,220))
						elif self.Map[i][j]==1:
							qp.setBrush(QColor(0,0,0))
						else:
							qp.setBrush(QColor(255,0,0))
 
					qp.drawRect(50+j*config.blockLength,50+i*config.blockLength,config.blockLength,config.blockLength)
		time2=time.time()
	#time.sleep(20)
		# print('绘制时间：',time2-time1)
	def timerEvent(self,e):
		try:
			data=next(self.yi)
		except Exception as e:
			self.addDisplayText('搜索结束:')
			print('搜索结束！')
			if self.search.result==None:
				self.addDisplayText('未找到可行路径')
				print('搜索结束！')
			else:
				self.addDisplayText('总计搜索节点数：%d'%self.search.count)
				self.addDisplayText('最终路径长度：%d'%len(self.search.result))
			self.centerTimer.stop()
			self.search=None
			self.yi=None
			self.special=None
			point.clear()
			self.button_start.setEnabled(True)
			self.button_clearSE.setEnabled(True)
			self.button_clearWall.setEnabled(True)
			self.displayFlush=True
		else:
			self.special=data
			self.repaint()
if __name__ == '__main__':
	app = QApplication(sys.argv)
	ex = GameBoard()
	sys.exit(app.exec_())

python

import pygame
import sys
import math
from tkinter import *
from tkinter import ttk
from tkinter import messagebox
import os
 
screen = pygame.display.set_mode((800,800))
 
class spot:
    def __init__(self,x,y):
        #坐标
        self.i = x
        self.j = y
        #分数
        self.f = 0
        self.g = 0
        self.h = 0
        #父节点
        self.previous = None
        #是不是障碍
        self.obs = False
        #选择状态
        self.closed = False
        #权
        self.value = 1
        #相邻节点
        self.neighbors = []
 
    def show(self,color,st):
        if self.closed == False:
            pygame.draw.rect(screen,color,(self.i*w,self.j*h,w,h),st)
            pygame.display.update()
    
    def path(self,color,st):
            pygame.draw.rect(screen,color,(self.i*w,self.j*h,w,h),st)
            pygame.display.update()
 
    def addNeighbors(self):
        i = self.i
        j = self.j
        if i < rows-1 and grid[i+1][j].obs == False:
            self.neighbors.append(grid[i+1][j])
        if i > 0 and grid[i-1][j].obs == False:
            self.neighbors.append(grid[i-1][j])
        if j < cols-1 and grid[i][j+1].obs == False:
            self.neighbors.append(grid[i][j+1])
        if j > 0 and grid[i][j-1].obs == False:
            self.neighbors.append(grid[i][j-1])
 
#行列数
cols = 50
rows = 50
 
 
 
#颜色
red = (255,0,0)
green = (0,255,0)
blue = (0,0,255)
grey = (220,220,220)
 
#格子长宽
w = 800/cols
h = 800/rows
 
#父节点列表
cameFrom = []
 
#创建节点
grid = [0 for i in range(cols)]
for i in range(cols):
    grid[i] = [0 for i in range(rows)]
 
for i in range(rows):
    for j in range(cols):
        grid[i][j] = spot(i,j)
 
#默认起点、终点
start = grid[5][5]
end = grid[7][19]
 
 
 
#画界面
for i in range(rows):
    for j in range(cols):
        grid[i][j].show((255,255,255),1)
 
#画围墙 
for i in range(rows):
    grid[i][0].show(grey,0)
    grid[i][cols-1].show(grey,0)
    grid[0][i].show(grey,0)
    grid[cols-1][i].show(grey,0)
    grid[i][0].show(grey,0)
    grid[i][cols-1].show(grey,0)
    grid[0][i].show(grey,0)
    grid[cols-1][i].show(grey,0)
    grid[i][0].obs = True
    grid[i][cols-1].obs = True
    grid[0][i].obs = True
    grid[cols-1][i].obs = True
    grid[i][0].obs = True
    grid[i][cols-1].obs = True
    grid[0][i].obs = True
    grid[cols-1][i].obs = True
 
def onsubmit():
    global start
    global end
    st = startBox.get().split(',')
    ed = endBox.get().split(',')
    start = grid[int(st[0])][int(st[1])]
    end = grid[int(ed[0])][int(ed[1])]
    window.quit()
    window.destroy()
 
#输入界面
window = Tk()
window.title('请输入')
label_1 = Label(window,text = '起点坐标(x,y): ')
startBox = Entry(window)
label_2 = Label(window,text = '终点坐标(x,y): ')
endBox = Entry(window)
var = IntVar()
showPath = ttk.Checkbutton(window,text = '显示每一步',onvalue=1,offvalue=0,variable=var)
submit = Button(window,text='提交',command=onsubmit)
 
#布局
label_1.grid(row = 0,column = 0,pady = 3)
label_2.grid(row = 1,column = 0,pady = 3)
startBox.grid(row = 0,column = 1,pady = 3)
endBox.grid(row = 1,column = 1,pady = 3)
showPath.grid(columnspan = 2,row = 2)
submit.grid(columnspan = 2,row = 3)
 
#启动输入界面
mainloop()
 
#两个表
openSet = [start]
closeSet = []
 
#显示起点终点
start.show((255,8,127),0)
end.show((255,8,127),0)
 
#监听鼠标位置
def mousePress(x):
    t = x[0]
    w = x[1]
    #判断在第几个格子
    g1 = t//(800//cols)
    g2 = w//(800//rows)
    #设置障碍
    set_obs = grid[g1][g2]
    if set_obs != start and set_obs!= end:
        set_obs.obs = True
        set_obs.show(grey,0)
 
#画障碍
loop = True
while loop:
    ev = pygame.event.poll()
    if pygame.mouse.get_pressed()[0]:
        try:
            pos = pygame.mouse.get_pos()
            mousePress(pos)
        except AttributeError:
            pass
    if ev.type == pygame.QUIT:
        pygame.quit()
    elif ev.type == pygame.KEYDOWN:
        if ev.key == pygame.K_SPACE:
            loop = False
 
#画好障碍后，初始邻接节点列表
for i in range(rows):
    for j in range(cols):
        grid[i][j].addNeighbors()
 
#启发式方法
def heurisitic(n,e):
    d = math.sqrt((n.i - e.i)**2 + (n.j - e.j)**2)
    return d
 
def main():
    #openSet初始化时已经包含起点
    #从中选择f分数最小的
    if(len(openSet) > 0):
        lowestIndex = 0
        for i in range(len(openSet)):
            if(openSet[i].f < openSet[lowestIndex].f):
                lowestIndex = i
    
    #对当前节点操作
        current = openSet[lowestIndex]
        #找到 打印路径
        if current == end:
            temp = current.f
            while current != start:
                current.closed = False
                current.show(blue, 0)
                current = current.previous
            end.show(red, 0)
 
            Tk().wm_withdraw()
            result = messagebox.askokcancel('Program Finished', ('The program finished, the shortest distance \n to the path is ' + str(temp) + ' blocks away, \n would you like to re run the program?'))
            if result == True:
                os.execl(sys.executable,sys.executable, *sys.argv)
            else:
                ag = True
                while ag:
                    ev = pygame.event.get()
                    for event in ev:
                        if event.type == pygame.KEYDOWN:
                            ag = False
                            break
            pygame.quit()
    
        openSet.pop(lowestIndex)
        closeSet.append(current)
 
        neighbors = current.neighbors
        for i in range(len(neighbors)):
            neighbor = neighbors[i]
            if neighbor not in closeSet:
                tmpG = current.g + current.value
                if neighbor in openSet:
                    if neighbor.g > tmpG:
                        neighbor.g = tmpG
                        neighbor.previous = current
                else:
                    neighbor.g = tmpG
                    openSet.append(neighbor)
                    neighbor.previous = current
 
            neighbor.h = heurisitic(neighbor,end)
            neighbor.f = neighbor.g + neighbor.h
 
    if var.get():
        for i in range(len(openSet)):
            openSet[i].show(green,0)
        
        for i in range(len(closeSet)):
            if closeSet[i] != start:
                closeSet[i].show(red,0)
    current.closed = True
    
 
 
while True:
    ev = pygame.event.poll()
    if ev.type == pygame.QUIT:
        pygame.quit()
    main()
 

鼠标左键长按添加障碍，空格开始寻路