基本A*算法python实现

cqlinx

　　本文由恋花蝶发表于http://blog.csdn.net/lanphaday，可以在保证全文完整的前提下任意形式自由传播，但必须保留本声明，违反必究。
　　最近因为一个任务要用到A*算法，就用C++实现了一份。不过只是用A*来检测从A点到B点有无通路，不必输出路径，后来想把代码贴出来，但又觉得不如实现一个简单的寻路应用好一些，就用python写了一个版本贴上来。
A*算法不仅仅可以用来寻路，寻路也不仅仅使用A*算法。这是使用学习和使用A*算法最要谨记的一点吧~
A*算法用以寻路实现算不得是人工智能，他本质上是一种启发式的试探回溯算法，不过业界似乎喜欢把它称为游戏人工智能（GameAI）的一个组成部分，听起来就“豪华”得多了。A*算法需要很大的内存（相对于深度优先搜索），需要很实现比较复杂的逻辑，容易出错。
A*过程：
1.将开始节点放入开放列表(开始节点的F和G值都视为0);
2.重复一下步骤:
i.在开放列表中查找具有最小F值的节点,并把查找到的节点作为当前节点;
ii.把当前节点从开放列表删除, 加入到封闭列表;
iii.对当前节点相邻的每一个节点依次执行以下步骤:
1.如果该相邻节点不可通行或者该相邻节点已经在封闭列表中,则什么操作也不执行,继续检验下一个节点;
2.如果该相邻节点不在开放列表中,则将该节点添加到开放列表中, 并将该相邻节点的父节点设为当前节点,同时保存该相邻节点的G和F值;
3.如果该相邻节点在开放列表中, 则判断若经由当前节点到达该相邻节点的G值是否小于原来保存的G值,若小于,则将该相邻节点的父节点设为当前节点,并重新设置该相邻节点的G和F值.
iv.循环结束条件:
当终点节点被加入到开放列表作为待检验节点时, 表示路径被找到,此时应终止循环;
或者当开放列表为空,表明已无可以添加的新节点,而已检验的节点中没有终点节点则意味着路径无法被找到,此时也结束循环;
3.从终点节点开始沿父节点遍历, 并保存整个遍历到的节点坐标,遍历所得的节点就是最后得到的路径;
　　好了，废话不多说，看代码吧，带详尽注释，但可能存在bug~，另：本示例程序未作优化。
　　参考资料：
http://www.gamedev.net/reference/programming/features/astar/default.asp
　　http://blog.csdn.net/win32asn/archive/2006/03/17/627098.aspx

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

#地图
tm=[
'############################################################',
'#..........................................................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.......S.....................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#.............................#............................#',
'#######.#######################################............#',
'#....#........#............................................#',
'#....#........#............................................#',
'#....##########............................................#',
'#..........................................................#',
'#..........................................................#',
'#..........................................................#',
'#..........................................................#',
'#..........................................................#',
'#...............................##############.............#',
'#...............................#........E...#.............#',
'#...............................#............#.............#',
'#...............................#............#.............#',
'#...............................#............#.............#',
'#...............................###########..#.............#',
'#..........................................................#',
'#..........................................................#',
'############################################################']

#因为python里string不能直接改变某一元素，所以用test_map来存储搜索时的地图
test_map=[]

#########################################################
classNode_Elem:
"""
开放列表和关闭列表的元素类型，parent用来在成功的时候回溯路径
"""
def__init__(self,parent,x,y,dist):
self.parent=parent
self.x=x
self.y=y
self.dist=dist

classA_Star:
"""
A星算法实现类
"""
#注意w,h两个参数，如果你修改了地图，需要传入一个正确值或者修改这里的默认参数
def__init__(self,s_x,s_y,e_x,e_y,w=60,h=30):
self.s_x=s_x
self.s_y=s_y
self.e_x=e_x
self.e_y=e_y

self.width=w
self.height=h

self.open=[]
self.close=[]
self.path=[]

#查找路径的入口函数
deffind_path(self):
#构建开始节点
p=Node_Elem(None,self.s_x,self.s_y,0.0)
whileTrue:
#扩展F值最小的节点
self.extend_round(p)
#如果开放列表为空，则不存在路径，返回
ifnotself.open:
return
#获取F值最小的节点
idx,p=self.get_best()
#找到路径，生成路径，返回
ifself.is_target(p):
self.make_path(p)
return
#把此节点压入关闭列表，并从开放列表里删除
self.close.append(p)
delself.open[idx]

