My A* pathfinding algorithm does not give the shortest path - algorithm

Question

I am writing an A* algorithm for my Godot game (Using GDScript), and something is messing up either when assigning node parentage or determining next best node that is causing the final path to take weird detours.
The function that calls the algorithm is below:
func mapClicked(pos):
if moveMode:
# Convert floats to ints.
pos = Vector3(int(pos[0]), int(pos[1]), int(pos[2]))
print("Moving to: " + str(pos))
var path = aStar(playerPos[0], playerPos[2], pos[0], pos[2])
print("\nPath: " + str(path))
Where the 'pos' variables represent a 3D coordinate in X, Y and Z.
The algorithm is shown below:
func aStar(x1, y1, x2, y2):
# Initialize open list with starting node.
var openList = [[x1, y1, 0]]
# Initialize closed list.
var closedList = []
# Initialize parents variable.
var parents = {}
while len(openList) > 0:
var q = null
for coord in openList:
if q == null or coord[2] < q[2]:
q = coord.duplicate()
openList.remove(openList.find(q))
# Generates successors
var successors = []
if q[0] - 1 >= 0:
successors.append([q[0] - 1, q[1]])
if q[0] + 1 < len(boardState):
successors.append([q[0] + 1, q[1]])
if q[1] - 1 >= 0:
successors.append([q[0], q[1] - 1])
if q[1] + 1 < len(boardState[q[0]]):
successors.append([q[0], q[1] + 1])
# Inspects successors.
for successor in successors:
# Calculates heuristic.
var g = distManhattan(x1, y1, successor[0], successor[1])
var h = distManhattan(x2, y2, successor[0], successor[1])
var f = g + h
# Sets successor's parent to the original node.
# TODO: FIX THIS.
if (not [q[0], q[1]] in parents) or (not (parents[[q[0], q[1]]][0] == successor[0] and parents[[q[0], q[1]]][1] == successor[1])) or (([[successor[0], successor[1]]] in parents) and (parents[[successor[0], successor[1]]][2] > f)):
parents[successor.duplicate()] = q.duplicate()
# Check for path found.
if successor[0] == x2 and successor[1] == y2:
# Generates path by moving backward through parentage.
print('\nOpen List: ' + str(openList))
print('\nClosed List: ' + str(closedList))
print('\nParentage:' + str(parents))
var path = [[successor[0], successor[1]]]
var currNode = parents[successor]
while currNode[0] != x1 or currNode[1] != y1:
path = [[currNode[0], currNode[1]]] + path
currNode = parents[[currNode[0], currNode[1]]]
path = [[currNode[0], currNode[1]]] + path
return path
# Determines whether current successor is better than the existing nodes.
var skip = false
for open in openList:
if open[0] == successor[0] and open[1] == successor[1]:
if open[2] <= f:
skip = true
break
if skip:
continue
for closed in closedList:
if closed[0] == successor[0] and closed[1] == successor[1]:
skip = true
break
if skip:
continue
successor.append(f)
openList.append(successor.duplicate())
closedList.append(q.duplicate())
And yes, I am aware that the z coordinates become y inside the algorithm, I just put it that way to make it easier for me to conceptualize.
And my print statements outputted the following:
I am at: (0, 0, 0)
map event triggered
Moving to: (5, 0, 4)
Open List: [[5, 0, 11], [5, 1, 11], [0, 6, 11], [5, 2, 11], [1, 6, 11], [5, 3, 11], [3, 5, 9], [2, 6, 11], [5, 4, 11]]
Closed List: [[0, 0, 0], [1, 0, 9], [0, 1, 9], [2, 0, 9], [1, 1, 9], [0, 2, 9], [3, 0, 9], [2, 1, 9], [1, 2, 9], [0, 3, 9], [4, 0, 9], [3, 1, 9], [2, 2, 9], [1, 3, 9], [0, 4, 9], [4, 1, 9], [3, 2, 9], [2, 3, 9], [1, 4, 9], [0, 5, 9], [4, 2, 9], [3, 3, 9], [2, 4, 9], [1, 5, 9], [4, 3, 9], [3, 4, 9], [2, 5, 9]]
Parentage:{[0, 1]:[0, 0, 0], [0, 2]:[0, 1, 9], [0, 3]:[0, 2, 9], [0, 4]:[0, 3, 9], [0, 5]:[0, 4, 9], [0, 6]:[0, 5, 9], [1, 0]:[1, 1, 9], [1, 1]:[1, 2, 9], [1, 2]:[1, 3, 9], [1, 3]:[1, 4, 9], [1, 4]:[1, 5, 9], [1, 5]:[0, 5, 9], [1, 6]:[1, 5, 9], [2, 0]:[2, 1, 9], [2, 1]:[2, 2, 9], [2, 2]:[2, 3, 9], [2, 3]:[2, 4, 9], [2, 4]:[2, 5, 9], [2, 5]:[1, 5, 9], [2, 6]:[2, 5, 9], [3, 0]:[3, 1, 9], [3, 1]:[3, 2, 9], [3, 2]:[3, 3, 9], [3, 3]:[3, 4, 9], [3, 4]:[2, 4, 9], [3, 5]:[2, 5, 9], [4, 0]:[4, 1, 9], [4, 1]:[4, 2, 9], [4, 2]:[4, 3, 9], [4, 3]:[4, 4, 9], [4, 4]:[3, 4, 9], [4, 5]:[4, 4, 9], [5, 0]:[4, 0, 9], [5, 1]:[4, 1, 9], [5, 2]:[4, 2, 9], [5, 3]:[4, 3, 9], [5, 4]:[4, 4, 9]}
Path: [[0, 0], [0, 1], [0, 2], [0, 3], [0, 4], [0, 5], [1, 5], [2, 5], [2, 4], [3, 4], [4, 4], [4, 5]]
I tried the pseudocode given in geeksforgeeks' A* search algorithm post and that caused an infinite loop when tracing the parents to get the shortest path, due to two nodes being the parents of each other. Thus, I created conditionals to prevent those types of parents from being created. This fixed the infinite loop.
However, the path should be the shortest possible one, and my algorithm is taking the detour as shown.
Any and all help would be greatly appreciated.

