s481876/wozek
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added A*

Browse Source
This commit is contained in:
s481876 2024-05-09 08:12:21 +02:00
parent 35928a17a4
commit 6b425159fb
1 changed files with 83 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
ai-wozek/wozek.py
View File

@ -3,14 +3,22 @@ import sys
import random import random
import os import os
import time import time
from collections import deque
import heapq
from classes import * from classes import *
class Node(): class Node():
def __init__(self,position,rotation,action,parent): def __init__(self,position,rotation,action,parent,cost):
self.position=position self.position=position
self.rotation=rotation self.rotation=rotation
self.action=action self.action=action
self.parent=parent self.parent=parent
self.cost=cost
def __lt__(self, other):
return (self.cost < other.cost)
def __le__(self, other):
return (self.cost <= other.cost)
# Initialize Pygame # Initialize Pygame
pygame.init() pygame.init()
@ -47,6 +55,14 @@ carrying_freight = False
carried_freight = None carried_freight = None
freight_positions = {} # Dictionary to keep track of freight positions and types freight_positions = {} # Dictionary to keep track of freight positions and types
tile_cost={}
tile_cost[(8,0)]=10
tile_cost[(7,1)]=10
tile_cost[(6,1)]=3
tile_cost[(5,0)]=10
# Load images # Load images
def load_images(): def load_images():
global forklift_image_full, freight_images_full global forklift_image_full, freight_images_full
@ -92,6 +108,10 @@ def draw_truck_bed_and_racks():
for y in range(5, 8): for y in range(5, 8):
for x in range(GRID_WIDTH): for x in range(GRID_WIDTH):
pygame.draw.rect(screen, (165, 42, 42), (x * TILE_SIZE, y * TILE_SIZE, TILE_SIZE, TILE_SIZE)) pygame.draw.rect(screen, (165, 42, 42), (x * TILE_SIZE, y * TILE_SIZE, TILE_SIZE, TILE_SIZE))
for key in tile_cost:
x=key[0]
y=key[1]
pygame.draw.rect(screen, (10*tile_cost[key], 130, 100), (x * TILE_SIZE, y * TILE_SIZE, TILE_SIZE, TILE_SIZE))
def draw_forklift_and_freight(): def draw_forklift_and_freight():
@ -158,15 +178,16 @@ def succ(current_node):
current_rotation=current_node.rotation current_rotation=current_node.rotation
x=current_node.position[0] x=current_node.position[0]
y=current_node.position[1] y=current_node.position[1]
current_cost=tile_cost.get((x,y),1)
successors=[] successors=[]
if(current_rotation=="N"): if(current_rotation=="N"):
if(y<7): if(y>0):
pos=[] pos=[]
pos.append(x) pos.append(x)
pos.append(y+1) pos.append(y-1)
action='FW' action='FW'
successor=Node(pos,current_rotation,action,current_node) successor=Node(pos,current_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
if(x>0): if(x>0):
pos = [] pos = []
@ -174,7 +195,7 @@ def succ(current_node):
pos.append(y) pos.append(y)
new_rotation='W' new_rotation='W'
action='L' action='L'
successor = Node(pos, new_rotation,action,current_node) successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
if(x<15): if(x<15):
pos = [] pos = []
@ -182,33 +203,33 @@ def succ(current_node):
pos.append(y) pos.append(y)
new_rotation='E' new_rotation='E'
action='R' action='R'
successor = Node(pos, new_rotation,action,current_node) successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
elif (current_rotation == "S"): elif (current_rotation == "S"):
if (y > 0): if (y < 7):
pos = [] pos = []
pos.append(x) pos.append(x)
pos.append(y - 1) pos.append(y + 1)
action = 'FW' action = 'FW'
successor = Node(pos, current_rotation,action,current_node) successor = Node(pos, current_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
if (x > 0): if (x <15):
pos = [] pos = []
pos.append(x) pos.append(x)
pos.append(y) pos.append(y)
new_rotation = 'E' new_rotation = 'E'
action='L' action='L'
successor = Node(pos, new_rotation,action,current_node) successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
if (x < 15): if (x > 0):
pos = [] pos = []
pos.append(x) pos.append(x)
pos.append(y) pos.append(y)
new_rotation = 'W' new_rotation = 'W'
action = 'R' action = 'R'
successor = Node(pos, new_rotation,action,current_node) successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
elif (current_rotation == "E"): elif (current_rotation == "E"):
@ -217,62 +238,65 @@ def succ(current_node):
pos.append(x+1) pos.append(x+1)
pos.append(y) pos.append(y)
action = 'FW' action = 'FW'
successor = Node(pos, current_rotation,action,current_node) successor = Node(pos, current_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
if (y <7): if (y <7):
pos = [] pos = []
pos.append(x) pos.append(x)
pos.append(y) pos.append(y)
new_rotation = 'N' new_rotation = 'S'
action='R' action='R'
successor = Node(pos, new_rotation,action,current_node) successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
if (y >0): if (y >0):
pos = [] pos = []
pos.append(x) pos.append(x)
pos.append(y) pos.append(y)
new_rotation = 'S' new_rotation = 'N'
action = 'L' action = 'L'
successor = Node(pos, new_rotation,action,current_node) successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
elif (current_rotation == "W"): elif (current_rotation == "W"):
if (x > 0): if (x > 0):
pos = [] pos = []
pos.append(x+1) pos.append(x-1)
pos.append(y) pos.append(y)
action = 'FW' action = 'FW'
successor = Node(pos, current_rotation,action,current_node) successor = Node(pos, current_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
if (y > 0): if (y >0):
pos = []
pos.append(x)
pos.append(y)
new_rotation = 'S'
action='R'
successor = Node(pos, new_rotation,action,current_node)
successors.append(successor)
if (y < 7):
pos = [] pos = []
pos.append(x) pos.append(x)
pos.append(y) pos.append(y)
new_rotation = 'N' new_rotation = 'N'
action='R'
successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor)
if (y <7):
pos = []
pos.append(x)
pos.append(y)
new_rotation = 'S'
action = 'L' action = 'L'
successor = Node(pos, new_rotation,action,current_node) successor = Node(pos, new_rotation,action,current_node,current_cost)
successors.append(successor) successors.append(successor)
return successors return successors
def distance(current_node,target):
return abs(current_node.position[0]-target.position[0])+abs(current_node.position[1]-target.position[1])
#bfs #bfs
def bfs(isstate,final): def bfs(isstate,final):
fringe=[] fringe=deque()
fringe.append(isstate) fringe.append(isstate)
path=[] path=[]
explored=[] explored=[]
while(True): while(True):
if(len(fringe)==0): if(len(fringe)==0):
return False return False
node=fringe.pop(0) node=fringe.popleft()
if(node.position[0]==final.position[0] and node.position[1]==final.position[1]): if(node.position[0]==final.position[0] and node.position[1]==final.position[1]):
while(node.parent!=None): while(node.parent!=None):
path.append(node) path.append(node)
@ -283,13 +307,37 @@ def bfs(isstate,final):
for successor in successors: for successor in successors:
if (successor not in fringe and successor not in explored): if (successor not in fringe and successor not in explored):
fringe.append(successor) fringe.append(successor)
def astar(isstate,final):
fringe=[]
heapq.heappush(fringe,(0,isstate))
path = []
explored = []
total_cost={isstate:0}
while(True):
if (len(fringe) == 0):
return False
a,node =heapq.heappop(fringe)
if (node.position[0] == final.position[0] and node.position[1] == final.position[1]):
while (node.parent != None):
path.append(node)
node = node.parent
return path
explored.append(node)
successors = succ(node)
for successor in successors:
new_cost=total_cost[node]+successor.cost
if (successor not in explored or new_cost<total_cost.get(successor,float('inf'))):
total_cost[successor]=new_cost
p=new_cost+distance(successor,final)
heapq.heappush(fringe,(p,successor))
# Main game loop # Main game loop
def game_loop(): def game_loop():
init_game() init_game()
current=Node(forklift_pos,rotation,'start',None) current=Node(forklift_pos,rotation,'start',None,0)
final=Node([10,6],'N','final',None) final=Node([10,5],'N','final',None,0)
path=bfs(current,final) path=astar(current,final)
path.reverse() path.reverse()
for node in path: for node in path:
print(node.action) print(node.action)