Welcome, guest | Sign In | My Account | Store | Cart
```# 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

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
+        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)
```