def __calc_route__(self): node_dict = {} map_dict = {} for item in self.ms.all_doors(): node = Node(item) node_dict[item] = node if not item.map_name in map_dict: map_dict[item.map_name] = [] map_dict[item.map_name].append(node) from_node = Node(wx.GetApp().GetNavFrom()) map_dict[wx.GetApp().GetNavFrom().map_name].append(from_node) to_node = Node(wx.GetApp().GetNavTo()) map_dict[wx.GetApp().GetNavTo().map_name].append(to_node) #beam beam_node = Node(self.ms.beam_location()) log.debug("beam_node: %s" % beam_node) from_node.edges.append((1, beam_node)) map_dict[beam_node.payload.map_name].append(beam_node) for map_name, map_nodes in map_dict.items(): for index, item_a in enumerate(map_nodes): for item_b in map_nodes[index + 1:]: cost = self.ds.in_map_distance(item_a.payload, item_b.payload) add_edge(cost, item_a, item_b) for item, node in node_dict.items(): other_item = self.ms.other_side(item) cost = self.ds.cost(item, other_item) cost = 1 node.edges.append((cost, node_dict[other_item])) node_dict[wx.GetApp().GetNavFrom()] = from_node node_dict[wx.GetApp().GetNavTo()] = to_node node_dict[self.ms.beam_location()] = beam_node nodes = node_dict.values() for item in nodes: log.debug(item) for item_b in item.edges: log.debug(" %s" % str(item_b)) from_node.cost = 0 solve(nodes) for item in nodes: log.debug(item) log.debug("ROUTE:") self.__route__ = get_route(nodes, node_dict[wx.GetApp().GetNavTo()])
dist_xml = dist_doc.xpath('//distance[@from="%s"][@to="%s"]'\ % (to_loc, from_loc)) if dist_xml: #~ print "dist_xml: %s" % etree.tostring(dist_xml[0]) cost = int(dist_xml[0].get('seconds')) else: from_loc = current.payload.loc_touple() to_loc = item.payload.loc_touple() #~ cost = math.sqrt( (from_loc[0] - to_loc[0])**2 +\ #~ (from_loc[1] - to_loc[1])**2) cost = distance(from_loc, to_loc) cost = int(cost * STD_UNIT_COST) #~ cost = 1 add_edge(cost, current, item) all_nodes.extend(nodes) #connect doors unconnted = copy(all_nodes) while len(unconnted) > 0: current = unconnted.pop() for item in unconnted: if current.payload.target() == item.payload.target(): unconnted.remove(item) add_edge(CHANGE_MAP_COST, current, item) #add beam point beam_location = Node(LocatableObject(doc.xpath('//beam')[0]))