A* 길찾기

astar.py

from timeit import repeat
from statistics import mean

# If you wanna test the execution time
def runAlgorithm(maze, start, end):
    setup_code = "from __main__ import aStar"

    stmt = f"aStar({maze},{start},{end})"
    times = repeat(setup=setup_code, stmt=stmt, repeat=3, number=10000)

    print(f"Mean execution time: {mean(times):.5f}초")


def heuristic(node, goal, D=1, D2=2 ** (1 / 2)):  # octile distance + diagonal H
    dx = abs(node.position[0] - goal.position[0])
    dy = abs(node.position[1] - goal.position[1])
    return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)


class Node:
    def __init__(self, parent=None, position=None):
        self.parent = parent
        self.position = position

        self.g = 0
        self.h = 0
        self.f = 0

    def __eq__(self, other):
        return self.position == other.position


def aStar(maze, start, end):
    # startNode, endNode init
    startNode = Node(None, start)
    endNode = Node(None, end)

    # openList, ClosedList init
    openList = []
    closedList = []

    # add startNode to openList
    openList.append(startNode)

    # while finding endNode
    while openList:

        # set current node
        currentNode = openList[0]
        currentIdx = 0

        # 이미 같은 노드가 openList에 있고, f 값이 더 크면
        # currentNode를 openList안에 있는 값으로 교체
        for index, item in enumerate(openList):
            if item.f < currentNode.f:
                currentNode = item
                currentIdx = index

        # openList에서 제거하고 closedList에 추가
        openList.pop(currentIdx)
        closedList.append(currentNode)

        # 현재 노드가 목적지면 current.position 추가하고
        # current의 부모로 이동
        if currentNode == endNode:
            path = []
            current = currentNode
            while current is not None:
                x, y = current.position
                maze[x][y] = 7
                path.append(current.position)
                current = current.parent
            return path[::-1]  # Return reversed path

        children = []
        # 인접한 xy좌표 전부
        for newPosition in [(0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)]:

            # 노드 위치 업데이트
            nodePosition = (
                currentNode.position[0] + newPosition[0],  # X
                currentNode.position[1] + newPosition[1])  # Y

            # Range
            rangeCriteria = [
                nodePosition[0] > (len(maze) - 1),
                nodePosition[0] < 0,
                nodePosition[1] > (len(maze[len(maze) - 1]) - 1),
                nodePosition[1] < 0,
            ]

            if any(rangeCriteria):
                continue

            # 장애물이 있으면 다른 위치 불러오기
            if maze[nodePosition[0]][nodePosition[1]] != 0:
                continue

            newNode = Node(currentNode, nodePosition)
            children.append(newNode)

        # 자식들 모두 loop
        for child in children:
            # 자식이 closedList에 있으면 check
            if len([closedChild for closedChild in closedList if closedChild == child]) > 0:
                continue

            # f, g, h값 업데이트
            child.g = currentNode.g + 1
            child.h = ((child.position[0] - endNode.position[0]) ** 2) + \
                      ((child.position[1] - endNode.position[1]) ** 2)
            child.f = child.g + child.h
            # child.h = heuristic(child, endNode)  More accurate h
            # print("position:", child.position)
            # print("from child to goal:", child.h)

            # 자식이 openList에 있으면 check
            if len([openNode for openNode in openList
                    if child == openNode and child.g > openNode.g]) > 0:
                continue

            openList.append(child)


def main():
    maze = [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 1, 1, 0, 1, 1, 0, 1, 1, 0],
            [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]

    start = (0, 9)
    end = (9, 0)

    path = aStar(maze, start, end)
    print(path)
    for i in range(len(maze)):
        print(maze[i])


if __name__ == '__main__':
    main()

result.txt

Min execution time : 0.0016s

# Path
[(0, 9), (0, 8), (0, 7), (1, 6), (2, 6), (3, 6), (4, 6), (5, 5), (5, 4), (6, 3), (7, 3), (8, 3), (9, 2), (9, 1), (9, 0)]

# Path visualization
[0, 0, 0, 0, 0, 0, 0, 7, 7, 7]
[0, 1, 1, 0, 1, 1, 7, 1, 1, 0]
[0, 1, 1, 0, 1, 1, 7, 1, 1, 0]
[0, 1, 1, 0, 1, 1, 7, 1, 1, 0]
[0, 1, 1, 0, 1, 1, 7, 1, 1, 0]
[0, 0, 0, 0, 7, 7, 0, 0, 0, 0]
[0, 1, 1, 7, 1, 1, 0, 1, 1, 0]
[0, 1, 1, 7, 1, 1, 0, 1, 1, 0]
[0, 1, 1, 7, 1, 1, 0, 1, 1, 0]
[7, 7, 7, 0, 0, 0, 0, 0, 0, 0]