python 3d A* pathfinging infinite loop

Question

I am trying to adapt an application I found here, I imagined that I just had to add an axis. The problem is that the script seems like it is stuck. Can someone tell me what I did wrong and how I can use A* with a 3d matrix (i j k)?

this is the portion of the A* function I changed

for new_position in [(0, 0, 0), (0, 0, 1), (0, 1, 0), (0, 1, 1), (1, 0, 0), (1, 0, 1), (1, 1, 0), (1, 1, 1)]: # Adjacent squares, (-1, -1), (-1, 1), (1, -1), (1, 1)]: # Adjacent squares removed to ignor diagonal movement

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

    # 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 or node_position[2] > (len(maze) - 1) or node_position[2] < 0 or node_position[2] > (len(maze[len(maze)-1]) -1):
        continue

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

it was originally:

for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)]: # 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

this is the whole script with my alterations:

import numpy as np

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 astar(maze, start, end):
    """Returns a list of tuples as a path from the given start to the given end in the given maze"""

    # 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 = []

    # Add the start node
    open_list.append(start_node)

    # Loop until you find the end
    while len(open_list) > 0:
        # Get the current node
        current_node = open_list[0]
        current_index = 0
        for index, item in enumerate(open_list):
            if item.f < current_node.f:
                current_node = item
                current_index = index

        # Pop current off open list, add to closed list
        open_list.pop(current_index)
        closed_list.append(current_node)

        # Found the goal
        if current_node == end_node:
            path = []
            current = current_node
            while current is not None:
                path.append(current.position)
                current = current.parent
            return path[::-1] # Return reversed path

        # Generate children
        children = []

        for new_position in [(0, 0, 0), (0, 0, 1), (0, 1, 0), (0, 1, 1), (1, 0, 0), (1, 0, 1), (1, 1, 0), (1, 1, 1)]: # Adjacent squares, (-1, -1), (-1, 1), (1, -1), (1, 1)]: # Adjacent squares removed to ignor diagonal movement

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

            # 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 or node_position[2] > (len(maze) - 1) or node_position[2] < 0 or node_position[2] > (len(maze[len(maze)-1]) -1):
                continue

            # Make sure walkable terrain
            if maze[node_position[0]][node_position[1]][node_position[2]] != 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
            for closed_child in closed_list:
                if child == closed_child:
                    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.position[2] - end_node.position[2]) ** 2)
            child.f = child.g + child.h

            # Child is already in the open list
            for open_node in open_list:
                if child == open_node and child.g > open_node.g:
                    continue

            # Add the child to the open list
            open_list.append(child)


def main():


    maze = np.zeros((12,12,12))
    start = (10, 9, 9)
    end = (1, 1, 1)

    path = astar(maze, start, end)
    print(path)


if __name__ == '__main__':
    main()

Answer 1

A* can work with any number of dimensions; it's a graph traversal algorithm, and no matter how many dimensions your problem space has, connecting one position to another still produces a graph.

You have two problems with generating new nodes, however.

• You included (0, 0, 0) in the list, so no change. You keep putting the current position back into the queue for consideration. That’s just busy work, because the current position is already in the closed list.

• You never subtract from any of your coordinates, you only add. So your x, y and z values can only ever go up. If reaching your goal requires a path around an obstacle, then you have a problem here because all your version can ever do is move in one direction along any given axis.

In a 3 by 3 by 3 3D matrix, with the current position in the middle, there are 3 times 3 times 3 minus 1 == 26 positions you can reach with a single step. Your code reaches just 7 of those, plus one that stays put.

If you extract your tuples in the for new_position in [...] list into a separate variable and add some newlines, and re-arrange them a bit to group them by how many 1s you have in the tuple, you get the following definition. I renamed this to deltas, because it's not a new position, it's the change relative to the old position, or delta. I re-arranged your tuples to make it easier to group them logically:

deltas = [
    (0, 0, 0),  # no change, 
    (0, 0, 1), (0, 1, 0), (1, 0, 0), # add to a single axis
    (0, 1, 1), (1, 0, 1), (1, 1, 0), # add to two axes
    (1, 1, 1)  # move in all 3 directions at once.
]

for delta in deltas:
    # ...

You want to drop the first one ((0, 0, 0)), and you need to add -1 versions of the other variants. You need 26 different tuples, but writing those by hand gets to be cumbersome, really fast. You can use itertools.product() to generate these, instead:

from itertools import product

# combinations of -1, 0 and 1, filtering out the (0, 0, 0) case:
deltas = [d for d in product((-1, 0, 1), repeat=3) if any(d)]

Now, your comment next to the loop says:

# Adjacent squares removed to ignor diagonal movement

It's not entirely clear what you mean by this, because your original list of deltas includes diagonals ((0, 1, 1) moves both in the y and z directions, and you have tuples combining movement in the x plus y and the x plus z axes), and even one that moves diagonal by incrementing all 3 axes. If you only wanted to move up, down, left, right, forward, and backward, you want to restrict movement to a single axis at a time, and deltas should be:

# movement without diagonals
deltas = [
    (1, 0, 0), (-1, 0, 0),  # only x
    (0, 1, 0), (0, -1, 0),  # only y
    (0, 0, 1), (0, 0, -1),  # only z
]

Personally, I'd move the whole business of generating new positions to a separate method on the Node class:

def possible_moves(self, map):
    x, y, z = self.x, self.y, self.z
    for dx, dy, dz in DELTAS:
        newx, newy, newz = x + dx, y + dy, z + dz
        try:
            if maze[newx][newy][newz] != 0:
                yield Node(self, (newx, newy, newz))
        except IndexError:
            # not inside the maze anymore, ignore
            pass

This method assumes that there is a DELTAS global variable that defines the possible moves, and is a generator; each time yield is reached a new Node() instance is returned to whatever is using this as an iterator (like a for loop would).

Then just use that method instead of the children = [] list you filled with the for new_position in ...: loop, so directly in the part that uses the original children list:

# Loop through children
for child in current_node.possible_moves(map):

    # Child is on the closed list
    for closed_child in closed_list:
        if child == closed_child:
            continue

    # etc.