85 lines
2.9 KiB
Python
85 lines
2.9 KiB
Python
|
from utilities import movement,check_moves
|
||
|
from DataModels.House import House
|
||
|
from DataModels.Container import Container
|
||
|
from config import GRID_WIDTH, GRID_HEIGHT
|
||
|
from math import sqrt
|
||
|
INF = float('Inf')
|
||
|
|
||
|
|
||
|
def CalculateDistance(gc, goal):
|
||
|
result = sqrt(pow(goal[0]-gc[0],2)+pow(goal[1]-gc[1],2))
|
||
|
return result
|
||
|
|
||
|
def BestFS(grid, available_movement, gc_moveset, houses_list, mode = "House", depth=0):
|
||
|
|
||
|
possible_goals = []
|
||
|
a = gc_moveset[-1][0]
|
||
|
b = gc_moveset[-1][1]
|
||
|
possible_goals.append([a+1,b])
|
||
|
possible_goals.append([a-1,b])
|
||
|
possible_goals.append([a,b+1])
|
||
|
possible_goals.append([a,b-1])
|
||
|
object_in_area = False
|
||
|
for location in possible_goals:
|
||
|
if GRID_WIDTH>location[0]>=0 and GRID_HEIGHT>location[1]>=0:
|
||
|
cell = grid[location[0]][location[1]]
|
||
|
if mode == "House":
|
||
|
if(type(cell) == House and cell.container.is_full and cell.unvisited):
|
||
|
cell.unvisited = False
|
||
|
object_in_area = True
|
||
|
print("***"+str([cell,location])+"***")
|
||
|
houses_list.remove([cell,location])
|
||
|
break
|
||
|
elif mode == "Dump":
|
||
|
if(type(cell) == Dump and cell.unvisited):
|
||
|
cell.unvisited = False
|
||
|
object_in_area = True
|
||
|
break
|
||
|
if(object_in_area):
|
||
|
xy = gc_moveset[-1]
|
||
|
gc_moveset.append("pick_garbage")
|
||
|
return (xy, gc_moveset)
|
||
|
|
||
|
if len(available_movement) == 0 or depth>30:
|
||
|
return
|
||
|
|
||
|
x,y = gc_moveset[-1]
|
||
|
|
||
|
print([x,y])
|
||
|
print(houses_list)
|
||
|
#calculate distance to the nearest object
|
||
|
min_distance_goal = ['-',INF]
|
||
|
for h in houses_list:
|
||
|
distance = CalculateDistance([a,b],h[1])
|
||
|
if(min_distance_goal[1] > distance):
|
||
|
min_distance_goal = [h[1], distance]
|
||
|
print(min_distance_goal)
|
||
|
|
||
|
#set preffered directions based on the closest object
|
||
|
preffered_directions = []
|
||
|
if(min_distance_goal[0][0] >= a):
|
||
|
preffered_directions.append("right")
|
||
|
if(min_distance_goal[0][0] <= a):
|
||
|
preffered_directions.append("left")
|
||
|
if(min_distance_goal[0][1] >= b):
|
||
|
preffered_directions.append("down")
|
||
|
if(min_distance_goal[0][1] <= b):
|
||
|
preffered_directions.append("up")
|
||
|
print(preffered_directions)
|
||
|
print(available_movement)
|
||
|
print("=")
|
||
|
output_list = available_movement.copy()
|
||
|
output_list = output_list.sort(key=lambda x: preffered_directions.index(x))
|
||
|
print(output_list)
|
||
|
print("------------------------------")
|
||
|
|
||
|
|
||
|
for direction in available_movement:
|
||
|
x_next, y_next = movement(grid,x,y)[0][direction]
|
||
|
available_movement_next = check_moves(grid, x_next,y_next,direction)
|
||
|
gc_moveset_next = gc_moveset.copy()
|
||
|
gc_moveset_next.append([x_next,y_next])
|
||
|
result = BestFS(grid, available_movement_next, gc_moveset_next, houses_list, "House", depth+1)
|
||
|
if result!= None:
|
||
|
return result
|