A*寻路算法

1：公式  F = G + H

2：实现步骤

a:检查起始点PointA、终点PointB是否合法(是否在地图范围内)

b:获取起始点PointA、终点PointB的block 判断是否可达

c:把起始点PointA加入关闭列表中

d:从起始点开始PointA 找周围的点，判断这些点是否是边界、是否是阻挡、是否在开启列表中、是否在关闭列表中 如果都不是才加入到开放列表

e:对开放列表排序，按照F值从小到大排序，并取出F值最小的点PointC

f:从开放列表中删除PointC,把PointC加入到关闭列表中

g:把PointC 当做起始点，重复d e f g操作，直到开放列表为空(没有找到路劲)或者开始点和结束点重合(找到路劲)

class AstarNode(object):

    def __init__(self,index,x,y,block):
        self.index = index
        self.x = x
        self.y = y
        self.block = block
        self.f = 0.0
        self.g = 0.0
        self.h = 0.0
        self.parent = None
    
    def updateInfo(self,g,h,parent):
        self.g = g
        self.h = h
        self.f = g + h
        self.parent = parent

    def __str__(self):
        return "index=%s x=%s y=%s [%s,%s,%s] block=%s" % (self.index,self.x,self.y,self.g,self.h,self.f,self.block)

class AstarMgr(object):

    def __init__(self):
        self.openList = []
        self.closeList = []
        self.nodes = []
        self.mapw = 0
        self.maph = 0
        

    def initMap(self,w,h,blocks):
        self.mapw = w
        self.maph = h
        for i in range(w):
            tmplist = []
            for j in range(h):
                index = i * h + j
                node = AstarNode(index,i,j,blocks[i][j])
                tmplist.append(node)
            self.nodes.append(tmplist)


    def findPath(self,startx,starty,endx,endy):
        resultpath = []
        if startx < 0 or startx >= self.mapw or starty < 0 or starty >= self.maph:
            print("start point error")
            return resultpath
        if endx < 0 or endx >= self.mapw or endy < 0 or endy >= self.maph:
            print("end point error")
            return resultpath
        startNode = self.nodes[startx][starty]
        endNode = self.nodes[endx][endy]

        if startNode.block == 1 or endNode.block == 1:
            print("start or end block")
            return resultpath
        
        self.closeList.clear()
        self.openList.clear()
        startNode.updateInfo(0,0,None)
        self.closeList.append(startNode)
        
        
        while True:
            self.findNearlyNodeToOpenList(startNode.x,    startNode.y + 1,10,startNode,endNode) #上
            self.findNearlyNodeToOpenList(startNode.x + 1,startNode.y + 1,14,startNode,endNode) #右上
            self.findNearlyNodeToOpenList(startNode.x + 1,startNode.y,    10,startNode,endNode) #右
            self.findNearlyNodeToOpenList(startNode.x + 1,startNode.y - 1,14,startNode,endNode) #右下
            self.findNearlyNodeToOpenList(startNode.x,    startNode.y - 1,10,startNode,endNode) #下
            self.findNearlyNodeToOpenList(startNode.x - 1,startNode.y - 1,14,startNode,endNode) #左下
            self.findNearlyNodeToOpenList(startNode.x - 1,startNode.y,    10,startNode,endNode) #左
            self.findNearlyNodeToOpenList(startNode.x - 1,startNode.y + 1,14,startNode,endNode) #左上
            
            if len(self.openList) == 0:
                print("die path")
                return resultpath

            self.openList.sort(key = lambda x:(x.f,x.h))
            for op in self.openList:
                print("sorted openlist:",op)
            
            startNode = self.openList[0]
            print(startNode,"xxxxx")
            self.openList.remove(startNode)
            self.closeList.append(startNode)

            if startNode == endNode:
                print("find over")
                resultpath.append(endNode)
                while endNode.parent:
                    resultpath.append(endNode.parent)
                    endNode = endNode.parent
                resultpath.reverse()
                return resultpath
        

    def findNearlyNodeToOpenList(self,x,y,g,parent,endnode):
        if x < 0 or x >= self.mapw or y < 0 or y >= self.maph:
            return
        node = self.nodes[x][y]
        if node == None or node.block == 1 or node in self.closeList or node in self.openList:
            return
        node.updateInfo(parent.g + g,abs(endnode.x - node.x) + abs(endnode.y - node.y),parent)
        self.openList.append(node)

if __name__ == "__main__":
    blocks = [
        [0,1,0,0,0,0],
        [0,1,1,0,0,0],
        [0,1,1,1,0,0],
        [0,0,1,1,1,0],
        [0,0,0,1,1,1],
        [0,0,0,0,0,0]
    ]
    astar = AstarMgr()
    astar.initMap(6,6,blocks)
    path = astar.findPath(0,0,5,5)
    for point in path:
        print(point)