39 lines
1.4 KiB
Python
39 lines
1.4 KiB
Python
from succ import succ as successors
|
|
from queue import PriorityQueue
|
|
from state import State
|
|
|
|
def astar(istate, goalx, goaly, passedFields):
|
|
fringe = PriorityQueue()
|
|
fringe.put(istate)
|
|
explored = set()
|
|
steps = []
|
|
while not fringe.empty():
|
|
state = fringe.get()
|
|
if state.xpos == goalx and state.ypos == goaly:
|
|
steps.insert(0, state)
|
|
while (state.parent != None):
|
|
state = state.parent
|
|
steps.insert(0, state)
|
|
return steps
|
|
|
|
element = successors(state, passedFields, goalx, goaly)
|
|
explored.add((state.xpos, state.ypos, state.orientation))
|
|
for value in element:
|
|
val = (value.xpos, value.ypos, value.orientation)
|
|
if val in explored:
|
|
continue
|
|
cost = state.priority + value.priority
|
|
succesorState = State(state, value.action,
|
|
value.xpos, value.ypos,
|
|
value.orientation, cost,
|
|
value.heuristic)
|
|
if value not in fringe.queue:
|
|
fringe.put(succesorState)
|
|
else :
|
|
for element in fringe.queue:
|
|
if (element.xpos==value.xpos and element.ypos==value.ypos) and element > value:
|
|
element.parent = state
|
|
element.priority = value.priority
|
|
return False
|
|
|