diff --git a/Traversal/BestFS.py b/Traversal/BestFS.py new file mode 100644 index 0000000..95de8af --- /dev/null +++ b/Traversal/BestFS.py @@ -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