python模拟实现A*寻路算法_python 脚本利用a星寻路

一、简介
两点之间寻找最短路径，要考虑到存在障碍物遮挡和斜线移动的情况。

二、具体说明
说明可以参考下面的链接，对A*算法实现的描述。

三、具体实现
1、实现功能

2、寻路具体流程

3、关于F值
f = g + h
g表示当前移动到下一个点的消耗，平移为1，斜移动为 sqrt((x1-x2）**2 +(y1-y2)**2);
h表示当前移动到终点的消耗，不考虑斜移，不考虑障碍物
具体原理请查看下面的参考链接。

完整实现：

# -*- coding: utf-8 -*-

import os,sys,random,math


#地图设置
gameMapWidth = 10
gameMapHeight = 10
gameMap = []

#地图障碍物
obstacleCount = 5

#块状态
ITEM_STAT_NORMAL = 0        #空点
ITEM_STAT_OBSTACLE = 1      #障碍物
ITEM_STAT_START = 2         #起点
ITEM_STAT_END = 3           #终点

#起点和终点
spNum = -1
epNum = -1

#每块的属性
class Item:
    def __init__(self,x,y,status):
        self.x = x
        self.y = y
        self.status = status
        self.mf = -1
        self.mg = -1
        self.mh = -1
        self.mParent = None
        self.isPath = 0

#初始化地图
def initMap():
    for wc in xrange(gameMapWidth):
        for hc in xrange(gameMapHeight):
            gameMap.append(Item(wc,hc,ITEM_STAT_NORMAL))

    #插入障碍物
    for oc in xrange(obstacleCount):
        choose = random.randint(gameMapWidth,gameMapWidth*gameMapHeight - 1)
        gameMap[choose].status = ITEM_STAT_OBSTACLE
    
    global spNum
    global epNum
    #选取起点和终点
    while (spNum == -1):
        choose = random.randint(0,gameMapWidth*gameMapHeight - 1)
        if gameMap[choose].status == 0:
            spNum = choose
            gameMap[spNum].status = ITEM_STAT_START

    while (epNum == -1):
        choose = random.randint(0,gameMapWidth*gameMapHeight - 1)
        if gameMap[choose].status == 0:
            epNum = choose
            gameMap[epNum].status = ITEM_STAT_END

#输出地图信息
def printMap():
    for itemc in xrange(len(gameMap)):
        if gameMap[itemc].status == ITEM_STAT_START:
            print "START",
        elif gameMap[itemc].status == ITEM_STAT_END:
            print "END  ",
        elif gameMap[itemc].isPath == 1:
            print "path ",
        else:
            print "%d    " %(gameMap[itemc].status),
        
        if (itemc + 1) % gameMapHeight == 0:
            print "\n"


#寻路
def findPath():
    global spNum
    global epNum
    
    
    #开启列表
    openPointList = []
    #关闭列表
    closePointList = []
    
    #开启列表插入起始点
    openPointList.append(gameMap[spNum])
    while (len(openPointList) > 0):
        #寻找开启列表中最小预算值的点
        minFPoint = findPointWithMinF(openPointList)
        #从开启列表移除,添加到关闭列表
        openPointList.remove(minFPoint)
        closePointList.append(minFPoint)
        #找到当前点周围点
        surroundList = findSurroundPoint(minFPoint,closePointList)
        
        #开始寻路
        for sp in surroundList:
            #存在在开启列表，说明上一块查找时并不是最优路径，考虑此次移动是否是最优路径
            if sp in openPointList:
                newPathG = CalcG(sp, minFPoint) #计算新路径下的G值
                if newPathG < sp.mg:
                    sp.mg = newPathG
                    sp.mf = sp.mg + sp.mh
                    sp.mParent = minFPoint
            else:
                sp.mParent = minFPoint      #当前查找到点指向上一个节点
                CalcF(sp, gameMap[epNum])
                openPointList.append(sp)
        if gameMap[epNum] in openPointList:
            gameMap[epNum].mParent = minFPoint
            break

    curp = gameMap[epNum]
    while True:
        curp.isPath = 1
        curp = curp.mParent
        if curp == None:
            break
    print "\n"
    printMap()

def CalcG(point, minp):
    return math.sqrt((point.x - point.mParent.x)**2 + (point.y - point.mParent.y)**2) + minp.mg

#计算每个点的F值
def CalcF(point,endp):
    h = abs(endp.x - point.x) + abs(endp.y - point.y)
    g = 0
    if point.mParent == None:
        g = 0
    else:
        g = point.mParent.mg + math.sqrt((point.x - point.mParent.x)**2 + (point.y - point.mParent.y)**2)
    point.mg = g
    point.mh = h
    point.mf = g + h
    return

#不能是障碍块，不包含在关闭列表中
def notObstacleAndClose(point,closePointList):
    if point not in closePointList and point.status != ITEM_STAT_OBSTACLE:
        return True
    return False

#查找周围块
def findSurroundPoint(point,closePointList):
    surroundList = []
    up = None
    down = None
    left = None
    right = None
    
    leftUp = None
    rightUp = None
    leftDown = None
    rightDown = None
    
    #上面的点存在
    if point.x > 0:
        up = gameMap[gameMapHeight*(point.x - 1) + point.y]
        if notObstacleAndClose(up,closePointList):
            surroundList.append(up)
    #下面的点存在
    if point.x < gameMapWidth - 1:
        down = gameMap[gameMapHeight*(point.x + 1)+ point.y]
        if notObstacleAndClose(down,closePointList):
            surroundList.append(down)
    #左边的点存在
    if point.y > 0:
        left = gameMap[gameMapHeight*(point.x) + point.y - 1]
        if notObstacleAndClose(left, closePointList):
            surroundList.append(left)
    #右边的点存在
    if point.y < gameMapHeight - 1:
        right = gameMap[gameMapHeight*(point.x) + point.y + 1]
        if notObstacleAndClose(right,closePointList):
            surroundList.append(right)
    #斜方向的点还需考虑对应正方向不是障碍物
    #左上角的点存在
    if point.x > 0 and point.y > 0:
        leftUp = gameMap[gameMapHeight*(point.x - 1) + point.y - 1]
        if notObstacleAndClose(leftUp,closePointList) and left.status != ITEM_STAT_OBSTACLE and up.status != ITEM_STAT_OBSTACLE:
            surroundList.append(leftUp)
    #右上角的点存在
    if point.x > 0 and point.y < gameMapHeight - 1:
        rightUp = gameMap[gameMapHeight*(point.x-1) + point.y + 1]
        if notObstacleAndClose(rightUp,closePointList) and right.status != ITEM_STAT_OBSTACLE and up.status != ITEM_STAT_OBSTACLE:
            surroundList.append(rightUp)
    #左下角的点存在
    if point.x < gameMapWidth - 1 and point.y > 0:
        leftDown = gameMap[gameMapHeight*(point.x+1) + point.y - 1]
        if notObstacleAndClose(leftDown,closePointList) and left.status != ITEM_STAT_OBSTACLE and down.status != ITEM_STAT_OBSTACLE:
            surroundList.append(leftDown)
    #右下角的点存在
    if point.x < gameMapWidth - 1 and point.y < gameMapHeight - 1:
        rightDown = gameMap[gameMapHeight*(point.x+1) + point.y + 1]
        if notObstacleAndClose(rightDown,closePointList) and right.status != ITEM_STAT_OBSTACLE and down.status != ITEM_STAT_OBSTACLE:
            surroundList.append(rightDown)
    return surroundList

#查找list中最小的f值
def findPointWithMinF(openPointList):
    f = 0xffffff
    temp = None
    for pc in openPointList:
        if pc.mf < f:
            temp = pc
            f = pc.mf
    return temp


def main():
    initMap()   ##初始化地图
    printMap()  ##输出初始化地图信息
    findPath()  ##查找最优路径

#入口
main()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222

参考：
https://www.gamedev.net/articles/programming/artificial-intelligence/a-pathfinding-for-beginners-r2003/
https://blog.csdn.net/qq_33747722/article/details/78436919