astar.py

# Credit for this: Nicholas Swift
# as found at https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2
from warnings import warn
import heapq

class Node:
    """
    A node class for A* Pathfinding
    """

    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 __repr__(self):
      return f"{self.position} - g: {self.g} h: {self.h} f: {self.f}"

    # defining less than for purposes of heap queue
    def __lt__(self, other):
      return self.f < other.f
    
    # defining greater than for purposes of heap queue
    def __gt__(self, other):
      return self.f > other.f

def return_path(current_node):
    path = []
    current = current_node
    while current is not None:
        path.append(current.position)
        current = current.parent
    return path[::-1]  # Return reversed path


def astar(maze, start, end, allow_diagonal_movement = False):
    """
    Returns a list of tuples as a path from the given start to the given end in the given maze
    :param maze:
    :param start:
    :param end:
    :return:
    """

    # Create start and end node
    start_node = Node(None, start)
    start_node.g = start_node.h = start_node.f = 0
    end_node = Node(None, end)
    end_node.g = end_node.h = end_node.f = 0

    # Initialize both open and closed list
    open_list = []
    closed_list = []

    # Heapify the open_list and Add the start node
    heapq.heapify(open_list) 
    heapq.heappush(open_list, start_node)

    # Adding a stop condition
    outer_iterations = 0
    max_iterations = (len(maze[0]) * len(maze) // 2)

    # what squares do we search
    adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0),)
    if allow_diagonal_movement:
        adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1),)

    # Loop until you find the end
    while len(open_list) > 0:
        outer_iterations += 1

        if outer_iterations > max_iterations:
          # if we hit this point return the path such as it is
          # it will not contain the destination
          warn("giving up on pathfinding too many iterations")
          return return_path(current_node)       
        
        # Get the current node
        current_node = heapq.heappop(open_list)
        closed_list.append(current_node)

        # Found the goal
        if current_node == end_node:
            return return_path(current_node)

        # Generate children
        children = []
        
        for new_position in adjacent_squares: # Adjacent squares

            # Get node position
            node_position = (current_node.position[0] + new_position[0], current_node.position[1] + new_position[1])

            # Make sure within range
            if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze)-1]) -1) or node_position[1] < 0:
                continue

            # Make sure walkable terrain
            if maze[node_position[0]][node_position[1]] != 0:
                continue

            # Create new node
            new_node = Node(current_node, node_position)

            # Append
            children.append(new_node)

        # Loop through children
        for child in children:
            # Child is on the closed list
            if len([closed_child for closed_child in closed_list if closed_child == child]) > 0:
                continue

            # Create the f, g, and h values
            child.g = current_node.g + 1
            child.h = ((child.position[0] - end_node.position[0]) ** 2) + ((child.position[1] - end_node.position[1]) ** 2)
            child.f = child.g + child.h

            # Child is already in the open list
            if len([open_node for open_node in open_list if child.position == open_node.position and child.g > open_node.g]) > 0:
                continue

            # Add the child to the open list
            heapq.heappush(open_list, child)

    warn("Couldn't get a path to destination")
    return None

def example(print_maze = True):

    maze = [[0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,] * 2,
            [0,0,0,1,1,0,0,1,1,1,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,] * 2,
            [0,0,0,1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1,0,] * 2,
            [0,0,0,1,0,1,1,1,1,0,1,1,0,0,1,1,1,0,0,0,1,1,1,1,1,1,1,0,0,0,] * 2,
            [0,0,0,1,0,1,0,0,0,0,0,1,0,0,0,0,1,1,0,1,0,0,0,0,0,0,1,1,1,0,] * 2,
            [0,0,0,1,0,1,1,0,1,1,0,1,1,1,0,0,0,0,0,1,0,0,1,1,1,1,1,0,0,0,] * 2,
            [0,0,0,1,0,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,1,0,1,0,1,1,] * 2,
            [0,0,0,1,0,1,0,1,1,0,1,1,1,1,0,0,1,1,1,1,1,1,1,0,1,0,1,0,0,0,] * 2,
            [0,0,0,1,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,1,1,1,0,] * 2,
            [0,0,0,1,0,1,1,1,1,0,1,0,0,1,1,1,0,1,1,1,1,0,1,1,1,0,1,0,0,0,] * 2,
            [0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,1,1,] * 2,
            [0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,] * 2,
            [1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,] * 2,]
    
    start = (0, 0)
    end = (len(maze)-1, len(maze[0])-1)

    path = astar(maze, start, end)

    if print_maze:
      for step in path:
        maze[step[0]][step[1]] = 2
      
      for row in maze:
        line = []
        for col in row:
          if col == 1:
            line.append("\u2588")
          elif col == 0:
            line.append(" ")
          elif col == 2:
            line.append(".")
        print("".join(line))

    print(path)

