Traktor/source/astar.py
2024-06-09 14:55:33 +02:00

133 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
if tile.image == "resources/images/rock_dirt.png":
return 500
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)