306 lines
10 KiB
Plaintext
306 lines
10 KiB
Plaintext
import sys
|
|
from queue import PriorityQueue
|
|
import numpy as np
|
|
|
|
import pygame
|
|
|
|
screen = []
|
|
objectArray = []
|
|
collisionsMap = []
|
|
|
|
|
|
class Position:
|
|
def __init__(self, x, y):
|
|
self.x = x
|
|
self.y = y
|
|
|
|
def get_moved(self, delta_x, delta_y):
|
|
return Position(self.x + delta_x, self.y + delta_y)
|
|
|
|
|
|
class Object:
|
|
def __init__(self, name, pos):
|
|
self.name = name
|
|
self.pos = pos
|
|
|
|
def draw(self, square):
|
|
leftTopX, leftTopY = 50 + self.pos.x * square, 10 + self.pos.y * square
|
|
pygame.draw.rect(screen, (0, 0, 0), (leftTopX, leftTopY, square, square))
|
|
|
|
|
|
def detect_collision(newPos):
|
|
return collisionsMap[newPos.x][newPos.y]
|
|
|
|
|
|
def position_in_grid(pos, gridLength):
|
|
return 0 <= pos.x < gridLength and 0 <= pos.y < gridLength
|
|
|
|
|
|
def movement_allowed(newPos, gridLength):
|
|
return position_in_grid(newPos, gridLength) and not detect_collision(newPos)
|
|
|
|
|
|
class Agent(Object):
|
|
def __init__(self, name, pos):
|
|
super().__init__(name, pos)
|
|
|
|
def draw(self, square):
|
|
## RYSUJEMY AGENTA
|
|
circleX = 52 + self.pos.x * square
|
|
circleY = 12 + self.pos.y * square
|
|
|
|
truck = pygame.image.load("car.png").convert_alpha() # tu ścieżka do zdjęcia w tle
|
|
truck = pygame.transform.scale(truck, (square, square))
|
|
screen.blit(truck, (circleX, circleY))
|
|
|
|
def move(self, event, gridLength):
|
|
if event.key == pygame.K_LEFT:
|
|
newPos = self.pos.get_moved(-1, 0)
|
|
self.move_if_possible(newPos, gridLength)
|
|
if event.key == pygame.K_RIGHT:
|
|
newPos = self.pos.get_moved(1, 0)
|
|
self.move_if_possible(newPos, gridLength)
|
|
if event.key == pygame.K_UP:
|
|
newPos = self.pos.get_moved(0, -1)
|
|
self.move_if_possible(newPos, gridLength)
|
|
if event.key == pygame.K_DOWN:
|
|
newPos = self.pos.get_moved(0, 1)
|
|
self.move_if_possible(newPos, gridLength)
|
|
|
|
def move_if_possible(self, newPos, gridLength):
|
|
if movement_allowed(newPos, gridLength):
|
|
self.pos = newPos
|
|
|
|
|
|
class House(Object):
|
|
def __init__(self, name, pos):
|
|
super().__init__(name, pos)
|
|
self.trash_cans = {
|
|
"paper": False,
|
|
"glass": False,
|
|
"plastic": False,
|
|
"bio": False
|
|
}
|
|
|
|
def draw(self, square):
|
|
x = 52 + self.pos.x * square
|
|
y = 12 + self.pos.y * square
|
|
|
|
house = pygame.image.load("house.png").convert_alpha() # tu ścieżka do zdjęcia w tle
|
|
house = pygame.transform.scale(house, (square, square))
|
|
screen.blit(house, (x, y))
|
|
|
|
|
|
class Junkyard(Object):
|
|
def __init__(self, name, pos):
|
|
super().__init__(name, pos)
|
|
self.heaps = {
|
|
"paper": True,
|
|
"glass": True,
|
|
"plastic": True,
|
|
"bio": True
|
|
}
|
|
|
|
def draw(self, square):
|
|
x = 52 + self.pos.x * square
|
|
y = 12 + self.pos.y * square
|
|
|
|
junkyard = pygame.image.load("junkyard.png").convert_alpha() # tu ścieżka do zdjęcia w tle
|
|
junkyard = pygame.transform.scale(junkyard, (square, square))
|
|
screen.blit(junkyard, (x, y))
|
|
|
|
|
|
class Hole(Object):
|
|
def __init__(self, name, pos):
|
|
super().__init__(name, pos)
|
|
|
|
def draw(self, square):
|
|
x = 52 + self.pos.x * square
|
|
y = 12 + self.pos.y * square
|
|
|
|
hole = pygame.image.load("hole.png").convert_alpha() # tu ścieżka do zdjęcia w tle
|
|
hole = pygame.transform.scale(hole, (square, square))
|
|
screen.blit(hole, (x, y))
|
|
|
|
|
|
def draw(square_num, objectArr):
|
|
# następne dwie linijki do odkomentowania, jak będzie wgrane zdjęcie do tła
|
|
# background = pygame.image.load("ścieżka do pliku").convert() #tu ścieżka do zdjęcia w tle
|
|
# screen.blit(background, (0, 0))
|
|
grid_color = (0, 0, 0) # kolor czarny
|
|
|
|
grid_size = 510 # rozmiar kraty
|
|
square = grid_size // square_num # rozmiar pojedyńczego kwadracika
|
|
|
|
a = 50
|
|
b = 10 # odległości kraty od krawędzi okna
|
|
|
|
for o in objectArr:
|
|
o.draw(square)
|
|
|
|
for i in range(square_num):
|
|
pygame.draw.line(screen, grid_color, (a + i * square, b), (a + i * square, b + grid_size), 2)
|
|
|
|
pygame.draw.line(screen, grid_color, (a, b + i * square), (a + grid_size, b + i * square), 2)
|
|
|
|
pygame.draw.line(screen, grid_color, (a, b + grid_size), (a + grid_size,
|
|
b + grid_size), 2)
|
|
pygame.draw.line(screen, grid_color, (a + grid_size, b),
|
|
(a + grid_size, b + grid_size), 2)
|
|
|
|
|
|
def kb_listen(objectArray, gridLength):
|
|
agent = objectArray[0]
|
|
for event in pygame.event.get():
|
|
if event.type == pygame.KEYDOWN:
|
|
agent.move(event, gridLength)
|
|
if event.type == pygame.QUIT:
|
|
sys.exit()
|
|
|
|
|
|
#moje
|
|
|
|
def manhattan(node1, node2):
|
|
x1, y1 = node1.state[0], node1.state[1]
|
|
x2, y2 = node2.state[0], node2.state[1]
|
|
distance = abs(x1 - x2) + abs(y1 - y2)
|
|
return distance
|
|
|
|
|
|
class Node: #prawie jak Field w bfs
|
|
def __init__(self, state, parent, action):
|
|
self.state = state #position - (x, y, direction)
|
|
self.parent = parent
|
|
self.action = action
|
|
|
|
def __eq__(self, other):
|
|
return True
|
|
|
|
def __lt__(self, other):
|
|
return True
|
|
|
|
|
|
def f(state):#tablica z losowymi wagami(kosztami) pól, w astar trzeba zsumować wagę pola z heurystyką - f + manhattan
|
|
weights = np.array([
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 8],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 4],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
|
|
[9, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 3],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 6],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 7],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 1],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2],
|
|
[1, 2, 1, 4, 5, 2, 7, 8, 1, 4, 1, 3, 4, 5, 2]
|
|
])
|
|
pos_x = state[0]
|
|
pos_y = state[1]
|
|
|
|
return weights[pos_x][pos_y]
|
|
|
|
|
|
def succ1(state):
|
|
successors = []#-90 obrót w lewo, +90 obrót w prawo
|
|
print(state[0])#operujemy na 0, 90, 180, 270
|
|
print(state[1])
|
|
right = state[2] + 90
|
|
left = state[2] - 90
|
|
if right == 360:
|
|
right = 0
|
|
successors.append((("turn right"), (state[0], state[1], right)))
|
|
if left == -90:
|
|
left = 270
|
|
successors.append((("turn left"), (state[0], state[1], left)))
|
|
if (state[0], state[1]) not in black_list:#działa
|
|
if state[2] == 0 and state[0] < 14:
|
|
new_x = state[0]+1
|
|
successors.append((("move forward"), (new_x, state[1], state[2])))
|
|
elif state[2] == 90 and state[1] > 0:
|
|
new_y = state[1]-1
|
|
successors.append((("move forward"), (state[0], new_y, state[2])))
|
|
elif state[2] == 180 and state[0] > 0:
|
|
new_x = state[0]-1
|
|
successors.append((("move forward"), (new_x, state[1], state[2])))
|
|
elif state[2] == 270 and state[1] < 14:
|
|
new_y = state[1]+1
|
|
successors.append((("move forward"), (state[0], new_y, state[2])))
|
|
return successors
|
|
|
|
|
|
def algorithm():
|
|
opened = PriorityQueue()#może być też lista
|
|
closed = []#już odwiedzone, odrzucone wierzchołki
|
|
list_of_actions = []
|
|
|
|
first_state = (0, 0, 0)#x, y, kierunek
|
|
final_state = (14, 14, 0)
|
|
starting_point = Node(first_state, False, False)
|
|
ending_point = Node(final_state, False, False)
|
|
|
|
pos1 = (starting_point.state[0], starting_point.state[1])
|
|
pos2 = (ending_point.state[0], ending_point.state[1])
|
|
|
|
opened.put((1, starting_point))
|
|
|
|
while not opened.empty():
|
|
elem = opened.get()[1]#[1] bo inaczej elem nie ma state
|
|
if elem.state[0] == ending_point.state[0] and elem.state[1] == ending_point.state[1]:
|
|
while elem.action is not False:
|
|
list_of_actions.insert(0, elem.action)
|
|
elem = elem.parent
|
|
return list_of_actions
|
|
if elem.state not in closed:
|
|
closed.append(elem.state)
|
|
for (action, state) in succ1(elem.state):
|
|
point = Node(state, elem, action)
|
|
score = manhattan(point, ending_point) + f(point.state)
|
|
print(score)
|
|
if state not in opened.queue:
|
|
opened.put((score, point))
|
|
if state in opened.queue:
|
|
opened.queue.remove(elem)
|
|
opened.put((score, point))
|
|
|
|
|
|
if __name__ == '__main__':
|
|
f((1, 1, 0))
|
|
pygame.init() # inicjalizacja modułów, na razie niepotrzebna
|
|
gridSize = 15
|
|
|
|
# Tworzymy nowego playera, czy tam agenta
|
|
agent = Agent("smieciarka", Position(0, 0))
|
|
junkyard = Junkyard("wysypisko", Position(10, 10))
|
|
houses = [House(f'dom-{i}', pos) for i, pos in enumerate([Position(x, y) for x, y in [
|
|
(7, 4), (3, 10), (8, 10), (4, 5), (1, 2), (10, 4), (13, 14), (6, 9)
|
|
]])]
|
|
holes = [Hole(f'dziura-{i}', pos) for i, pos in enumerate([Position(x, y) for x, y in [
|
|
(4, 9), (5, 11), (11, 7), (13, 8)
|
|
]])]
|
|
objectArray.append(agent)
|
|
objectArray.append(junkyard)
|
|
objectArray += houses
|
|
objectArray += holes
|
|
|
|
collisionsMap = [[False] * gridSize for _ in range(gridSize)]
|
|
for object in objectArray[1:]:
|
|
collisionsMap[object.pos.x][object.pos.y] = True
|
|
|
|
black_list = [(10, 10), (7, 4), (3, 10), (8, 10), (4, 5), (1, 2), (10, 4), (13, 14), (6, 9), (4, 9), (5, 11), (11, 7), (13, 8)]
|
|
c = algorithm()
|
|
print(c)
|
|
|
|
width = 610
|
|
height = 530
|
|
screen = pygame.display.set_mode((width, height)) # ustalanie rozmiarów okna
|
|
|
|
while 1:
|
|
c = (255, 255, 255) # tymczasowy kolor tła - do usunięcia, jak już będzie zdjęcie
|
|
screen.fill(c)
|
|
draw(gridSize, objectArray)
|
|
kb_listen(objectArray, gridSize)
|
|
pygame.display.update() # by krata pojawiła się w okienku - update powierzc |