defmake_path(self,p):
#从结束点回溯到开始点，开始点的parent==None
whilep:
self.path.append((p.x,p.y))
p=p.parent

defis_target(self,i):
returni.x==self.e_xandi.y==self.e_y

defget_best(self):
best=None
bv=1000000#如果你修改的地图很大，可能需要修改这个值
bi=-1
foridx,iinenumerate(self.open):
value=self.get_dist(i)#获取F值
ifvalue<bv:#比以前的更好，即F值更小
best=i
bv=value
bi=idx
returnbi,best

defget_dist(self,i):
#F=G+H
#G为已经走过的路径长度，H为估计还要走多远
#这个公式就是A*算法的精华了。
returni.dist+math.sqrt(
(self.e_x-i.x)*(self.e_x-i.x)
+(self.e_y-i.y)*(self.e_y-i.y))*1.2

defextend_round(self,p):
#可以从8个方向走
xs=(-1,0,1,-1,1,-1,0,1)
ys=(-1,-1,-1,0,0,1,1,1)
#只能走上下左右四个方向
#xs=(0,-1,1,0)
#ys=(-1,0,0,1)
forx,yinzip(xs,ys):
new_x,new_y=x+p.x,y+p.y
#无效或者不可行走区域，则勿略
ifnotself.is_valid_coord(new_x,new_y):
continue
#构造新的节点
node=Node_Elem(p,new_x,new_y,p.dist+self.get_cost(
p.x,p.y,new_x,new_y))
#新节点在关闭列表，则忽略
ifself.node_in_close(node):
continue
i=self.node_in_open(node)
ifi!=-1:
#新节点在开放列表
ifself.open.dist>node.dist:
#现在的路径到比以前到这个节点的路径更好~
#则使用现在的路径
self.open.parent=p
self.open.dist=node.dist
continue
self.open.append(node)

defget_cost(self,x1,y1,x2,y2):
"""
上下左右直走，代价为1.0，斜走，代价为1.4
"""
ifx1==x2ory1==y2:
return1.0
return1.4

defnode_in_close(self,node):
foriinself.close:
ifnode.x==i.xandnode.y==i.y:
returnTrue
returnFalse

defnode_in_open(self,node):
fori,ninenumerate(self.open):
ifnode.x==n.xandnode.y==n.y:
returni
return-1

defis_valid_coord(self,x,y):
ifx<0orx>=self.widthory<0ory>=self.height:
returnFalse
returntest_map[y][x]!='#'

defget_searched(self):
l=[]
foriinself.open:
l.append((i.x,i.y))
foriinself.close:
l.append((i.x,i.y))
returnl

#########################################################
defprint_test_map():
"""
打印搜索后的地图
"""
forlineintest_map:
print''.join(line)

defget_start_XY():
returnget_symbol_XY('S')

defget_end_XY():
returnget_symbol_XY('E')

defget_symbol_XY(s):
fory,lineinenumerate(test_map):
try:
x=line.index(s)
except:
continue
else:
break
returnx,y

#########################################################
defmark_path(l):
mark_symbol(l,'*')

defmark_searched(l):
mark_symbol(l,'')

defmark_symbol(l,s):
forx,yinl:
test_map[y][x]=s

defmark_start_end(s_x,s_y,e_x,e_y):
test_map[s_y][s_x]='S'
test_map[e_y][e_x]='E'

deftm_to_test_map():
forlineintm:
test_map.append(list(line))

deffind_path():
s_x,s_y=get_start_XY()
e_x,e_y=get_end_XY()
a_star=A_Star(s_x,s_y,e_x,e_y)
a_star.find_path()
searched=a_star.get_searched()
path=a_star.path
#标记已搜索区域
mark_searched(searched)
#标记路径
mark_path(path)
print"pathlengthis%d"%(len(path))
print"searchedsquarescountis%d"%(len(searched))
#标记开始、结束点
mark_start_end(s_x,s_y,e_x,e_y)

if__name__=="__main__":
#把字符串转成列表
tm_to_test_map()
find_path()
print_test_map()