A*算法 -- python

#coding=utf-8
import math

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

#由於python裏string不能直接改變某一元素，因此用test_map來存儲搜索時的地圖
test_map = []

#########################################################
class Node_Elem:
	"""
開放列表和關閉列表的元素類型，parent用來在成功的時候回溯路徑
"""
	def __init__(self, parent, x, y, dist):
		self.parent = parent
		self.x = x
		self.y = y
		self.dist = dist

class A_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 = []

	#查找路徑的入口函數
	def find_path(self):
		#構建開始節點
		p = Node_Elem(None, self.s_x, self.s_y, 0.0)
		while True:
			#擴展F值最小的節點
			self.extend_round(p)
			#若是開放列表爲空，則不存在路徑，返回
			if not self.open:
				return
				#獲取F值最小的節點
			idx, p = self.get_best()
			#找到路徑，生成路徑，返回
			if self.is_target(p):
				self.make_path(p)
				return
				#把此節點壓入關閉列表，並從開放列表裏刪除
			self.close.append(p)
			del self.open[idx]

	def make_path(self,p):
		#從結束點回溯到開始點，開始點的parent == None
		while p:
			self.path.append((p.x, p.y))
			p = p.parent

	def is_target(self, i):
		return i.x == self.e_x and i.y == self.e_y

	def get_best(self):
		best = None
		bv = 1000000 #若是你修改的地圖很大，可能須要修改這個值
		bi = -1
		for idx, i in enumerate(self.open):
			value = self.get_dist(i)#獲取F值
			if value < bv:#比之前的更好，即F值更小
				best = i
				bv = value
				bi = idx
		return bi, best

	def get_dist(self, i):
		# F = G + H
		# G 爲已經走過的路徑長度， H爲估計還要走多遠
		# 這個公式就是A*算法的精華了。
		return i.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

	def extend_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)
		for x, y in zip(xs, ys):
			new_x, new_y = x + p.x, y + p.y
			#無效或者不可行走區域，則勿略
			if not self.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))
			#新節點在關閉列表，則忽略
			if self.node_in_close(node):
				continue
			i = self.node_in_open(node)
			if i != -1:
				#新節點在開放列表
				if self.open[i].dist > node.dist:
					#如今的路徑到比之前到這個節點的路徑更好~
					#則使用如今的路徑
					self.open[i].parent = p
					self.open[i].dist = node.dist
				continue
			self.open.append(node)

	def get_cost(self, x1, y1, x2, y2):
		"""
上下左右直走，代價爲1.0，斜走，代價爲1.4
"""
		if x1 == x2 or y1 == y2:
			return 1.0
		return 1.4

	def node_in_close(self, node):
		for i in self.close:
			if node.x == i.x and node.y == i.y:
				return True
		return False

	def node_in_open(self, node):
		for i, n in enumerate(self.open):
			if node.x == n.x and node.y == n.y:
				return i
		return -1

	def is_valid_coord(self, x, y):
		if x < 0 or x >= self.width or y < 0 or y >= self.height:
			return False
		return test_map[y][x] != '#'

	def get_searched(self):
		l = []
		for i in self.open:
			l.append((i.x, i.y))
		for i in self.close:
			l.append((i.x, i.y))
		return l

#########################################################
def print_test_map():
	"""
打印搜索後的地圖
"""
	for line in test_map:
		print ''.join(line)

def get_start_XY():
	return get_symbol_XY('S')

def get_end_XY():
	return get_symbol_XY('E')

def get_symbol_XY(s):
	for y, line in enumerate(test_map):
		try:
			x = line.index(s)
		except:
			continue
		else:
			break
	return x, y

#########################################################
def mark_path(l):
	mark_symbol(l, '*')

def mark_searched(l):
	mark_symbol(l, ' ')

def mark_symbol(l, s):
	for x, y in l:
		test_map[y][x] = s

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

def tm_to_test_map():
	for line in tm:
		test_map.append(list(line))

def find_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 "path length is %d"%(len(path))
	print "searched squares count is %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()