""" f(n) = g(n) + h(n) g(n) = dotychczasowy koszt -> dodać currentCost w Node lub brać koszt na nowo przy oddtawrzaniu ścieżki h(n) = abs(state['x'] - goalTreassure[0]) + abs(state['y'] - goalTreassure[1]) -> odległość Manhatan -> można zrobić jeszcze drugą wersje gdzie mnoży się razy 5.5 ze wzgledu na średni koszt przejścia Należy zaimplementować kolejkę priorytetową oraz zaimplementować algorytm przeszukiwania grafu stanów z uwzględnieniem kosztu za pomocą przerobienia algorytmu przeszukiwania grafu stanów """ import random import pygame import Node import BFS from displayControler import NUM_X, NUM_Y from Pole import stoneList from queue import PriorityQueue def getRandomGoalTreasure(): while True: goalTreasure = (random.randint(0, NUM_X - 1), random.randint(0, NUM_Y - 1)) # Współrzędne celu if goalTreasure not in stoneList: break return goalTreasure def heuristic(state, goal): # Oblicz odległość Manhattanowską między aktualnym stanem a celem manhattan_distance = abs(state['x'] - goal[0]) + abs(state['y'] - goal[1]) return manhattan_distance '''def get_cost_for_plant(plant_name): plant_costs = { "pszenica": 7, "kukurydza": 9, "ziemniak": 2, "slonecznik": 5, "borowka": 3, "winogrono": 4, "mud": 15, "dirt": 0, } if plant_name in plant_costs: return plant_costs[plant_name] else: # Jeśli nazwa rośliny nie istnieje w słowniku, zwróć domyślną wartość return 0 ''' def A_star(istate, pole, goalTreasure): # goalTreasure = (random.randint(0,NUM_X-1), random.randint(0,NUM_Y-1)) # #jeśli chcemy używać random musimy wykreslić sloty z kamieniami, ponieważ tez mogą się wylosować i wtedy traktor w ogóle nie rusza #lub zrobić to jakoś inaczej, np. funkcja szukająca najmniej nawodnionej rośliny # przeniesione wyżej do funkcji getRandomGoalTreasure, wykorzystywana jest w App.py # while True: # goalTreasure = (random.randint(0, NUM_X - 1), random.randint(0, NUM_Y - 1)) # Współrzędne celu # if goalTreasure not in stoneList: # break fringe = PriorityQueue() # Kolejka priorytetowa dla wierzchołków do rozpatrzenia explored = [] # Lista odwiedzonych stanów obrot = 1 # Tworzenie węzła początkowego x = Node.Node(istate) x.g = 0 x.h = heuristic(x.state, goalTreasure) fringe.put((x.g + x.h, x)) # Dodanie węzła do kolejki total_cost = 0 while not fringe.empty(): _, elem = fringe.get() # Pobranie węzła z najniższym priorytetem if BFS.goalTest3(elem.state, goalTreasure): # Sprawdzenie, czy osiągnięto cel path = [] cost_list=[] while elem.parent is not None: # Odtworzenie ścieżki path.append([elem.parent, elem.action]) elem = elem.parent for node, action in path: # Obliczanie kosztu ścieżki dla każdego pola i wyświetlanie plant_cost = get_plant_name_and_cost_from_coordinates(node.state['x'],node.state['y'], pole) if action == "left" or action == "right": # Liczenie kosztu tylko dla pól nie będących obrotami total_cost += obrot cost_list.append(obrot) else: total_cost += plant_cost cost_list.append(plant_cost) return path,cost_list,total_cost explored.append(elem.state) for resp in succ3A(elem.state): child_state = resp[1] if child_state not in explored: child = Node.Node(child_state) child.parent = elem child.action = resp[0] # Pobranie nazwy rośliny z danego slotu na podstawie współrzędnych plant_cost = get_plant_name_and_cost_from_coordinates(child_state['x'], child_state['y'], pole) # Pobranie kosztu dla danej rośliny #plant_cost = get_cost_for_plant(plant_name) if child.action == "left" or child.action == "right": child.g = elem.g + obrot else: child.g = elem.g + plant_cost # Obliczenie heurystyki dla dziecka child.h = heuristic(child.state, goalTreasure) in_fringe = False for priority, item in fringe.queue: if item.state == child.state: in_fringe = True if priority > child.g + child.h: # Jeśli znaleziono węzeł w kolejce o gorszym priorytecie, zastąp go nowym fringe.queue.remove((priority, item)) fringe.put((child.g + child.h, child)) break if not in_fringe: # Jeśli stan dziecka nie jest w kolejce, dodaj go do kolejki fringe.put((child.g + child.h, child)) for event in pygame.event.get(): if event.type == pygame.QUIT: quit() return False def get_plant_name_and_cost_from_coordinates(x, y, pole): if (x, y) in pole.slot_dict: # Sprawdzenie, czy podane współrzędne znajdują się na polu slot = pole.slot_dict[(x, y)] # Pobranie slotu na podstawie współrzędnych if slot.plant: # Sprawdzenie, czy na slocie znajduje się roślina return slot.plant.stan.koszt # Zwrócenie nazwy rośliny na slocie else: return 0 # jeśli na slocie nie ma rośliny else: return 0 # jeśli podane współrzędne są poza polem #to ogólnie identyczna funkcja jak w BFS ale nie chciałam tam ruszać, żeby przypadkiem nie zapsuć do BFS, #tylko musiałam dodac sprawdzenie kolizji, bo traktor brał sloty z Y których nie ma na planszy def succ3A(state): resp = [] if state["direction"] == "N": if state["y"] > 0 and (state['x'], state["y"] - 1) not in stoneList: resp.append(["forward", {'x': state["x"], 'y': state["y"]-1, 'direction': state["direction"]}]) resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "E"}]) resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "W"}]) elif state["direction"] == "S": if state["y"] < NUM_Y - 1 and (state['x'], state["y"] + 1) not in stoneList: resp.append(["forward", {'x': state["x"], 'y': state["y"]+1, 'direction': state["direction"]}]) resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "W"}]) resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "E"}]) elif state["direction"] == "E": if state["x"] < NUM_X - 1 and (state['x'] + 1, state["y"]) not in stoneList: resp.append(["forward", {'x': state["x"]+1, 'y': state["y"], 'direction': state["direction"]}]) resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "S"}]) resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "N"}]) else: #state["direction"] == "W" if state["x"] > 0 and (state['x'] - 1, state["y"]) not in stoneList: resp.append(["forward", {'x': state["x"]-1, 'y': state["y"], 'direction': state["direction"]}]) resp.append(["right", {'x': state["x"], 'y': state["y"], 'direction': "N"}]) resp.append(["left", {'x': state["x"], 'y': state["y"], 'direction': "S"}]) return resp def heuristic2(state, goal): # Oblicz odległość Manhattanowską między aktualnym stanem a celem manhattan_distance = (abs(state['x'] - goal[0]) + abs(state['y'] - goal[1])) * 2.5 return manhattan_distance def A_star2(istate, pole, goalTreasure): # goalTreasure = (random.randint(0,NUM_X-1), random.randint(0,NUM_Y-1)) # #jeśli chcemy używać random musimy wykreslić sloty z kamieniami, ponieważ tez mogą się wylosować i wtedy traktor w ogóle nie rusza #lub zrobić to jakoś inaczej, np. funkcja szukająca najmniej nawodnionej rośliny # przeniesione wyżej do funkcji getRandomGoalTreasure, wykorzystywana jest w App.py # while True: # goalTreasure = (random.randint(0, NUM_X - 1), random.randint(0, NUM_Y - 1)) # Współrzędne celu # if goalTreasure not in stoneList: # break fringe = PriorityQueue() # Kolejka priorytetowa dla wierzchołków do rozpatrzenia explored = [] # Lista odwiedzonych stanów obrot = 1 # Tworzenie węzła początkowego x = Node.Node(istate) x.g = 0 x.h = heuristic2(x.state, goalTreasure) fringe.put((x.g + x.h, x)) # Dodanie węzła do kolejki total_cost=0 while not fringe.empty(): _, elem = fringe.get() # Pobranie węzła z najniższym priorytetem if BFS.goalTest3(elem.state, goalTreasure): # Sprawdzenie, czy osiągnięto cel path = [] cost_list=[] while elem.parent is not None: # Odtworzenie ścieżki path.append([elem.parent, elem.action]) elem = elem.parent for node, action in path: # Obliczanie kosztu ścieżki dla każdego pola i wyświetlanie plant_cost = get_plant_name_and_cost_from_coordinates(node.state['x'],node.state['y'], pole) if action == "left" or action == "right": # Liczenie kosztu tylko dla pól nie będących obrotami total_cost += obrot cost_list.append(obrot) else: total_cost += plant_cost cost_list.append(plant_cost) return path,cost_list,total_cost explored.append(elem.state) for resp in succ3A(elem.state): child_state = resp[1] if child_state not in explored: child = Node.Node(child_state) child.parent = elem child.action = resp[0] # Pobranie nazwy rośliny z danego slotu na podstawie współrzędnych plant_cost = get_plant_name_and_cost_from_coordinates(child_state['x'], child_state['y'], pole) if child.action == "left" or child.action == "right": child.g = elem.g + obrot else: child.g = elem.g + plant_cost # Obliczenie heurystyki dla dziecka child.h = heuristic2(child.state, goalTreasure) in_fringe = False for priority, item in fringe.queue: if item.state == child.state: in_fringe = True if priority > child.g + child.h: # Jeśli znaleziono węzeł w kolejce o gorszym priorytecie, zastąp go nowym fringe.queue.remove((priority, item)) fringe.put((child.g + child.h, child)) break if not in_fringe: # Jeśli stan dziecka nie jest w kolejce, dodaj go do kolejki fringe.put((child.g + child.h, child)) for event in pygame.event.get(): if event.type == pygame.QUIT: quit() return False """ TO TEST SPEED OF ASTAR test_speed = False if test_speed: time1 = 0 time2 = 0 cost1 = 0 cost2 = 0 for i in range(500): print(i) start = time.time() aStarRoot, cost_list, total_cost = AStar.A_star({'x': 0, 'y': 0, 'direction': "E"}, pole, goalTreasure) end = time.time() time1 += end - start cost1 += total_cost start = time.time() aStarRoot2, cost_list, total_cost = AStar.A_star2({'x': 0, 'y': 0, 'direction': "E"}, pole, goalTreasure) end = time.time() time2 += end - start cost2 += total_cost print(time1, time2) print(float(cost1 / 1000), float(cost2 / 1000)) """