63 lines
1.7 KiB
Python
63 lines
1.7 KiB
Python
import heapq
|
|
|
|
class PriorityQueue:
|
|
def __init__(self):
|
|
self.elements = []
|
|
|
|
def put(self, item, priority):
|
|
heapq.heappush(self.elements, (priority, item))
|
|
|
|
def get(self):
|
|
return heapq.heappop(self.elements)[1]
|
|
|
|
def heuristic(xy1, xy2):
|
|
return abs(xy1[0] - xy2[0]) + abs(xy1[1] - xy2[1])
|
|
|
|
def neighbors(point):
|
|
x, y = point
|
|
list=((x+1,y), (x,y+1), (x,y-1), (x-1,y))
|
|
return list
|
|
|
|
#determining the cost of a specific field in the grid
|
|
def checkCost(grid, xy):
|
|
x, y = xy
|
|
cost = grid[x][y]
|
|
return cost
|
|
|
|
def aStar(grid, start, goal):
|
|
openlist = PriorityQueue()
|
|
openlist.put(start, 0)
|
|
|
|
fScore = {}
|
|
origin = {start: None}
|
|
fScore[start] = 0
|
|
closedlist=[]
|
|
|
|
cost=0
|
|
while openlist!={}:
|
|
current = openlist.get()
|
|
|
|
if current == goal:
|
|
path = []
|
|
#following from the succesors to the root our starting point
|
|
while current != start:
|
|
path.append(current)
|
|
current = origin[current]
|
|
path.reverse()
|
|
break
|
|
|
|
# succescor function
|
|
for succ in neighbors(current):
|
|
#checking if didn't go out of the maze
|
|
if(succ[0] < 0 or succ[1] < 0 or succ[0] > 19 or succ[1] > 19):
|
|
continue
|
|
|
|
gScore = fScore[current[0],current[1]] + checkCost(grid, current)
|
|
if succ not in closedlist or gScore < fScore[succ[0],succ[1]]:
|
|
closedlist.append(succ)
|
|
origin[succ[0],succ[1]] = current
|
|
fScore[succ[0],succ[1]] = gScore
|
|
priority = gScore + heuristic(goal, succ)
|
|
openlist.put(succ, priority)
|
|
return path
|