Inteligentna_smieciarka/astar.py

39 lines
1.4 KiB
Python
Raw Normal View History

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)
2023-05-05 12:13:47 +02:00
explored.add((state.xpos, state.ypos, state.orientation))
for value in element:
2023-05-05 12:13:47 +02:00
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