AI2020_Project/alg.py

63 lines
1.7 KiB
Python
Raw Normal View History

2020-05-01 12:37:20 +02:00
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