from frontier import Frontier from node import Node frontier = Frontier() frontier.add_or_update('a', 40) frontier.add_or_update('b', 20) frontier.add_or_update('c', 30) assert frontier.remove() == 'b' assert frontier.remove() == 'c' frontier.add_or_update('d', 10) frontier.add_or_update('e', 50) assert frontier.remove() == 'd' assert frontier.remove() == 'a' assert frontier.remove() == 'e' assert frontier.remove() is None
class RouteFinder(object): def __init__(self, space): self.__space = space self.__frontier = Frontier() self.__goal = None self.__origin = None def __calculate_distance(self, from_xy, to_xy): """ Calculate the distance between the given coordinates using MATH. :param from_xy: a tuple of the form (x, y) :param to_xy: a tuple of the form (x, y) :returns: float """ diff_x = math.fabs(from_xy[0] - to_xy[0]) diff_y = math.fabs(from_xy[1] - to_xy[1]) return math.sqrt(math.pow(diff_x, 2) + math.pow(diff_y, 2)) def __initialize_goal(self, goal_id): """ Initialise the goal node, identified by the given ID. :param goal_id: the goal id """ self.__goal = self.__space.nodes[goal_id] self.__goal.distance_to_goal = 0 def __initialize_origin(self, origin_id): """ Initialise the origin node, identified by the given ID. :param origin: the origin id """ self.__origin = self.__space.nodes[origin_id] self.__origin.route_distance = 0 def __add_to_frontier(self, from_node, to_node): """ Attempt to add the "to" node to the frontier. :param from_node: the previous node :param to_node: the next node """ if to_node.visited: return if from_node is None: distance_to_node = 0 else: distance_to_node = self.__calculate_distance( (from_node.x, from_node.y), (to_node.x, to_node.y)) + from_node.distance_to_here # Have we found a shorter route to this node? if distance_to_node < to_node.distance_to_here: to_node.distance_to_here = distance_to_node to_node.via = from_node if math.isinf(to_node.distance_to_goal): # Always low-ball to estimated distance, otherwise A* won't work to_node.distance_to_goal = self.__calculate_distance( (to_node.x, to_node.y), (self.__goal.x, self.__goal.y)) * 0.9 self.__frontier.add_or_update(to_node.id, to_node.total_distance_to_goal) def __get_route_ids(self): """ Get the IDs of the best route from the origin to the goal. :returns: a list of node IDs """ route = [] node = self.__goal while node is not None: route.append(node.id) node = node.via return list(reversed(route)) def __find_route(self): """ Find the shortest route between the origin and the goal. :returns: a list of IDs representing the steps from the origin to the goal, or None """ active_node = self.__space.nodes[self.__frontier.remove()] # Exit conditions if active_node is None: return None if active_node == self.__goal: return self.__get_route_ids() active_node.visited = True # Add the linked nodes to the frontier for linked_id in self.__space.find_linked_nodes(active_node.id): if linked_id not in self.__space.nodes: # Something is awry with the map data return None self.__add_to_frontier(active_node, self.__space.nodes[linked_id]) return self.__find_route() def find_route(self, origin_id, goal_id): """ Find the shortest route between the specified nodes. :param origin_id: the ID of the "origin" node :param goal_id: the ID of the "goal" node :returns: a list of IDs representing the steps from the origin to the goal, or None """ if origin_id not in self.__space.nodes or goal_id not in self.__space.nodes: return None if origin_id == goal_id: return [origin_id] self.__space.reset() self.__frontier.reset() self.__initialize_origin(origin_id) self.__initialize_goal(goal_id) self.__add_to_frontier(None, self.__origin) return self.__find_route()