Added distance calculating and selecting directions

This commit is contained in:
Magdalena Wilczynska 2019-05-13 10:27:39 +02:00
parent 1963068dd6
commit 0981dcb8cc

72
Traversal/BestFS.py Normal file
View File

@ -0,0 +1,72 @@
from utilities import movement,check_moves
from DataModels.House import House
from DataModels.Container import Container
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, 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])
house_in_area = False
for location in possible_goals:
if location[0]>=0 and location[1]>=0:
try:
cell = grid[location[0]][location[1]]
if(type(cell) == House and cell.container.is_full and cell.unvisited):
cell.unvisited = False
house_in_area = True
break
except:
continue
if(house_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])
#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[1] == 1):
preffered_directions.append("pick_garbage")
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("------------------------------")
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, depth+1)
if result!= None:
return result