import math wall=tuple([(4,2),(4,3),(4,4),(4,5),(4,6),(4,7)]) #定义墙的坐标 openlist=list() closelist=list() parent=dict() GH=dict() def Probe(sx,sy,dx,dy): closelist.append((sx,sy)) if (sx+1,sy) not in wall and ((sx+1,sy) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx-1)**2 + (dy-sy)**2)) if sum(GH.get((sx+1,sy),(100000,100)))>sum(gh): GH[(sx+1,sy)]=gh openlist.append((sx+1,sy)) parent[(sx+1,sy)]=(sx,sy) if (sx-1,sy) not in wall and ((sx-1,sy) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx+1)**2 + (dy-sy)**2)) if sum(GH.get((sx-1,sy),(100000,100)))>sum(gh): GH[(sx-1,sy)]=gh openlist.append((sx-1,sy)) parent[(sx-1,sy)]=(sx,sy) if (sx,sy+1) not in wall and ((sx,sy+1) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx)**2 + (dy-sy-1)**2)) if sum(GH.get((sx,sy+1),(100000,100)))>sum(gh): GH[(sx,sy+1)]=gh openlist.append((sx,sy+1)) parent[(sx,sy+1)]=(sx,sy) if (sx,sy-1) not in wall and ((sx,sy-1) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx)**2 + (dy-sy+1)**2)) if sum(GH.get((sx,sy-1),(100000,100)))>sum(gh): GH[(sx,sy-1)]=gh openlist.append((sx,sy-1)) parent[(sx,sy-1)]=(sx,sy) if (sx+1,sy+1) not in wall and ((sx+1,sy+1) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx-1)**2 + (dy-sy-1)**2)) if sum(GH.get((sx+1,sy+1),(100000,100)))>sum(gh): GH[(sx+1,sy+1)]=gh openlist.append((sx+1,sy+1)) parent[(sx+1,sy+1)]=(sx,sy) if (sx+1,sy-1) not in wall and ((sx+1,sy-1) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx-1)**2 + (dy-sy+1)**2)) if sum(GH.get((sx+1,sy-1),(100000,100)))>sum(gh): GH[(sx+1,sy-1)]=gh openlist.append((sx+1,sy-1)) parent[(sx+1,sy-1)]=(sx,sy) if (sx-1,sy+1) not in wall and ((sx-1,sy+1) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx+1)**2 + (dy-sy-1)**2)) if sum(GH.get((sx-1,sy+1),(100000,100)))>sum(gh): GH[(sx-1,sy+1)]=gh openlist.append((sx-1,sy+1)) parent[(sx-1,sy+1)]=(sx,sy) if (sx-1,sy-1) not in wall and ((sx-1,sy-1) not in closelist): gh=((GH.get((sx,sy),(1,1)))[0],math.sqrt((dx-sx+1)**2 + (dy-sy+1)**2)) if sum(GH.get((sx-1,sy-1),(100000,100)))>sum(gh): GH[(sx-1,sy-1)]=gh openlist.append((sx-1,sy-1)) parent[(sx-1,sy-1)]=(sx,sy) if (dx,dy) in openlist: return sortlist=sorted(openlist,key=lambda d:sum(GH[d])) sortlist=sortlist[0] openlist.remove(sortlist) Probe(sortlist[0],sortlist[1],dx,dy) def search(sx,sy,dx,dy): if (dx,dy)==(sx,sy) : return search(sx,sy,parent[(dx,dy)][0],parent[(dx,dy)][1]) print('node %d,%d -> %d,%d'%(parent[(dx,dy)][0],parent[(dx,dy)][1],dx,dy)) Probe(1,1,5,4) # 探测路线 search(1,1,5,4) #打印路线
图片形式:橙色代表走的路线