131 lines
3.3 KiB
Python
131 lines
3.3 KiB
Python
|
from area.constants import TILE_SIZE, ROWS
|
||
|
|
import copy
|
||
|
|
import heapq
|
||
|
|
from area.field import tiles, fieldX, fieldY
|
||
|
|
from bfs import get_moves
|
||
|
|
|
||
|
|
class Node:
|
||
|
|
|
||
|
|
def __init__(self, priority, x, y, direction, parent, action, cost=0):
|
||
|
|
self.x = x
|
||
|
|
self.y = y
|
||
|
|
self.direction = direction
|
||
|
|
self.action = action
|
||
|
|
self.parent = parent
|
||
|
|
self.cost = cost
|
||
|
|
self.priority = priority
|
||
|
|
|
||
|
|
|
||
|
|
def __int__(self):
|
||
|
|
return self.priority
|
||
|
|
|
||
|
|
# getters and setters:
|
||
|
|
|
||
|
|
def get_parent(self):
|
||
|
|
return self.parent
|
||
|
|
|
||
|
|
def set_parent(self, _parent):
|
||
|
|
self.parent = _parent
|
||
|
|
|
||
|
|
def get_action(self):
|
||
|
|
return self.action
|
||
|
|
|
||
|
|
def set_action(self, _action):
|
||
|
|
self.parent = _action
|
||
|
|
|
||
|
|
def get_x(self):
|
||
|
|
return self.x
|
||
|
|
|
||
|
|
def set_x(self, _x):
|
||
|
|
self.x = _x
|
||
|
|
|
||
|
|
def get_y(self):
|
||
|
|
return self.y
|
||
|
|
|
||
|
|
def set_y(self, _y):
|
||
|
|
self.y = _y
|
||
|
|
|
||
|
|
def get_direction(self):
|
||
|
|
return self.direction
|
||
|
|
|
||
|
|
def set_direction(self, _direction):
|
||
|
|
self.parent = _direction
|
||
|
|
|
||
|
|
def get_cost(self):
|
||
|
|
return self.cost
|
||
|
|
|
||
|
|
def set_cost(self, _cost):
|
||
|
|
self.cost = _cost
|
||
|
|
|
||
|
|
def get_priority(self):
|
||
|
|
return self.priority
|
||
|
|
|
||
|
|
def set_priority(self, _priority):
|
||
|
|
self.priority = _priority
|
||
|
|
|
||
|
|
def __lt__(self, other):
|
||
|
|
return self.get_priority() < other.get_priority()
|
||
|
|
|
||
|
|
def copy(self):
|
||
|
|
return copy.copy(self)
|
||
|
|
|
||
|
|
|
||
|
|
def goal_test(elem, goalstate):
|
||
|
|
if elem.get_x() == goalstate[0] and elem.get_y() == goalstate[1]:
|
||
|
|
return True
|
||
|
|
else:
|
||
|
|
return False
|
||
|
|
|
||
|
|
def tile_cost(tile):
|
||
|
|
if tile.image == "resources/images/sampling.png":
|
||
|
|
return 100
|
||
|
|
else:
|
||
|
|
return 1
|
||
|
|
|
||
|
|
def heuristic(current_x, current_y, end_x, end_y):
|
||
|
|
return abs(end_x - current_x) + abs(end_y - current_y)
|
||
|
|
# State as a tuple (x,y,direction)
|
||
|
|
|
||
|
|
# actions(string): move, rotate_to_left, rotate_to_right
|
||
|
|
|
||
|
|
# main search function:
|
||
|
|
def a_star(istate, succ, goaltest, tractor):
|
||
|
|
fringe = []
|
||
|
|
explored = set()
|
||
|
|
node = Node(0, istate.get_x(), istate.get_y(), istate.get_direction(), None, None, 0)
|
||
|
|
heapq.heappush(fringe, node)
|
||
|
|
|
||
|
|
while True:
|
||
|
|
|
||
|
|
if not fringe:
|
||
|
|
return False
|
||
|
|
|
||
|
|
elem = heapq.heappop(fringe)
|
||
|
|
temp = copy.copy(elem)
|
||
|
|
if goal_test(elem, goaltest) is True: # jesli True zwroc ciag akcji
|
||
|
|
return get_moves(elem)
|
||
|
|
|
||
|
|
explored.add(elem)
|
||
|
|
|
||
|
|
for (action, state) in succ(temp, tractor):
|
||
|
|
fringe_tuple = []
|
||
|
|
explored_tuple = []
|
||
|
|
|
||
|
|
for node in fringe:
|
||
|
|
fringe_tuple.append((node.get_x(), node.get_y(), node.get_direction()))
|
||
|
|
for node in explored:
|
||
|
|
explored_tuple.append((node.get_x(), node.get_y(), node.get_direction()))
|
||
|
|
|
||
|
|
|
||
|
|
tile = int((temp.get_y() - fieldY) / TILE_SIZE) * ROWS + int((temp.get_x() - fieldX) / TILE_SIZE)
|
||
|
|
cost = temp.cost + tile_cost(tiles[tile])
|
||
|
|
priority = cost + heuristic(state[0], state[1], goaltest[0], goaltest[1])
|
||
|
|
|
||
|
|
x = Node(priority, state[0], state[1], state[2], elem, action, cost)
|
||
|
|
|
||
|
|
if state not in fringe_tuple and state not in explored_tuple:
|
||
|
|
heapq.heappush(fringe, x)
|
||
|
|
|
||
|
|
elif state in fringe_tuple and elem.get_priority() < priority:
|
||
|
|
elem.set_priority(priority)
|