2024-04-24 22:16:34 +02:00
|
|
|
"""
|
|
|
|
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
|
2024-04-25 22:56:18 +02:00
|
|
|
"""
|
2024-04-26 16:32:50 +02:00
|
|
|
import random
|
2024-04-25 22:56:18 +02:00
|
|
|
import pygame
|
|
|
|
import Node
|
|
|
|
import BFS
|
|
|
|
from displayControler import NUM_X, NUM_Y
|
|
|
|
from Pole import stoneList
|
|
|
|
from queue import PriorityQueue
|
|
|
|
|
|
|
|
|
2024-04-27 00:59:48 +02:00
|
|
|
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
|
|
|
|
|
|
|
|
|
2024-04-25 22:56:18 +02:00
|
|
|
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
|
|
|
|
|
|
|
|
|
2024-04-28 11:51:47 +02:00
|
|
|
'''def get_cost_for_plant(plant_name):
|
2024-04-25 22:56:18 +02:00
|
|
|
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
|
2024-04-28 11:51:47 +02:00
|
|
|
'''
|
2024-04-25 22:56:18 +02:00
|
|
|
|
2024-04-27 00:59:48 +02:00
|
|
|
def A_star(istate, pole, goalTreasure):
|
2024-04-25 22:56:18 +02:00
|
|
|
# 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
|
2024-04-27 00:59:48 +02:00
|
|
|
|
|
|
|
# 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
|
|
|
|
|
2024-04-25 22:56:18 +02:00
|
|
|
fringe = PriorityQueue() # Kolejka priorytetowa dla wierzchołków do rozpatrzenia
|
|
|
|
explored = [] # Lista odwiedzonych stanów
|
2024-04-27 20:09:12 +02:00
|
|
|
obrot = 1
|
2024-04-25 22:56:18 +02:00
|
|
|
|
|
|
|
# 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
|
2024-04-27 20:09:12 +02:00
|
|
|
total_cost = 0
|
2024-04-25 22:56:18 +02:00
|
|
|
|
|
|
|
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 = []
|
2024-04-28 12:58:58 +02:00
|
|
|
cost_list=[]
|
2024-04-25 22:56:18 +02:00
|
|
|
while elem.parent is not None: # Odtworzenie ścieżki
|
|
|
|
path.append([elem.parent, elem.action])
|
|
|
|
elem = elem.parent
|
2024-04-27 20:09:12 +02:00
|
|
|
for node, action in path:
|
|
|
|
# Obliczanie kosztu ścieżki dla każdego pola i wyświetlanie
|
2024-04-28 12:58:58 +02:00
|
|
|
plant_cost = get_plant_name_and_cost_from_coordinates(node.state['x'],node.state['y'], pole)
|
2024-04-27 20:09:12 +02:00
|
|
|
if action == "left" or action == "right": # Liczenie kosztu tylko dla pól nie będących obrotami
|
|
|
|
total_cost += obrot
|
2024-04-28 12:58:58 +02:00
|
|
|
cost_list.append(obrot)
|
2024-04-27 20:09:12 +02:00
|
|
|
else:
|
|
|
|
total_cost += plant_cost
|
2024-04-28 12:58:58 +02:00
|
|
|
cost_list.append(plant_cost)
|
|
|
|
return path,cost_list,total_cost
|
2024-04-25 22:56:18 +02:00
|
|
|
|
|
|
|
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
|
2024-04-28 11:51:47 +02:00
|
|
|
plant_cost = get_plant_name_and_cost_from_coordinates(child_state['x'], child_state['y'], pole)
|
2024-04-25 22:56:18 +02:00
|
|
|
# Pobranie kosztu dla danej rośliny
|
2024-04-28 11:51:47 +02:00
|
|
|
#plant_cost = get_cost_for_plant(plant_name)
|
2024-04-25 22:56:18 +02:00
|
|
|
|
2024-04-27 20:09:12 +02:00
|
|
|
if child.action == "left" or child.action == "right":
|
|
|
|
child.g = elem.g + obrot
|
|
|
|
else:
|
|
|
|
child.g = elem.g + plant_cost
|
|
|
|
|
2024-04-25 22:56:18 +02:00
|
|
|
# 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
|
|
|
|
|
|
|
|
|
2024-04-28 11:51:47 +02:00
|
|
|
def get_plant_name_and_cost_from_coordinates(x, y, pole):
|
2024-04-25 22:56:18 +02:00
|
|
|
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
|
2024-04-28 11:51:47 +02:00
|
|
|
return slot.plant.stan.koszt # Zwrócenie nazwy rośliny na slocie
|
2024-04-25 22:56:18 +02:00
|
|
|
else:
|
2024-04-28 11:51:47 +02:00
|
|
|
return 0 # jeśli na slocie nie ma rośliny
|
2024-04-25 22:56:18 +02:00
|
|
|
else:
|
2024-04-28 11:51:47 +02:00
|
|
|
return 0 # jeśli podane współrzędne są poza polem
|
2024-04-25 22:56:18 +02:00
|
|
|
|
|
|
|
|
|
|
|
#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":
|
2024-04-26 16:32:50 +02:00
|
|
|
if state["y"] > 0 and (state['x'], state["y"] - 1) not in stoneList:
|
2024-04-25 22:56:18 +02:00
|
|
|
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":
|
2024-04-26 16:32:50 +02:00
|
|
|
if state["y"] < NUM_Y - 1 and (state['x'], state["y"] + 1) not in stoneList:
|
2024-04-25 22:56:18 +02:00
|
|
|
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":
|
2024-04-26 16:32:50 +02:00
|
|
|
if state["x"] < NUM_X - 1 and (state['x'] + 1, state["y"]) not in stoneList:
|
2024-04-25 22:56:18 +02:00
|
|
|
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"
|
2024-04-26 16:32:50 +02:00
|
|
|
if state["x"] > 0 and (state['x'] - 1, state["y"]) not in stoneList:
|
2024-04-25 22:56:18 +02:00
|
|
|
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
|
|
|
|
|
2024-04-26 16:32:50 +02:00
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def heuristic2(state, goal):
|
|
|
|
# Oblicz odległość Manhattanowską między aktualnym stanem a celem
|
2024-04-29 14:05:26 +02:00
|
|
|
manhattan_distance = (abs(state['x'] - goal[0]) + abs(state['y'] - goal[1])) * 2.5
|
2024-04-26 16:32:50 +02:00
|
|
|
return manhattan_distance
|
|
|
|
|
|
|
|
|
2024-04-27 00:59:48 +02:00
|
|
|
def A_star2(istate, pole, goalTreasure):
|
2024-04-26 16:32:50 +02:00
|
|
|
# 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
|
2024-04-27 00:59:48 +02:00
|
|
|
|
|
|
|
# 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
|
2024-04-26 16:32:50 +02:00
|
|
|
fringe = PriorityQueue() # Kolejka priorytetowa dla wierzchołków do rozpatrzenia
|
|
|
|
explored = [] # Lista odwiedzonych stanów
|
2024-04-28 12:58:58 +02:00
|
|
|
obrot = 1
|
2024-04-26 16:32:50 +02:00
|
|
|
|
|
|
|
# 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
|
2024-04-28 12:58:58 +02:00
|
|
|
total_cost=0
|
2024-04-26 16:32:50 +02:00
|
|
|
|
|
|
|
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 = []
|
2024-04-28 12:58:58 +02:00
|
|
|
cost_list=[]
|
2024-04-26 16:32:50 +02:00
|
|
|
while elem.parent is not None: # Odtworzenie ścieżki
|
|
|
|
path.append([elem.parent, elem.action])
|
|
|
|
elem = elem.parent
|
2024-04-28 12:58:58 +02:00
|
|
|
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
|
2024-04-26 16:32:50 +02:00
|
|
|
|
|
|
|
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
|
2024-04-28 11:51:47 +02:00
|
|
|
plant_cost = get_plant_name_and_cost_from_coordinates(child_state['x'], child_state['y'], pole)
|
2024-04-26 16:32:50 +02:00
|
|
|
|
2024-04-28 12:58:58 +02:00
|
|
|
if child.action == "left" or child.action == "right":
|
|
|
|
child.g = elem.g + obrot
|
|
|
|
else:
|
|
|
|
child.g = elem.g + plant_cost
|
2024-04-26 16:32:50 +02:00
|
|
|
# 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()
|
|
|
|
|
2024-04-29 14:05:26 +02:00
|
|
|
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))
|
|
|
|
"""
|