python 3d A* pathfinging infinite loop - python-3.x

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.