# Author: Christian Careaga (christian.careaga7@gmail.com) # A* Pathfinding in Python (2.7) # Please give credit if used import numpy from heapq import * def heuristic(a, b): return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2 def astar(array, start, goal): neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] close_set = set() came_from = {} gscore = {start:0} fscore = {start:heuristic(start, goal)} oheap = [] heappush(oheap, (fscore[start], start)) while oheap: current = heappop(oheap)[1] if current == goal: data = [] while current in came_from: data.append(current) current = came_from[current] return data close_set.add(current) for i, j in neighbors: neighbor = current[0] + i, current[1] + j tentative_g_score = gscore[current] + heuristic(current, neighbor) if 0 <= neighbor[0] < array.shape[0]: if 0 <= neighbor[1] < array.shape[1]: if array[neighbor[0]][neighbor[1]] == 1: continue else: # array bound y walls continue else: # array bound x walls continue if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0): continue if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]: came_from[neighbor] = current gscore[neighbor] = tentative_g_score fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal) heappush(oheap, (fscore[neighbor], neighbor)) return False '''Here is an example of using my algo with a numpy array, astar(array, start, destination) astar function returns a list of points (shortest path)''' nmap = numpy.array([ [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,0,1,1,1,1,1,1,1,1,1,1,1,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,0,1,1,1,1,1,1,1,1,1,1,1,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0]]) print astar(nmap, (0,0), (10,13))

#### Diff to Previous Revision

--- revision 2 2014-08-05 23:28:11 +++ revision 3 2014-08-06 10:03:01 @@ -5,84 +5,58 @@ import numpy from heapq import * + def heuristic(a, b): - """calculates the H cost using the Manhattan Method""" - return 10*(abs(a[0] - b[0]) + abs(a[1] - b[1])) + return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2 -def getG(current, neighbor): - """Function to calculate G cost, 10 for ortogonal movement, and 14 for diagonal movement""" - vector_sub = [abs(neighbor[0] - current[0]), abs(neighbor[1] - current[1])] - if sum(vector_sub) > 1: - move_cost = 14 - else: - move_cost = 10 - return move_cost +def astar(array, start, goal): -def astar(array, start, dest): - """ - Function to calculate the shortest path from point a to point - b in a numpy array, using A* pathfinding algorithm and a binary heap. - 0's in the array are walkable nodes, while 1's are solid obstacles - """ neighbors = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] - closed = [] - open_list = [] + close_set = set() came_from = {} - - gscore = {start : 0} - fscore = {start : heuristic(start, dest)} + gscore = {start:0} + fscore = {start:heuristic(start, goal)} oheap = [] - #place start and its fscore to heap and add start to open_set heappush(oheap, (fscore[start], start)) - open_list.append(start) + + while oheap: - while len(open_list): - #Grab fscore and corresponding position off top of heap current = heappop(oheap)[1] - open_list.append(current) - - if current == dest: #check if path is done and trace it back - path = [] + + if current == goal: + data = [] while current in came_from: - path.append(current) + data.append(current) current = came_from[current] - return list(reversed(path)) + return data - #drop current and place it in closed list - open_list.remove(current) - closed.append(current) - - for x, y in neighbors: #check current nodes neighbors - current_neighbor = current[0]+x , current[1]+y - if current_neighbor in closed: - continue - - new_g = gscore[current] + getG(current, current_neighbor) - - #check if neighbors are outside array bounds or they are obstacles - if 0 <= current_neighbor[0] < array.shape[0]: - if 0 <= current_neighbor[1] < array.shape[1]: - if array[current_neighbor[0]][current_neighbor[1]] == 1: - closed.append(current_neighbor) - continue # solid obstacle + close_set.add(current) + for i, j in neighbors: + neighbor = current[0] + i, current[1] + j + tentative_g_score = gscore[current] + heuristic(current, neighbor) + if 0 <= neighbor[0] < array.shape[0]: + if 0 <= neighbor[1] < array.shape[1]: + if array[neighbor[0]][neighbor[1]] == 1: + continue else: # array bound y walls continue else: # array bound x walls continue - - if current_neighbor not in open_list or new_g < gscore.get(current_neighbor, 0): - #make current node parent of neighbor node, and update G and F costs - came_from[current_neighbor] = current - gscore[current_neighbor] = new_g - fscore[current_neighbor] = new_g + getG(current, current_neighbor) - - #add neighbors to open_set and to bin heap - open_list.append(current_neighbor) - heappush(oheap, (fscore[current_neighbor], current_neighbor)) + + if neighbor in close_set and tentative_g_score >= gscore.get(neighbor, 0): + continue + + if tentative_g_score < gscore.get(neighbor, 0) or neighbor not in [i[1]for i in oheap]: + came_from[neighbor] = current + gscore[neighbor] = tentative_g_score + fscore[neighbor] = tentative_g_score + heuristic(neighbor, goal) + heappush(oheap, (fscore[neighbor], neighbor)) + + return False '''Here is an example of using my algo with a numpy array, astar(array, start, destination)