From 511713de706f97b84040312939347ae9105453ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mateusz=20Cwojdzi=C5=84ski?= Date: Thu, 28 Apr 2022 21:32:34 +0200 Subject: [PATCH] Implement A-Star algorithm --- astar.py | 185 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ main.py | 5 +- 2 files changed, 188 insertions(+), 2 deletions(-) create mode 100644 astar.py diff --git a/astar.py b/astar.py new file mode 100644 index 0000000..c32ac84 --- /dev/null +++ b/astar.py @@ -0,0 +1,185 @@ +from operator import itemgetter +import settings +import copy + +class State: + def __init__(self, direction, x, y): + self.direction = direction + self.x = x + self.y = y + + def get_direction(self): + return self.direction + + def set_direction(self, direction): + self.direction = direction + + 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 + +class Node: + def __init__(self, action, direction, parent, x, y): + self.action = action + self.direction = direction + self.parent = parent + self.x = x + self.y = y + + def get_action(self): + return self.action + + def set_action(self, action): + self.action = action + + def get_direction(self): + return self.direction + + def set_direction(self, direction): + self.direction = direction + + def get_parent(self): + return self.parent + + def set_parent(self, parent): + self.parent = parent + + 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 cost(map, node): + cost = 0 + x = node.get_x() + y = node.get_y() + if map[x][y] == 'dirt': + cost = 5 + elif map[x][y] == 'grass': + cost = 3 + elif map[x][y] == 'cobble': + cost = 1 + elif map[x][y] == 'sand': + cost = 10 + elif map[x][y] == 'station': + cost = 0 + + return cost + + +def f(goaltest, map, node): + return cost(map, node) + heuristic(goaltest, node) + + +def goal_test(element,goaltest): + if element.get_x() == goaltest[0] and element.get_y() == goaltest[1]: + return True + else: + return False + + +def graphsearch(explored, f, fringe, goaltest, state, map, succ): + node = Node(None, state.get_direction(), None, state.get_x(), state.get_y()) + fringe.append((node, 0)) + while True: + if not fringe: return False + elem = fringe.pop(0) + temp = copy.copy(elem[0]) + if goal_test(elem[0], goaltest) is True: + return print_moves(elem[0]) + explored.append(elem) + for (action, state) in succ(temp): + fringe_tuple = [] + fringe_tuple_priority = [] + explored_tuple = [] + for (x, y) in fringe: + fringe_tuple.append((x.get_direction(), x.get_x(), x.get_y())) + fringe_tuple_priority.append(((x.get_direction(), x.get_x(), x.get_y()), y)) + for (x, y) in explored: + explored_tuple.append((x.get_direction(), x.get_x(), x.get_y())) + x = Node(action, state[0], elem[0], state[1], + state[2]) + p = f(goaltest, map, x) + if state not in fringe_tuple and state not in explored_tuple: + fringe.append((x, p)) + fringe = sorted(fringe, key=itemgetter(1)) + elif state in fringe_tuple: + i = 0 + for (state_prio, r) in fringe_tuple_priority: + if str(state_prio) == str(state): + if r > p: + fringe.insert(i, (x, p)) + fringe.pop(i + 1) + fringe = sorted(fringe, key=itemgetter(1)) + break + i = i + 1 + + +def heuristic(goaltest, node): + return abs(node.get_x() - goaltest[0]) + abs(node.get_y() - goaltest[1]) + +def print_moves(elem): + moves_list = [] + while (elem.get_parent() != None): + moves_list.append(elem.get_action()) + elem = elem.get_parent() + moves_list.reverse() + return moves_list + +def is_move_allowed(node): + if node.get_direction() == 2 and node.get_x() + 1 < settings.Pygame.width(): + return "x + 1" + elif node.get_direction() == 1 and node.get_y() - 1 >= 0: + return "y - 1" + elif node.get_direction() == 3 and node.get_y() + 1 < settings.Pygame.height(): + return "y + 1" + elif node.get_direction() == 4 and node.get_x() - 1 >= 0: + return "x - 1" + else: + return False + +def succ(node): + actions_list = [] + direction = copy.copy(node.get_direction()) + if direction == 1: + direction = 4 + else: + direction = direction - 1 + actions_list.append(("l", (direction, node.get_x(), node.get_y()))) + direction = copy.copy(node.get_direction()) + + if direction == 4: + direction = 1 + else: + direction = direction + 1 + actions_list.append(("r", (direction, node.get_x(), node.get_y()))) + temp_move_south = node.get_y() + 1 + temp_move_west = node.get_x() - 1 + temp_move_east = node.get_x() + 1 + temp_move_north = node.get_y() - 1 + + if is_move_allowed(node) == "x + 1": + actions_list.append(("f", (node.get_direction(), temp_move_east, node.get_y()))) + elif is_move_allowed(node) == "y - 1": + actions_list.append(("f", (node.get_direction(), node.get_x(), temp_move_north))) + elif is_move_allowed(node) == "y + 1": + actions_list.append(("f", (node.get_direction(), node.get_x(), temp_move_south))) + elif is_move_allowed(node) == "x - 1": + actions_list.append(("f", (node.get_direction(), temp_move_west, node.get_y()))) + return actions_list \ No newline at end of file diff --git a/main.py b/main.py index f7bf3a9..330bfad 100644 --- a/main.py +++ b/main.py @@ -7,6 +7,7 @@ import common import agent import math import time +import astar possibleFields = { 'dirt': field.Dirt(), @@ -121,8 +122,8 @@ def main(): pygame.display.set_caption(settings.Pygame.display_name()) fields, fields_2 = randomize_map() - state = graph.State(2, 0, 0) - move_list = (graph.graphsearch([], [], [10,10] , state, graph.succ)) + state = astar.State(2, 0, 0) + move_list = (astar.graphsearch([], astar.f,[], [10,10] , state, fields_2 ,astar.succ)) print(move_list) x = True while common.get('game_running'): -- 2.20.1