Big thanks to Nicholas Swift, @ryancollingwood , @stabtazer , @Yichen-Lei, and others. I put together the best version based on the feedback and comments provided by everybody, which included implementing the sq root for the value of h. The algorithm works fine on a variety of mazes from 2 x 2, 1 x 3, up to 500 x 500 (although the large maze took a long time to solve).

Things for your consideration:

1. I implemented a change when increasing the value of g when making a diagonal move. I changed the current cost value of 1 to be 1.4 which is the distance moved from Node center to Node center along the diagonal. Using the 1.4 value differentiates the difference between moving orthogonal to the grid or diagonally.

Example: The cost of an orthogonal move from (0, 0) to (0, 1) is 1. The cost of an orthogonal move from (0, 0) to (1, 1) is 2. With the current implementation, while using the allow_diagonal_movement flag, the cost to move from (0, 0) to (1, 1) is also 1.

With my change when the allow_diagonal_movement flag is in use the cost to move from (0, 0) to (1, 1) is 1.4; thus its bigger than 1 and smaller than 2.

2. There is a bias in finding the path because the algorithm does not randomize the initial or next search direction. I solved this issue by randomly selecting from the the adjacent_squares list the next direction to search.

3. Here are my changes to address item 1 and 2:

Additions to initialize the data structures needed to support the diagonal move cost and the direction bias.

    #OLD CODE
    # what squares do we search
    #adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0),)
    #if allow_diagonal_movement:
    #   adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1),)
    #NEW CODE
    if allow_diagonal_movement:
        adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1))
        direction_cost = (1.0, 1.0, 1.0, 1.0, 1.4, 1.4, 1.4, 1.4)
        adjacent_square_pick_index = [0, 1, 2, 3, 4, 5, 6, 7]
    else:
        adjacent_squares = ((0, -1), (0, 1), (-1, 0), (1, 0))
        direction_cost= (1.0, 1.0, 1.0, 1.0)
        adjacent_square_pick_index = [0, 1, 2, 3]

    #EXISTING CODE
    # Loop until you find the end
    while len(open_list) > 0:

        #NEW CODE
        #Randomize the order of the adjacent_squares_pick_index to avoid a decision making bias
        random.shuffle(adjacent_square_pick_index)

Change to the for loop when selecting the initial and next direction to search, and to assign the direction move cost.

        # OLD CODE
        #for new_position in adjacent_squares: # Adjacent squares
        # NEW CODE
        for pick_index in adjacent_squares_pick_index:
            new_position = adjacent_squares[pick_index]
            direction_cost_factor = direction_cost[pick_index]

Change to the increase in g when assigning the move cost.

            # OLD CODE
            #child.g = current_node.g + 1
            # NEW CODE
            child.g = current_node.g + direction_cost_factor 

I hope this is useful.

Are there any ways to do this with multiple nodes with edges? I'm a bit daunted at the prospect of doing so. I'm not looking for a straight up answer, just a hint, some help.

@ameerkatofficial, can you provide a more descriptive problem statement to help me better understand the problem you are looking to solve. I would benefit from better understanding what is meant by "multiple nodes with edges".

@frankhale @stabtazer @ryancollingwood I simplify your code as below:

           \# Child is already in the open list
            if child in open_list: 
                idx = open_list.index(child) 
                if child.g < open_list[idx].g:
                    # update the node in the open list
                    open_list[idx].g = child.g
                    open_list[idx].f = child.f
                    open_list[idx].h = child.h
            else:
                \# Add the child to the open list
                heapq.heappush(open_list, child)

