259 lines
8.7 KiB
Plaintext
259 lines
8.7 KiB
Plaintext
from asyncio.windows_events import NULL
|
|
from collections import deque
|
|
from lib2to3.pgen2.token import CIRCUMFLEX
|
|
from queue import Queue
|
|
import sys
|
|
from unicodedata import name
|
|
import pygame
|
|
import random
|
|
import time
|
|
|
|
screen = []
|
|
objectArray = []
|
|
pathToPrint = []
|
|
|
|
|
|
class Object:
|
|
def __init__(self, name, xPos, yPos):
|
|
self.name = name
|
|
self.xPos = xPos
|
|
self.yPos = yPos
|
|
|
|
class Field:
|
|
def __init__(self, xPos, yPos, isObject):
|
|
self.xPos = xPos
|
|
self.yPos = yPos
|
|
self.visited = False
|
|
self.parent = None
|
|
number = random.randint(0, 9)
|
|
if number > 5 and not isObject:
|
|
self.isBlock = True
|
|
else:
|
|
self.isBlock = False
|
|
|
|
def printXandY(self):
|
|
print("X: " + str(self.xPos) + ". Y: " + str(self.yPos))
|
|
|
|
|
|
def draw(square_num, objectArr, pathToTarget, num):
|
|
grid_color = (0, 0, 0)
|
|
grid_size = 500
|
|
square = grid_size/square_num
|
|
a = 50
|
|
b = 10
|
|
|
|
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)
|
|
|
|
circleX = objectArr[0].xPos * square + square + square / 2
|
|
circleY = objectArr[0].yPos * square + square / 2
|
|
|
|
pygame.draw.circle(screen, (0, 0, 255), (circleX + 15, circleY + 15), 8)
|
|
|
|
circleX = objectArray[1].xPos * square + square * 2
|
|
circleY = objectArr[1].yPos * square + square
|
|
radius = 8
|
|
grid_color = (255, 0, 0)
|
|
pygame.draw.circle(screen, grid_color, (circleX, circleY), radius)
|
|
|
|
|
|
def kb_listen(objectArray, gridLength):
|
|
for event in pygame.event.get():
|
|
if event.type == pygame.KEYDOWN:
|
|
if event.key == pygame.K_LEFT and objectArray[0].xPos > 0:
|
|
objectArray[0].xPos = objectArray[0].xPos - 1
|
|
print(objectArray[0].xPos)
|
|
if event.key == pygame.K_RIGHT and objectArray[0].xPos < gridLength - 1:
|
|
objectArray[0].xPos = objectArray[0].xPos + 1
|
|
print(objectArray[0].xPos)
|
|
if event.key == pygame.K_UP and objectArray[0].yPos > 0:
|
|
objectArray[0].yPos = objectArray[0].yPos - 1
|
|
print(objectArray[0].yPos)
|
|
if event.key == pygame.K_DOWN and objectArray[0].yPos < gridLength - 1:
|
|
objectArray[0].yPos = objectArray[0].yPos + 1
|
|
print(objectArray[0].yPos)
|
|
if event.type == pygame.QUIT:
|
|
sys.exit()
|
|
|
|
|
|
fieldQueue = []
|
|
isGoalAchieved = False
|
|
|
|
def startQueue(agentX, agentY, fieldList, gridNum):
|
|
currentX = agentX
|
|
currentY = agentY
|
|
currentIndex = currentY * gridNum + currentX
|
|
|
|
fieldQueue.append(fieldList[currentIndex])
|
|
|
|
checkFlag = False
|
|
|
|
while not checkFlag:
|
|
result = checkGoal(fieldList, gridNum)
|
|
if result:
|
|
checkFlag = True
|
|
return result
|
|
|
|
|
|
def checkGoal(fieldList, gridNum):
|
|
currentField = fieldQueue.pop(0)
|
|
|
|
currentY = currentField.yPos
|
|
currentX = currentField.xPos
|
|
currentIndex = currentY * gridNum + currentX
|
|
|
|
# SPRAWDZAMY CZY DANE POLE JEST POLEM W KTORYM ZNAJDUJE SIE NASZ CEL
|
|
if currentField.xPos == objectArray[1].xPos and currentField.yPos == objectArray[1].yPos:
|
|
return currentField
|
|
|
|
# SPRAWDZAMY CZY DANE POLE ZOSTALO JUZ WCZESNIEJ ODWIEDZONE - JEZELI NIE TO USTAWIAMY STATUS
|
|
if not currentField.visited:
|
|
currentField.visited = True
|
|
|
|
# JEZELI DANE POLE NIE ZOSTALO ODWIEDZONE, TO DODAJEMY DO KOLEJKI WSZYSTKIE POLA OBOK I LECIMY DALEJ
|
|
if currentIndex + 15 <= 224 and fieldList[currentIndex + 15].visited == False and fieldList[currentIndex + 15].isBlock == False:
|
|
fieldQueue.append(fieldList[currentIndex + 15])
|
|
fieldList[currentIndex + 15].parent = fieldList[currentIndex]
|
|
if currentIndex - 15 > -1 and fieldList[currentIndex - 15].visited == False and fieldList[currentIndex - 15].isBlock == False:
|
|
fieldQueue.append(fieldList[currentIndex - 15])
|
|
fieldList[currentIndex - 15].parent = fieldList[currentIndex]
|
|
if (currentIndex + 1) % 15 != 0 and fieldList[currentIndex + 1].visited == False and fieldList[currentIndex + 1].isBlock == False:
|
|
fieldQueue.append(fieldList[currentIndex + 1])
|
|
fieldList[currentIndex + 1].parent = fieldList[currentIndex]
|
|
if (currentIndex - 1) % 15 != 14 and not currentIndex - 1 < 0 and fieldList[currentIndex - 1].visited == False and fieldList[currentIndex - 1].isBlock == False:
|
|
fieldQueue.append(fieldList[currentIndex - 1])
|
|
fieldList[currentIndex - 1].parent = fieldList[currentIndex]
|
|
|
|
def generateStartEndPos():
|
|
startX = random.randint(0, 14)
|
|
startY = random.randint(0, 14)
|
|
targetX = None
|
|
targetY = None
|
|
|
|
while targetX == None and targetY == None or targetX == startX and targetY == startY:
|
|
targetX = random.randint(0, 14)
|
|
targetY = random.randint(0, 14)
|
|
|
|
return [startX, startY, targetX, targetY]
|
|
|
|
|
|
|
|
def drawFull(index):
|
|
c = (255, 255, 255)
|
|
screen.fill(c)
|
|
|
|
for node in pathToTarget:
|
|
if node == pathToTarget[0] or node == pathToTarget[-1]:
|
|
continue
|
|
if pathToTarget.index(node) <= index:
|
|
continue
|
|
green = (0, 255, 0)
|
|
pygame.draw.circle(screen, green, ((node.xPos + 1)
|
|
* 500 / 15 + 30, (node.yPos + 1) * 500 / 15), 5)
|
|
draw(15, objectArray, pathToTarget, 1)
|
|
|
|
for node in fields:
|
|
if node.isBlock == True:
|
|
black = (0, 0, 0)
|
|
pygame.draw.rect(screen, black, pygame.Rect((node.xPos + 1) * 500 / 15 + 30, (node.yPos + 1) * 500 / 15 - 10, 10, 10))
|
|
|
|
##kb_listen(objectArray, 15)
|
|
pygame.display.update()
|
|
|
|
if __name__ == '__main__':
|
|
pygame.init()
|
|
|
|
width = 600
|
|
height = 530
|
|
screen = pygame.display.set_mode(
|
|
(width, height))
|
|
|
|
[sX, sY, eX, eY] = generateStartEndPos()
|
|
|
|
fields = []
|
|
for y in range(15):
|
|
for x in range(15):
|
|
if x == sX and y == sY or x == eX and y == eY:
|
|
newField = Field(x, y, True)
|
|
else:
|
|
newField = Field(x, y, False)
|
|
fields.append(newField)
|
|
|
|
agent = Object("AGENT", sX, sY)
|
|
target = Object("TARGET", eX, eY)
|
|
|
|
objectArray.append(agent)
|
|
objectArray.append(target)
|
|
|
|
result = startQueue(objectArray[0].xPos,
|
|
objectArray[0].yPos, fields, 15)
|
|
|
|
|
|
print("RED DOT X AND Y POSITION ============================")
|
|
print("X: " + str(objectArray[1].xPos) + " Y: " + str(objectArray[1].yPos))
|
|
print("TRUCK X AND Y POSITION ============================")
|
|
print("X: " + str(objectArray[0].xPos) + " Y: " + str(objectArray[0].yPos))
|
|
|
|
result_parent = result.parent
|
|
|
|
currentIndex = (objectArray[0].yPos) * 15 + objectArray[0].xPos
|
|
pathToTarget = deque()
|
|
pathToTarget.appendleft(result)
|
|
|
|
while result_parent.parent != None:
|
|
pathToTarget.appendleft(result_parent)
|
|
result_parent = result_parent.parent
|
|
pathToTarget.appendleft(fields[currentIndex])
|
|
|
|
print("PRINTING PATH TO TARGET ============================")
|
|
for node in pathToTarget:
|
|
node.printXandY()
|
|
|
|
print("============================ SEARCH PROBLEM DIAGRAM ============================")
|
|
for node in pathToTarget:
|
|
currentNode = node
|
|
parentNode = node.parent
|
|
direction = None
|
|
|
|
index = pathToTarget.index(node)
|
|
|
|
if parentNode == None:
|
|
continue
|
|
|
|
if currentNode.xPos != parentNode.xPos:
|
|
if currentNode.xPos > parentNode.xPos:
|
|
direction = "EAST"
|
|
objectArray[0].xPos = objectArray[0].xPos + 1
|
|
else:
|
|
direction = "WEST"
|
|
objectArray[0].xPos = objectArray[0].xPos - 1
|
|
else:
|
|
if currentNode.yPos > parentNode.yPos:
|
|
direction = "SOUTH"
|
|
objectArray[0].yPos = objectArray[0].yPos + 1
|
|
else:
|
|
direction = "NORTH"
|
|
objectArray[0].yPos = objectArray[0].yPos - 1
|
|
time.sleep(1)
|
|
print("---- NEXT STEP ---- ")
|
|
print("AGENT TURNING TO " + direction)
|
|
print("AGENT MOVING FORWARD")
|
|
print("ACTUAL AGENT STATE: (X -> " + str(objectArray[0].xPos) + ", Y -> " + str(objectArray[0].yPos) + ", " + direction + ")")
|
|
|
|
draw(15, objectArray, pathToTarget, 1)
|
|
pygame.display.update()
|
|
drawFull(index)
|
|
|
|
time.sleep(5)
|
|
quit()
|
|
|