a_star.py

# coding: utf-8
import heapq
import itertools


class AStar(object):

    def __init__(self, map, obstacle_symbol=1):
        self.map = map
        self.obstacle = obstacle_symbol

    def search(self, initial_pos, goal):
        passed_list = [initial_pos]
        initial_score = self.distance_from_start(passed_list) + self.distance_to_goal(initial_pos)
        # 探索済み座標と、その座標に辿り着いた経路のスコアを格納
        evaluated = {initial_pos: initial_score}
        # 経路リストとそのスコアを格納する探索ヒープ
        path_heapq = []
        # 探索ヒープに経路リストを格納
        heapq.heappush(path_heapq, (initial_score, passed_list))
        # 探索不可能になるまで
        while len(path_heapq) > 0:
            # 現在までに探索した経路の中から、スコアが最小になる
            # ときのスコアと経路リストを取得
            score, current_path = heapq.heappop(path_heapq)
            # 最後に探索した座標が目的地なら探索ヒープを返す
            if current_path[-1] == goal:
                return current_path
            # 最後に探索した座標の八方を探索
            for pos in self.neighbors(current_path[-1]):
                # 経路リストに探索中の座標を追加した一時リストを作成
                new_passed_list = current_path + [pos]
                # 一時リストのスコアを計算
                pos_score = self.distance_from_start(new_passed_list) + self.distance_to_goal(pos)
                # 探索中の座標が、他の経路で探索済みかどうかチェック
                # 探索済みの場合、前回のスコアと今回のスコアを比較
                # 今回のスコアのほうが大きい場合、次の方角の座標の探索へ
                if pos in evaluated and evaluated[pos] <= pos_score:
                    continue
                # 今回のスコアのほうが小さい場合、チェック済みリストに格納
                # 探索ヒープにスコアと経路リストを格納
                evaluated[pos] = pos_score
                heapq.heappush(path_heapq, (pos_score, new_passed_list))
        return []

    def distance_to_goal(self, pos):
        return ((pos[0] - goal[0]) ** 2 + (pos[1] - goal[1]) ** 2) ** 0.5

    def distance_from_start(self, path):
        return len(path)

    def neighbors(self, pos):
        '''8 neighbors'''
        for a, b in itertools.product([' + 1', ' - 1', ''], repeat=2):
            if a or b:
                if self.map[eval('pos[0]' + a)][eval('pos[1]' + b)] != self.obstacle:
                    yield (eval('pos[0]' + a), eval('pos[1]' + b))


if __name__ == "__main__":
    map = [
        'OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO',
        'OS  O     O     O         O          O',
        'O   O  O  O  O            O    OOOO GO',
        'O      O     O  O   OOOO  O    O  OOOO',
        'OOOOOOOOOOOOOOOOOO  O     O    O     O',
        'O                O  O     O          O',
        'O        OOO     O  O     OOOOOOOOO  O',
        'O  OO    O    OOOO  O     O      OO  O',
        'O   O    O          O     O  O   O   O',
        'O   OOO  O          O        O   O   O',
        'O        O          O        O       O',
        'OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO',
        ]

    def find_symbol(symbol):
        for i, l in enumerate(map):
            for j, p in enumerate(l):
                if p == symbol:
                    return (i, j)
    # スタート
    init = find_symbol("S")
    # ゴール
    goal = find_symbol("G")


    def render_path(path):
        ''' 結果の出力 '''
        buf = [[ch for ch in l] for l in map]

        for pos in path[1:-1]:
            buf[pos[0]][pos[1]] = "*"

        buf[path[0][0]][path[0][1]] = "s"
        buf[path[-1][0]][path[-1][1]] = "g"
        return ["".join(l) for l in buf]

    astar = AStar(map, obstacle_symbol="O")
    path = astar.search(init, goal)

    if len(path) > 0:
        print("\n".join(render_path(path)))

    else:
        print('failed')