diff --git a/AStar.py b/AStar.py index 0a020cc..d12a80d 100644 --- a/AStar.py +++ b/AStar.py @@ -4,6 +4,7 @@ g(n) = dotychczasowy koszt -> dodać currentCost w Node lub brać koszt na nowo 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 @@ -40,7 +41,10 @@ def A_star(istate, pole): # 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 - goalTreasure = (18, 11) # Współrzędne celu + 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 @@ -116,28 +120,99 @@ def get_plant_name_from_coordinates(x, y, pole): def succ3A(state): resp = [] if state["direction"] == "N": - if state["y"] > 0 and (state['x'], state["y"] - 1) not in stoneList and is_field_available(state["x"], state["y"] - 1): + 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 and (state['x'], state["y"] + 1) not in stoneList and is_field_available(state["x"], state["y"] + 1): + 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 and (state['x'] + 1, state["y"]) not in stoneList and is_field_available(state["x"] + 1, state["y"]): + 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 and is_field_available(state["x"] - 1, state["y"]): + 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 is_field_available(x, y): - # Sprawdzenie, czy współrzędne pola znajdują się na polu i czy pole jest dostępne - return 0 <= x < NUM_X and 0 <= y < NUM_Y and (x, y) not in stoneList \ No newline at end of file + + + +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])) * 5.5 + return manhattan_distance + + +def A_star2(istate, pole): + # 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 + 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 + + # 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 + + 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 = [] + while elem.parent is not None: # Odtworzenie ścieżki + path.append([elem.parent, elem.action]) + elem = elem.parent + return path + + 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_name = get_plant_name_from_coordinates(child_state['x'], child_state['y'], pole) + # Pobranie kosztu dla danej rośliny + plant_cost = get_cost_for_plant(plant_name) + + # Obliczenie kosztu ścieżki dla dziecka + 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 \ No newline at end of file diff --git a/App.py b/App.py index 51b3ebf..fd65da2 100644 --- a/App.py +++ b/App.py @@ -14,8 +14,9 @@ import AStar bfs1_flag=False bfs2_flag=False #Change this lines to show different bfs implementation bfs3_flag=False -Astar = True -if bfs3_flag or Astar: +Astar = False +Astar2 = True +if bfs3_flag or Astar or Astar2: Pole.stoneFlag = True @@ -88,7 +89,14 @@ def init_demo(): #Demo purpose traktor.move_by_root(aStarRoot, pole, [traktor.irrigateSlot]) else: print_to_console("Nie można znaleźć ścieżki A*") # Wyświetl komunikat, jeśli nie znaleziono ścieżki - + if (Astar2): + aStarRoot2 = AStar.A_star2({'x': 0, 'y': 0, 'direction': "E"}, pole) + if aStarRoot2: + aStarRoot2.reverse() + print_to_console("Traktor porusza się obliczoną ścieżką A*") + traktor.move_by_root(aStarRoot2, pole, [traktor.irrigateSlot]) + else: + print_to_console("Nie można znaleźć ścieżki A*") # Wyświetl komunikat, jeśli nie znaleziono ścieżki start_flag=False diff --git a/BFS.py b/BFS.py index 3b59213..4324471 100644 --- a/BFS.py +++ b/BFS.py @@ -108,12 +108,12 @@ def succ3(state): 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 and (state['x'], state["y"] + 1) not in stoneList: + 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 and (state['x'] + 1, state["y"]) not in stoneList: + 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"}])