diff --git a/ai-wozek/wozek.py b/ai-wozek/wozek.py index 9b5621b..c7a13d1 100644 --- a/ai-wozek/wozek.py +++ b/ai-wozek/wozek.py @@ -3,14 +3,22 @@ import sys import random import os import time +from collections import deque +import heapq from classes import * class Node(): - def __init__(self,position,rotation,action,parent): + def __init__(self,position,rotation,action,parent,cost): self.position=position self.rotation=rotation self.action=action 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 pygame.init() @@ -47,6 +55,14 @@ carrying_freight = False carried_freight = None 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 def load_images(): global forklift_image_full, freight_images_full @@ -92,6 +108,10 @@ def draw_truck_bed_and_racks(): for y in range(5, 8): for x in range(GRID_WIDTH): 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(): @@ -158,15 +178,16 @@ def succ(current_node): current_rotation=current_node.rotation x=current_node.position[0] y=current_node.position[1] + current_cost=tile_cost.get((x,y),1) successors=[] if(current_rotation=="N"): - if(y<7): + if(y>0): pos=[] pos.append(x) - pos.append(y+1) + pos.append(y-1) action='FW' - successor=Node(pos,current_rotation,action,current_node) + successor=Node(pos,current_rotation,action,current_node,current_cost) successors.append(successor) if(x>0): pos = [] @@ -174,7 +195,7 @@ def succ(current_node): pos.append(y) new_rotation='W' action='L' - successor = Node(pos, new_rotation,action,current_node) + successor = Node(pos, new_rotation,action,current_node,current_cost) successors.append(successor) if(x<15): pos = [] @@ -182,33 +203,33 @@ def succ(current_node): pos.append(y) new_rotation='E' action='R' - successor = Node(pos, new_rotation,action,current_node) + successor = Node(pos, new_rotation,action,current_node,current_cost) successors.append(successor) elif (current_rotation == "S"): - if (y > 0): + if (y < 7): pos = [] pos.append(x) - pos.append(y - 1) + pos.append(y + 1) action = 'FW' - successor = Node(pos, current_rotation,action,current_node) + successor = Node(pos, current_rotation,action,current_node,current_cost) successors.append(successor) - if (x > 0): + if (x <15): pos = [] pos.append(x) pos.append(y) new_rotation = 'E' action='L' - successor = Node(pos, new_rotation,action,current_node) + successor = Node(pos, new_rotation,action,current_node,current_cost) successors.append(successor) - if (x < 15): + if (x > 0): pos = [] pos.append(x) pos.append(y) new_rotation = 'W' action = 'R' - successor = Node(pos, new_rotation,action,current_node) + successor = Node(pos, new_rotation,action,current_node,current_cost) successors.append(successor) elif (current_rotation == "E"): @@ -217,62 +238,65 @@ def succ(current_node): pos.append(x+1) pos.append(y) action = 'FW' - successor = Node(pos, current_rotation,action,current_node) + successor = Node(pos, current_rotation,action,current_node,current_cost) successors.append(successor) if (y <7): pos = [] pos.append(x) pos.append(y) - new_rotation = 'N' + new_rotation = 'S' action='R' - successor = Node(pos, new_rotation,action,current_node) + successor = Node(pos, new_rotation,action,current_node,current_cost) successors.append(successor) if (y >0): pos = [] pos.append(x) pos.append(y) - new_rotation = 'S' + new_rotation = 'N' action = 'L' - successor = Node(pos, new_rotation,action,current_node) + successor = Node(pos, new_rotation,action,current_node,current_cost) successors.append(successor) elif (current_rotation == "W"): if (x > 0): pos = [] - pos.append(x+1) + pos.append(x-1) pos.append(y) action = 'FW' - successor = Node(pos, current_rotation,action,current_node) + successor = Node(pos, current_rotation,action,current_node,current_cost) successors.append(successor) - 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): + if (y >0): pos = [] pos.append(x) pos.append(y) 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' - successor = Node(pos, new_rotation,action,current_node) + successor = Node(pos, new_rotation,action,current_node,current_cost) successors.append(successor) 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 def bfs(isstate,final): - fringe=[] + fringe=deque() fringe.append(isstate) path=[] explored=[] while(True): if(len(fringe)==0): return False - node=fringe.pop(0) + node=fringe.popleft() if(node.position[0]==final.position[0] and node.position[1]==final.position[1]): while(node.parent!=None): path.append(node) @@ -283,13 +307,37 @@ def bfs(isstate,final): for successor in successors: if (successor not in fringe and successor not in explored): 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