用python實作基本A*算法

本文由戀花蝶發表于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 -*-
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()