A*をPythonで - rokujyouhitoma's blog

C++でA*が必要なのだけど、実装経験がなかったのでまずはPythonでリファレンス実装を行った。
という背景があるので、setとかは使ってない。neighbor_nodes関数のタプルもなくしたいのだががががが。

参考にしたのは、wikipediaのA*。wikipedia英語版のA*の擬似コードをPythonで実装した。ほぼ一緒。
wikipedia日本語版のA*は擬似コード載ってないからどんまい。

距離の評価関数は、マンハッタン距離とユークリッド距離の２つ。
となりのノードは上下左右のみでok。

コード

テストはない。

import math

def astar(start, goal, neighbor_nodes, dist_between, heuristic_cost_estimate):
    closedset = []
    openset   = [start]
    came_from = {}
    g_score   = {}
    f_score   = {}
    g_score[start] = 0
    f_score[start] = g_score[start] + heuristic_cost_estimate(start, goal)
    while openset:
        current = min((f_score[node], node) for node in openset)[1]
        if current == goal:
            return reconstruct_path(came_from, goal)
        openset.remove(current)
	closedset.append(current)
        for neighbor in neighbor_nodes(current):
            tentative_g_score = g_score[current] + dist_between(current, neighbor)
            tentative_f_score = tentative_g_score + heuristic_cost_estimate(neighbor, goal)
            if neighbor in closedset and tentative_f_score >= f_score[neighbor]:
		continue
            if neighbor not in openset or tentative_f_score < f_score[neighbor]:
                came_from[neighbor] = current
		g_score[neighbor] = tentative_g_score
                f_score[neighbor] = tentative_f_score
                if neighbor not in openset:
                    openset.append(neighbor)

    return False

def reconstruct_path(came_from, current_node):
    path = [current_node]
    while current_node in came_from:
        current_node = came_from[current_node]
        path.append(current_node)
    return path

MAP_ROW = 5
MAP_COL = 5
map = [[1,1,1,1,1],
       [1,0,0,0,1],
       [1,0,0,0,1],
       [1,0,0,0,1],
       [1,0,0,0,1],
       [1,1,1,1,1]]

def is_movable(p):
    x, y = p
    if x < 0 or x > MAP_COL-1 or y < 0 or y > MAP_ROW-1:
        return False
    if map[y][x] == 1:
        return False
    return True

def neighbor_nodes(p):
    x, y = p
    neighbors = [(x-1, y), (x, y-1), (x+1, y), (x, y+1)]
    return filter(is_movable, neighbors)

def heuristic_cost_estimate(p1, p2):
    return manhattan_distance(p1, p2)

def manhattan_distance(p1, p2):
    return abs(p1[0]-p2[0]) + abs(p1[1]-p2[1])

def euclidean_distance(p1, p2):
    return math.sqrt((p1[0]-p2[0])**2 + (p1[1]-p2[1])**2)

if __name__ == '__main__':
    start = (1, 1)
    goal  = (3, 4)
    path  = astar(start, goal, neighbor_nodes, heuristic_cost_estimate, heuristic_cost_estimate)
    if path:
        for position in reversed(path):
            x,y = position
            print(x,y)