Through in function, we can fastly check whether child has been in open list. Also, I think we only need to update the g,h,f of the node which has the same position as child rather than operating the heap. About the calculation of Euclidean distance, I agree to have a sqrt on h, otherwise, h will play the most role in wayfinding and the result will be a straight line (on weight map).

@Yichen-Lei that's a great solution, however you forgot to update the parent, this leads to suboptimal paths. You should just update the entire open_list[idx]

           # Child is already in the open list
            if child in open_list: 
                idx = open_list.index(child) 
                if child.g < open_list[idx].g:
                    # update the node in the open list
                    open_list[idx] = child
            else:
                # Add the child to the open list
                heapq.heappush(open_list, child)

After messing around with this code for a uni project, I perfected the code and created a version that works for both 2d and 3d

from warnings import warn
import heapq
from itertools import product

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 __repr__(self):
      return f"{self.position} - g: {self.g} h: {self.h} f: {self.f}"

    # defining less than for purposes of heap queue
    def __lt__(self, other):
      return self.f < other.f
    
    # defining greater than for purposes of heap queue
    def __gt__(self, other):
      return self.f > other.f

    def return_path(self):
        path = []
        current = self
        while current is not None:
            path.append(current.position)
            current = current.parent
        return path[::-1]
    
    def euclidean_distance(self, other):
        return round(sum([(self.position[i] - other.position[i])**2 for i in range(len(self.position))]) ** 0.5, 3)

    def is_outside(self, maze):
        def check_dimension(dim_maze, position, dim=0):
            if dim >= len(position):
                return False
            
            if position[dim] < 0 or position[dim] >= len(dim_maze):
                return True
            
            return check_dimension(dim_maze[position[dim]], position, dim + 1)
        
        return check_dimension(maze, self.position)

    def is_wall(self, maze):
        def navigate_dimension(dim_maze, position, dim=0):
            if dim == len(position) - 1:
                return dim_maze[position[dim]] == 0
            
            return navigate_dimension(dim_maze[position[dim]], position, dim + 1)
        
        return navigate_dimension(maze, self.position)

def astar(maze, start, end):
    dimension = len(maze.shape)
    if len(start) != dimension or len(end) != dimension:
        raise ValueError("Start and end must have the same number of dimensions as the maze")

    # Create start and end node
    start_node = Node(None, start)
    end_node = Node(None, end)

    closed_list = []
    open_list = []
    heapq.heapify(open_list) #create priority queue for open list
    heapq.heappush(open_list, start_node)

    outer_iterations = 0
    max_iterations = (maze.size // 2)

    # Define the adjacent squares (including diagonals)
    adjacent_squares = [c for c in product((-1, 0, 1), repeat=dimension) if any(c)]

    # Loop until you find the end
    while len(open_list) > 0:
        outer_iterations += 1

        if outer_iterations > max_iterations:# if we cannot find by searching half the maze, we give up
            warn("giving up on pathfinding. too many iterations")
            return current_node.return_path()
        
        # Get the current node
        current_node = heapq.heappop(open_list)
        closed_list.append(current_node)

        if current_node == end_node and open_list[0].f >= current_node.f:
            #! we are done
            return current_node.return_path()

        for new_position in adjacent_squares:
            # Get node position
            node_position = tuple([current_node.position[i] + new_position[i] for i in range(dimension)])
            new_node = Node(current_node, node_position)

            if new_node.is_outside(maze):
                continue

            if new_node.is_wall(maze):
                continue

            if new_node in closed_list:
                continue

            child = new_node #the new node is a valid child of the current_node
            #calculate the heuristic
            cost = sum([(child.position[i] - current_node.position[i])**2 for i in range(dimension)]) ** 0.5
            child.g = current_node.g + cost
            child.h = child.euclidean_distance(end_node)
            child.f = child.g + child.h

            #add or update the child to the open list
            if child in open_list: 
                i = open_list.index(child) 
                if child.g < open_list[i].g:
                    # update the node in the open list
                    open_list[i] = child
            else:
                heapq.heappush(open_list, child)

    warn("Couldn't get a path to destination")
    return None