Created
November 15, 2023 22:40
-
-
Save JacobMJones/bdb2161adcbc4b8349af3f8c86511ec4 to your computer and use it in GitHub Desktop.
A star path finding explanation in python
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# The total cost of a node is calculated as | |
# f(n) = g(n) + h(n) | |
# g(n) is the cost from the start node to the current node, and | |
# h(n) is the estimated cost from the current node to the goal. | |
# f value is the sum of g and h and is used to compare nodes when deciding | |
# the parent attribute of a Node | |
# is used to keep track of the path | |
# from the start node to the current node. | |
class Node: | |
def __init__(self, position=None, parent=None): | |
self.position = position | |
self.parent = parent | |
self.g = 0 | |
self.h = 0 | |
self.f = 0 | |
# Compare nodes | |
def __eq__(self, other): | |
return self.position == other.position | |
###################################################### | |
# astar is the main function | |
# start is where the node begins and end_node is the goal | |
#open list is for nodes that have been discovered but not evaluated. It begins | |
#with the start node, new nodes (neighbors of the currently evaluated nodes) are added to this list. | |
#these are potential candidates for path continuation | |
#the algorithm will judge based on their f values. | |
#closed list is for nodes whose neighbours f values have been evaluated | |
#The algorithm terminates when the goal node is added to the closed list | |
def astar(grid, start, end): | |
# Create start and end node | |
start_node = Node(start, None) | |
end_node = Node(end, None) | |
print(f"Start: {start_node.position}") | |
print(f"End: {end_node.position}") | |
# Initialize open and closed list | |
open_list = [] | |
closed_list = [] | |
# Add the start node | |
open_list.append(start_node) | |
# Loop until the end node is found | |
while len(open_list) > 0: | |
# print('open', [node.position for node in open_list]) | |
# Get the current node (node with the lowest f value) | |
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 node from open list and add to closed list | |
open_list.pop(current_index) | |
closed_list.append(current_node) | |
# Check if we reached the end | |
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, -1), (0, 1), (-1, 0), (1, 0)]: # Adjacent squares | |
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(grid) - 1) or node_position[0] < 0 or node_position[1] > (len(grid[len(grid)-1]) -1) or node_position[1] < 0: | |
continue | |
# Make sure walkable | |
if grid[node_position[0]][node_position[1]] != 0: | |
continue | |
# Create new node and set current node as parent | |
new_node = Node(node_position, current_node) | |
# 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 == open_node and child.g > open_node.g]) > 0: | |
continue | |
# Add the child to the open list | |
open_list.append(child) | |
return None # No path found | |
# Example usage | |
grid = [[0, 0, 0, 0, 0], | |
[0, 1, 1, 1, 0], | |
[0, 1, 0, 0, 0], | |
[0, 0, 0, 1, 1], | |
[0, 0, 0, 0, 0]] | |
start = (0, 0) | |
end = (4, 4) | |
path = astar(grid, start, end) | |
#print(path) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment