111 lines
3.1 KiB
Python
111 lines
3.1 KiB
Python
from .obj.TemporaryState import TemporaryState
|
|
from queue import PriorityQueue
|
|
|
|
|
|
class StateController:
|
|
def __init__(self, istate):
|
|
self.path = []
|
|
self.explored = []
|
|
self.fringe = PriorityQueue()
|
|
self.istate = istate
|
|
self.goal = istate.position
|
|
|
|
def reset(self):
|
|
self.path.clear()
|
|
self.explored.clear()
|
|
self.fringe = PriorityQueue()
|
|
|
|
def build_path(self, goal_state, engine):
|
|
total_cost = goal_state.cost
|
|
self.path.append(goal_state)
|
|
engine.goals.pop()
|
|
while self.path[-1].parent.agent_role not in ["blank", "waiter"]:
|
|
self.path.append(self.path[-1].parent)
|
|
total_cost += self.path[-1].cost
|
|
|
|
print("Total path cost:\t{0}".format(total_cost))
|
|
return self.path
|
|
|
|
def graphsearch(self, engine): # A*
|
|
print("Search path")
|
|
|
|
self.goal = list(engine.goals[-1].position)
|
|
|
|
self.reset()
|
|
|
|
start = TemporaryState(self.istate, 0)
|
|
|
|
self.fringe.put(start)
|
|
|
|
while self.fringe.queue and not self.path:
|
|
self.explored.append(self.fringe.get())
|
|
if self.goal_test(engine):
|
|
return
|
|
|
|
self.succ(self.explored[-1].front(), engine)
|
|
self.succ(self.explored[-1].left(), engine)
|
|
self.succ(self.explored[-1].right(), engine)
|
|
|
|
engine.redraw()
|
|
|
|
self.reset()
|
|
for o in engine.objects:
|
|
o.compare_pos(engine.goals[-1].position)
|
|
o.agent_role = "block"
|
|
engine.goals.pop()
|
|
|
|
print("Not found")
|
|
|
|
return False
|
|
|
|
def succ(self, state, engine):
|
|
if state.collide_test():
|
|
return
|
|
elif any(e.compare(state) for e in self.explored):
|
|
return
|
|
elif any([o.collide_test(state) for o in engine.objects]):
|
|
return
|
|
|
|
for o in engine.objects:
|
|
if state.cost != 1:
|
|
break
|
|
if o.position == state.position:
|
|
state.change_cost(o)
|
|
|
|
state.cost_so_far = self.explored[-1].cost_so_far + state.cost
|
|
|
|
in_explored = any([state.compare(s) for s in self.explored]
|
|
)
|
|
|
|
in_frige = any([state.compare(f) for f in self.fringe.queue])
|
|
|
|
if not in_explored and not in_frige:
|
|
state.heuristic(self.goal)
|
|
self.fringe.put(state)
|
|
|
|
elif in_frige:
|
|
fringe = state
|
|
for f in self.fringe.queue:
|
|
if state.compare(f):
|
|
fringe = f
|
|
break
|
|
|
|
if state.cost_so_far < fringe.cost_so_far:
|
|
fringe.replace(state)
|
|
|
|
def goal_test(self, engine) -> bool:
|
|
if self.explored[-1].position == self.goal:
|
|
self.__is_goal__(self.explored[-1], engine)
|
|
return True
|
|
|
|
for fringe in self.fringe.queue:
|
|
if fringe.position == self.goal:
|
|
self.__is_goal__(fringe, engine)
|
|
return True
|
|
|
|
return False
|
|
|
|
def __is_goal__(self, goal_state, engine):
|
|
self.reset()
|
|
self.build_path(goal_state, engine)
|