added A*
This commit is contained in:
parent
35928a17a4
commit
6b425159fb
@ -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<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
|
||||
def game_loop():
|
||||
init_game()
|
||||
current=Node(forklift_pos,rotation,'start',None)
|
||||
final=Node([10,6],'N','final',None)
|
||||
path=bfs(current,final)
|
||||
current=Node(forklift_pos,rotation,'start',None,0)
|
||||
final=Node([10,5],'N','final',None,0)
|
||||
path=astar(current,final)
|
||||
path.reverse()
|
||||
for node in path:
|
||||
print(node.action)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user