Answer 1

Be aware that Godot has classes AStar and AStar2D that you could use. Similarly, you could make your implementation using Vector2 or Vector3.
I tried your code like this:
extends Spatial
var boardState =[
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""]
]
func _ready() -> void:
var playerPos = Vector3.ZERO
var pos = Vector3(5, 0, 4)
print("I am at: " + str(playerPos))
print("Moving to: " + str(pos))
var path = aStar(playerPos[0], playerPos[2], pos[0], pos[2])
print("\nPath: " + str(path))
func distManhattan(x1, y1, x2, y2):
return abs(x2 - x1) + abs(y2 - y1)
func aStar(x1, y1, x2, y2):
# Initialize open list with starting node.
var openList = [[x1, y1, 0]]
# Initialize closed list.
var closedList = []
# Initialize parents variable.
var parents = {}
while len(openList) > 0:
var q = null
for coord in openList:
if q == null or coord[2] < q[2]:
q = coord.duplicate()
openList.remove(openList.find(q))
# Generates successors
var successors = []
if q[0] - 1 >= 0:
successors.append([q[0] - 1, q[1]])
if q[0] + 1 < len(boardState):
successors.append([q[0] + 1, q[1]])
if q[1] - 1 >= 0:
successors.append([q[0], q[1] - 1])
if q[1] + 1 < len(boardState[q[0]]):
successors.append([q[0], q[1] + 1])
# Inspects successors.
for successor in successors:
# Calculates heuristic.
var g = distManhattan(x1, y1, successor[0], successor[1])
var h = distManhattan(x2, y2, successor[0], successor[1])
var f = g + h
# Sets successor's parent to the original node.
# TODO: FIX THIS.
if (not [q[0], q[1]] in parents) or (not (parents[[q[0], q[1]]][0] == successor[0] and parents[[q[0], q[1]]][1] == successor[1])) or (([[successor[0], successor[1]]] in parents) and (parents[[successor[0], successor[1]]][2] > f)):
parents[successor.duplicate()] = q.duplicate()
# Check for path found.
if successor[0] == x2 and successor[1] == y2:
# Generates path by moving backward through parentage.
print('\nOpen List: ' + str(openList))
print('\nClosed List: ' + str(closedList))
print('\nParentage:' + str(parents))
var path = [[successor[0], successor[1]]]
var currNode = parents[successor]
while currNode[0] != x1 or currNode[1] != y1:
path = [[currNode[0], currNode[1]]] + path
currNode = parents[[currNode[0], currNode[1]]]
path = [[currNode[0], currNode[1]]] + path
return path
# Determines whether current successor is better than the existing nodes.
var skip = false
for open in openList:
if open[0] == successor[0] and open[1] == successor[1]:
if open[2] <= f:
skip = true
break
if skip:
continue
for closed in closedList:
if closed[0] == successor[0] and closed[1] == successor[1]:
skip = true
break
if skip:
continue
successor.append(f)
openList.append(successor.duplicate())
closedList.append(q.duplicate())
This is the output I get:
I am at: (0, 0, 0)
Moving to: (5, 0, 4)
Open List: [[4, 4, 7], [3, 5, 7], [2, 6, 7], [1, 7, 7], [0, 8, 7], [9, 0, 9], [8, 1, 9], [7, 2, 9], [6, 3, 9]]
Closed List: [[0, 0, 0], [1, 0, -7], [0, 1, -7], [2, 0, -5], [1, 1, -5], [0, 2, -5], [3, 0, -3], [2, 1, -3], [1, 2, -3], [0, 3, -3], [4, 0, -1], [3, 1, -1], [2, 2, -1], [1, 3, -1], [0, 4, -1], [5, 0, 1], [4, 1, 1], [3, 2, 1], [2, 3, 1], [1, 4, 1], [0, 5, 1], [6, 0, 3], [5, 1, 3], [4, 2, 3], [3, 3, 3], [2, 4, 3], [1, 5, 3], [0, 6, 3], [7, 0, 5], [6, 1, 5], [5, 2, 5], [4, 3, 5], [3, 4, 5], [2, 5, 5], [1, 6, 5], [0, 7, 5], [8, 0, 7], [7, 1, 7], [6, 2, 7]]
Parentage:{[0, 1]:[0, 0, 0], [0, 2]:[0, 1, -7], [0, 3]:[0, 2, -5], [0, 4]:[0, 3, -3], [0, 5]:[0, 4, -1], [0, 6]:[0, 5, 1], [0, 7]:[0, 6, 3], [0, 8]:[0, 7, 5], [1, 0]:[1, 1, -5], [1, 1]:[1, 2, -3], [1, 2]:[1, 3, -1], [1, 3]:[1, 4, 1], [1, 4]:[1, 5, 3], [1, 5]:[1, 6, 5], [1, 6]:[0, 6, 3], [1, 7]:[0, 7, 5], [2, 0]:[2, 1, -3], [2, 1]:[2, 2, -1], [2, 2]:[2, 3, 1], [2, 3]:[2, 4, 3], [2, 4]:[2, 5, 5], [2, 5]:[1, 5, 3], [2, 6]:[1, 6, 5], [3, 0]:[3, 1, -1], [3, 1]:[3, 2, 1], [3, 2]:[3, 3, 3], [3, 3]:[3, 4, 5], [3, 4]:[2, 4, 3], [3, 5]:[2, 5, 5], [4, 0]:[4, 1, 1], [4, 1]:[4, 2, 3], [4, 2]:[4, 3, 5], [4, 3]:[3, 3, 3], [4, 4]:[3, 4, 5], [5, 0]:[5, 1, 3], [5, 1]:[5, 2, 5], [5, 2]:[5, 3, 7], [5, 3]:[4, 3, 5], [5, 4]:[5, 3, 7], [6, 0]:[6, 1, 5], [6, 1]:[6, 2, 7], [6, 2]:[5, 2, 5], [6, 3]:[5, 3, 7], [7, 0]:[7, 1, 7], [7, 1]:[6, 1, 5], [7, 2]:[6, 2, 7], [8, 0]:[7, 0, 5], [8, 1]:[7, 1, 7], [9, 0]:[8, 0, 7]}
Path: [[0, 0], [0, 1], [0, 2], [0, 3], [0, 4], [0, 5], [0, 6], [1, 6], [1, 5], [2, 5], [2, 4], [3, 4], [3, 3], [4, 3], [5, 3], [5, 4]]
Which is different from what you posted. Although it exhibits a similar behavior.
In the A* algorithm you check if you reached the goal before checking the successors. So the code you have under "Check for path found" would be for the current node, and be before the successors loop, instead of being for the current successor. Also, you can add the current node to the closed list early.
You should discard successors that are in the closed list early.
I also don't see you discarding not passable successors. I'll assume your particular graph is a full grid.
By the way, you are using elements [x, y, f], and you initialize the starting node with [x1, y1, 0] which might look odd because the value f for it would not be 0. It is not relevant for your particular graph.
These changes gives you a the correct behavior. I tried this code:
extends Spatial
var boardState =[
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""],
["", "", "", "", "", "", "", "", "", ""]
]
func _ready() -> void:
var playerPos = Vector3.ZERO
var pos = Vector3(5, 0, 4)
print("I am at: " + str(playerPos))
print("Moving to: " + str(pos))
var path = aStar(playerPos[0], playerPos[2], pos[0], pos[2])
print("\nPath: " + str(path))
func distManhattan(x1, y1, x2, y2):
return abs(x2 - x1) + abs(y2 - y1)
func aStar(x1, y1, x2, y2):
# Initialize open list with starting node.
var openList = [[x1, y1, distManhattan(x1, y1, x1, y2)]]
# Initialize closed list.
var closedList = []
# Initialize parents variable.
var parents = {}
while len(openList) > 0:
var q = null
for coord in openList:
if q == null or coord[2] < q[2]:
q = coord.duplicate()
openList.remove(openList.find(q))
# Check for path found.
if q[0] == x2 and q[1] == y2:
# Generates path by moving backward through parentage.
print('\nOpen List: ' + str(openList))
print('\nClosed List: ' + str(closedList))
print('\nParentage:' + str(parents))
var path = [[q[0], q[1]]]
var currNode = parents[[q[0], q[1]]]
while currNode[0] != x1 or currNode[1] != y1:
path = [[currNode[0], currNode[1]]] + path
currNode = parents[[currNode[0], currNode[1]]]
path = [[currNode[0], currNode[1]]] + path
return path
closedList.append(q.duplicate())
# Generates successors
var successors = []
if q[0] - 1 >= 0:
successors.append([q[0] - 1, q[1]])
if q[0] + 1 < len(boardState):
successors.append([q[0] + 1, q[1]])
if q[1] - 1 >= 0:
successors.append([q[0], q[1] - 1])
if q[1] + 1 < len(boardState[q[0]]):
successors.append([q[0], q[1] + 1])
# Inspects successors.
for successor in successors:
var skip = false
for closed in closedList:
if closed[0] == successor[0] and closed[1] == successor[1]:
skip = true
break
if skip:
continue
# Calculates heuristic.
var g = distManhattan(x1, y1, successor[0], successor[1])
var h = distManhattan(x2, y2, successor[0], successor[1])
var f = g + h
# Sets successor's parent to the original node.
# TODO: FIX THIS.
if (not [q[0], q[1]] in parents) or (not (parents[[q[0], q[1]]][0] == successor[0] and parents[[q[0], q[1]]][1] == successor[1])) or (([[successor[0], successor[1]]] in parents) and (parents[[successor[0], successor[1]]][2] > f)):
parents[successor.duplicate()] = q.duplicate()
# Determines whether current successor is better than the existing nodes.
for open in openList:
if open[0] == successor[0] and open[1] == successor[1]:
if open[2] <= f:
skip = true
break
if skip:
continue
successor.append(f)
openList.append(successor.duplicate())
And got this result:
I am at: (0, 0, 0)
Moving to: (5, 0, 4)
Open List: [[4, 5, 9], [3, 6, 9], [2, 7, 9], [1, 8, 9], [0, 9, 9], [9, 1, 11], [8, 2, 11], [7, 3, 11], [6, 4, 11]]
Closed List: [[0, 0, 0], [1, 0, -7], [0, 1, -7], [2, 0, -5], [1, 1, -5], [0, 2, -5], [3, 0, -3], [2, 1, -3], [1, 2, -3], [0, 3, -3], [4, 0, -1], [3, 1, -1], [2, 2, -1], [1, 3, -1], [0, 4, -1], [5, 0, 1], [4, 1, 1], [3, 2, 1], [2, 3, 1], [1, 4, 1], [0, 5, 1], [6, 0, 3], [5, 1, 3], [4, 2, 3], [3, 3, 3], [2, 4, 3], [1, 5, 3], [0, 6, 3], [7, 0, 5], [6, 1, 5], [5, 2, 5], [4, 3, 5], [3, 4, 5], [2, 5, 5], [1, 6, 5], [0, 7, 5], [8, 0, 7], [7, 1, 7], [6, 2, 7], [5, 3, 7], [4, 4, 7], [3, 5, 7], [2, 6, 7], [1, 7, 7], [0, 8, 7], [9, 0, 9], [8, 1, 9], [7, 2, 9], [6, 3, 9]]
Parentage:{[0, 1]:[0, 0, 0], [0, 2]:[0, 1, -7], [0, 3]:[0, 2, -5], [0, 4]:[0, 3, -3], [0, 5]:[0, 4, -1], [0, 6]:[0, 5, 1], [0, 7]:[0, 6, 3], [0, 8]:[0, 7, 5], [0, 9]:[0, 8, 7], [1, 0]:[0, 0, 0], [1, 1]:[0, 1, -7], [1, 2]:[0, 2, -5], [1, 3]:[0, 3, -3], [1, 4]:[0, 4, -1], [1, 5]:[0, 5, 1], [1, 6]:[0, 6, 3], [1, 7]:[0, 7, 5], [1, 8]:[0, 8, 7], [2, 0]:[1, 0, -7], [2, 1]:[1, 1, -5], [2, 2]:[1, 2, -3], [2, 3]:[1, 3, -1], [2, 4]:[1, 4, 1], [2, 5]:[1, 5, 3], [2, 6]:[1, 6, 5], [2, 7]:[1, 7, 7], [3, 0]:[2, 0, -5], [3, 1]:[2, 1, -3], [3, 2]:[2, 2, -1], [3, 3]:[2, 3, 1], [3, 4]:[2, 4, 3], [3, 5]:[2, 5, 5], [3, 6]:[2, 6, 7], [4, 0]:[3, 0, -3], [4, 1]:[3, 1, -1], [4, 2]:[3, 2, 1], [4, 3]:[3, 3, 3], [4, 4]:[3, 4, 5], [4, 5]:[3, 5, 7], [5, 0]:[4, 0, -1], [5, 1]:[4, 1, 1], [5, 2]:[4, 2, 3], [5, 3]:[4, 3, 5], [5, 4]:[4, 4, 7], [6, 0]:[5, 0, 1], [6, 1]:[5, 1, 3], [6, 2]:[5, 2, 5], [6, 3]:[5, 3, 7], [6, 4]:[6, 3, 9], [7, 0]:[6, 0, 3], [7, 1]:[6, 1, 5], [7, 2]:[6, 2, 7], [7, 3]:[6, 3, 9], [8, 0]:[7, 0, 5], [8, 1]:[7, 1, 7], [8, 2]:[7, 2, 9], [9, 0]:[8, 0, 7], [9, 1]:[8, 1, 9]}
Path: [[0, 0], [0, 1], [0, 2], [0, 3], [0, 4], [1, 4], [2, 4], [3, 4], [4, 4], [5, 4]]
Notice the resulting path does not overshoot or backpedal.
You have a commend "TODO: FIX THIS." so I'll have a closer look there. It looks like this:
# TODO: FIX THIS.
if (not [q[0], q[1]] in parents) or (not (parents[[q[0], q[1]]][0] == successor[0] and parents[[q[0], q[1]]][1] == successor[1])) or (([[successor[0], successor[1]]] in parents) and (parents[[successor[0], successor[1]]][2] > f)):
parents[successor.duplicate()] = q.duplicate()
You might express this differently. This is what you want:
The successor is not in the closed list (you already skipped those, so you might as well not check for that here).
The successor is not in the open list (we haven't seen it yet).
The successor is in the open list, and it would be better (checking f).
In fact, you will be checking the open list for the same criteria… And you would be skipping every node for which you should not update the parent. So just do this:
# Determines whether current successor is better than the existing nodes.
for open in openList:
if open[0] == successor[0] and open[1] == successor[1]:
if open[2] <= f:
skip = true
break
if skip:
continue
# Sets successor's parent to the original node.
parents[successor.duplicate()] = q.duplicate()
Everything else I can think of changing would be static typing or optimization. However, I will point a couple things:
Aside from the parents, you only ever modify arrays by appending. When extending the successor with f, you might as well create a new array directly and save the call to duplicate. Then you would only be modifying the open list, the closed list and the parents. Knowing this you would also know when you don't need to worry about duplicating the arrays.
You can initialize the current node with an INF on the third element, so it is never null.