Пример #1
0
def solve(data):
    grid = data

    base = grid
    for j in range(1, 5):
        appendix = base + j
        appendix[appendix > 9] -= 9
        grid = np.concatenate((grid, appendix), axis = 0)

    base = grid
    for i in range(1, 5):
        appendix = base + i
        appendix[appendix > 9] -= 9
        grid = np.concatenate((grid, appendix), axis = 1)

    h, w = np.shape(grid)

    graph = Graph()

    for j in range(0, h):
        for a, b in pairwise(range(0, w)):
            graph.add_edge(j * h + a, j * h + b, grid[j][b])
            graph.add_edge(j * h + b, j * h + a, grid[j][a])

    for i in range(0, w):
        for a, b in pairwise(range(0, h)):
            graph.add_edge(a * h + i, b * h + i, grid[b][i])
            graph.add_edge(b * h + i, a * h + i, grid[a][i])

    return find_path(graph, 0, h * w - 1).total_cost
Пример #2
0
def get_sorted_map_path_data(map_data: dict) -> dict:
    """
    获取地图下各节点到目标节点的最短路径数据
    Keyword arguments:
    map_data -- 地图节点数据
    """
    graph = Graph()
    sorted_path_data = {}
    for node in map_data.keys():
        for target in map_data[node]:
            graph.add_edge(node, target, {"cost": map_data[node][target]})
    cost_func = lambda u, v, e, prev_e: e["cost"]
    for node in map_data.keys():
        new_data = {node: {}}
        for target in map_data.keys():
            if target != node:
                find_path_data = find_path(graph,
                                           node,
                                           target,
                                           cost_func=cost_func)
                new_data[node].update({
                    target: {
                        "Path": find_path_data.nodes[1:],
                        "Time": find_path_data.costs,
                    }
                })
        sorted_path_data.update(new_data)
    return sorted_path_data
Пример #3
0
    def dijkstra_planning(self):
        graph = Graph()

        temp_nodes = self.nodes[:]
        temp_nodes.pop(0)
        k = 0
        for i in range(len(self.nodes)):
            k = k + 1
            for j in range(len(temp_nodes)):
                node_a = self.nodes[i]
                node_b = temp_nodes[j]
                if np.linalg.norm(node_a - node_b) != 0:
                    print("add edge:", i, j + k)
                    graph.add_edge(i, j + k, np.linalg.norm(node_a - node_b))
            if temp_nodes:
                temp_nodes.pop(0)

        print("done")

        path = find_path(graph, 1, 3)
        print("path:", path)


# if __name__ == "__main__":
#     test_nodes = [np.array([21, 34]),
#                   np.array([45, 28]),
#                   np.array([76, 14]),
#                   np.array([12, 56]),
#                   np.array([48, 32])]
#
#     motion_planner = Planner(test_nodes)
#     motion_planner.dynamic_programming()
Пример #4
0
def crea_matrice_distanze(dist):
    initial_matr = np.array(dist)
    print initial_matr
    matr = initial_matr.copy()
    # dict_matr = {(partenza, arrivi): lunghezza for partenza, arrivi in enumerate(dist) for arrivo, lunghezza in
    #              enumerate(arrivi)}
    graph = Graph()

    for partenza, arrivi in enumerate(dist):
        for arrivo, lunghezza in enumerate(arrivi):
            if lunghezza >= 0:
                graph.add_edge(partenza, arrivo, {'cost': lunghezza})
    cost_func = lambda u, v, e, prev_e: e['cost']

    for partenza, arrivi in enumerate(dist):
        for arrivo in range(len(arrivi)):
            if partenza == arrivo:
                matr[partenza, arrivo] = -1
            else:
                try:
                    min_dist = find_path(graph,
                                         partenza,
                                         arrivo,
                                         cost_func=cost_func)[3]
                    matr[partenza, arrivo] = min_dist
                except Exception as e:
                    matr[partenza, arrivo] = -1
    print "matr ditanze\n", matr
    return matr
Пример #5
0
def get_sorted_map_path_data(
    map_data: Dict[str, Dict[str, int]]
) -> Dict[str, Dict[str, game_type.TargetPath]]:
    """
    获取地图下各节点到目标节点的最短路径数据
    Keyword arguments:
    map_data -- 地图节点数据 当前节点:可通行节点:所需时间
    Return arguments:
    Dict[int,Dict[int,game_type.TargetPath]] -- 最短路径数据 当前节点:目标节点:路径对象
    """
    graph = Graph()
    sorted_path_data = {}
    for node in map_data.keys():
        for target in map_data[node]:
            graph.add_edge(node, target, {"cost": map_data[node][target]})
    cost_func = lambda u, v, e, prev_e: e["cost"]
    for node in map_data.keys():
        new_data = {node: {}}
        for target in map_data.keys():
            if target != node:
                find_path_data = find_path(graph,
                                           node,
                                           target,
                                           cost_func=cost_func)
                target_path = game_type.TargetPath()
                target_path.path = find_path_data.nodes[1:]
                target_path.time = find_path_data.costs
                new_data[node][target] = target_path
        sorted_path_data.update(new_data)
    return sorted_path_data
Пример #6
0
    def graph3(self):
        graph = Graph({
            'a': {
                'b': 10,
                'c': 100,
                'd': 1
            },
            'b': {
                'c': 10
            },
            'd': {
                'b': 1,
                'e': 1
            },
            'e': {
                'f': 1
            },
        })

        graph.add_node('f', {'c': 1})
        graph['f'] = {'c': 1}

        graph.add_edge('f', 'c', 1)
        graph.add_edge('g', 'b', 1)

        nodes = list(graph)
        nodes.sort()
        self.assertEqual(nodes, ['a', 'b', 'd', 'e', 'f', 'g'])

        incoming = graph.get_incoming('c')
        incoming_nodes = list(incoming.keys())
        incoming_nodes.sort()
        self.assertEqual(incoming_nodes, ['a', 'b', 'f'])

        return graph
Пример #7
0
def find_conversions(source, target, versions_graph):
    """
    This finds the minimum number of version upgrades required to reach the target version.
    :param source: source version
    :param target: target version
    :param versions_graph: graph to fetch update path from
    :return: list of conversions in order of execution
    """
    graph = Graph()
    for source_version, target_version in versions_graph.query(
            """SELECT ?source_version ?target_version{
                        ?source_version version:convertsTo ?target_version .
    }"""):
        graph.add_edge(str(source_version), str(target_version),
                       {"conversions": 1})

    # Find the shortest path
    res = find_path(
        graph,
        str(source),
        str(target),
        cost_func=lambda u, v, e, prev_e: e["conversions"],
    )

    # Create and return the conversions
    conversions = []
    print(" -> ".join(res.nodes))
    current = source
    for node in res.nodes:
        conversions.append((current, node))
        current = node
    return conversions[1:]
 def findBestPath(self, src, dst):
   graph = Graph()
   for node1 in costBetweenNodes.keys():
     for node2 in costBetweenNodes[node1].keys():
       graph.add_edge(node1, node2, {'cost': costBetweenNodes[node1][node2]})
   cost_func = lambda u, v, e, prev_e: e['cost']
   return find_path(graph, src, dst, cost_func=cost_func).nodes
Пример #9
0
    def make_graph(self):

        con = pymysql.connect(host='localhost',
                              user='******',
                              password='******',
                              db='drive2',
                              charset='utf8')

        curs = con.cursor()

        graph = Graph()
        sql = "select nodeid from drive2.c1_node"
        sql2 = "select tonode, length, format(speed,0) from drive2.a3_link, jcm_drive.jcm_a3_link where fromnode = "

        curs.execute(sql)

        rows = curs.fetchall()
        for i in rows:
            nodeid = list(map(lambda x: x, i))
            curs.execute(sql2 + "'" + nodeid[0] + "';")

            rows = curs.fetchall()

            if rows is None:
                continue
            else:
                for item in list(rows):
                    cost = item[1] / int(item[2])
                    graph.add_edge(i[0], item[0], cost)

        con.close()
        return graph
Пример #10
0
def getSortedMapPathData(mapData: dict) -> dict:
    '''
    获取地图下各节点到目标节点的最短路径数据
    Keyword arguments:
    mapData -- 地图节点数据
    '''
    graph = Graph()
    sortedPathData = {}
    for node in mapData.keys():
        for target in mapData[node]:
            graph.add_edge(node, target, {'cost': mapData[node][target]})
    cost_func = lambda u, v, e, prev_e: e['cost']
    for node in mapData.keys():
        newData = {node: {}}
        for target in mapData.keys():
            if target != node:
                findPathData = find_path(graph,
                                         node,
                                         target,
                                         cost_func=cost_func)
                newData[node].update({
                    target: {
                        "Path": findPathData.nodes[1:],
                        "Time": findPathData.costs
                    }
                })
        sortedPathData.update(newData)
    return sortedPathData
Пример #11
0
def shortestPathAStar(start_ind, goal_ind):

    global R
    # R[0] is the node for the start configuration
    # R[1] is the node for the goal configuration

    # each node has edges in the form: self.edges = [] List of tuples (node_id, dist)
    # loop through our graph and convert it to a nice format for dijkstar
    graph = Graph()
    for node in R:
        for edge in node.edges:
            graph.add_edge(node.id, edge[0], edge[1])

    cfg_array = []

    # catch path not found exception
    try:
        pathinfo = find_path(graph, start_ind, goal_ind)

    except:
        print('Could NOT find a path from start to goal')
        return cfg_array

    # get the configurations from each node in this found path
    for node_id in pathinfo.nodes:
        cfg_array.append(R[node_id].cfg)

    return cfg_array
Пример #12
0
def sol_6_b(data_str):
    orbit_tree = read_orbits(data_str)
    g = Graph()
    for planet in orbit_tree:
        g.add_edge(orbit_tree[planet], planet, 1)
        g.add_edge(planet, orbit_tree[planet], 1)
    path = find_path(g, "YOU", orbit_tree["SAN"])
    return path.total_cost - 1
Пример #13
0
def _get_graph(nets_ids, links):
    graph = Graph()
    for link in links:
        src_connectable_id = link['src_connectable_id']
        dst_connectable_id = link['dst_connectable_id']
        if src_connectable_id in nets_ids and dst_connectable_id in nets_ids:
            graph.add_edge(src_connectable_id, dst_connectable_id, 100)
    return graph
Пример #14
0
    def setUpGraph(self):
        graph = Graph()
        for camino in Camino.objects.all():
            graph.add_edge(
                camino.desde.pk, camino.hasta.pk, {'cost': camino.distancia})
            graph.add_edge(
                camino.hasta.pk, camino.desde.pk, {'cost': camino.distancia})

        return graph
Пример #15
0
def dijkstra(d):
    g = Graph()
    for i in range(len(d)):
        for j in range(len(d[i])):
            dirs = ((i - 1, j), (i + 1, j), (i, j - 1), (i, j + 1))
            for x, y in dirs:
                if 0 <= x < len(d) and 0 <= y < len(d[i]):
                    g.add_edge((i, j), (x, y), d[x][y])
    return find_path(g, (0, 0), (len(d) - 1, len(d[0]) - 1)).total_cost
Пример #16
0
    def graph_search(self):
        # set up the graph
        graph = Graph()
        self.nodes = 0
        self.total_cost = 0
        # add edges
        for i in range(len(self.TE)):
            # pdb.set_trace()
            edge = self.TE[i]
            dist = math.sqrt((edge[0][0] - edge[1][0])**2 +
                             (edge[0][1] - edge[1][1])**2)
            # pdb.set_trace()
            # find vertex from self.V that matches the edges
            vertex1 = self.TV.index(edge[0])
            vertex2 = self.TV.index(edge[1])
            # pdb.set_trace()
            graph.add_edge(vertex1, vertex2, dist)
        # pdb.set_trace()
        temp = find_path(graph, 0, len(self.TV) - 1)
        self.nodes = temp[0]
        self.total_cost = temp[3]

        # now that we have the final path of the robot, we need to extract the ideal robot positions
        # interpolate all of the vertices to make a series of points that the robot can follow
        self.robot_positions = []
        # iterate over each node in the final path and connect between them
        for i in range(len(self.nodes) - 1):
            # take the first and second nodes
            from_node = self.TV[self.nodes[i]]
            to_node = self.TV[self.nodes[i + 1]]

            # take each edge and interpolate a distance equivalent 1/10th of the robot speed per second
            diff_x = to_node[0] - from_node[0]
            diff_y = to_node[1] - from_node[1]
            dist_speed = self.speed * 1 / 10  # m/s * s = m
            # now find number of points from distance between points and speed
            dist_covered = math.sqrt(diff_x**2 + diff_y**2)
            num_points = round(dist_covered / dist_speed)
            # interpolate using these points
            x_int = from_node[0]
            y_int = from_node[1]
            interpolateds = [x_int, y_int]
            for i in range(num_points):
                # append the robot positions
                self.robot_positions.append(interpolateds)
                # add the points in by moving the direction amount calculated in x and y
                x_int = interpolateds[0] + diff_x / num_points
                y_int = interpolateds[1] + diff_y / num_points
                interpolateds = [x_int, y_int]

            # append goal
        if self.solution_found == 1:
            self.robot_positions.append(self.qgoal)
            # self.robot_positions.append(np.linspace(edge[0],edge[1], num = self.speed/50,endpoint=True,retstep=True))

        return self.nodes, self.total_cost
Пример #17
0
 def test_initialise_dijk_grid_with_no_blocks(self):
     self.grid.width, self.grid.height, self.grid.blocks = 2, 2, []
     self.grid.initialise_dijk_grid()
     test_grid = Graph()
     for i in range(2):
         test_grid.add_edge((i, 0), (i, 1), 1)
         test_grid.add_edge((i, 1), (i, 0), 1)
         test_grid.add_edge((0, i), (1, i), 1)
         test_grid.add_edge((1, i), (0, i), 1)
     self.assertEqual(self.grid.dijk_grid, test_grid)
Пример #18
0
def graph_gen(
    df_edge
):  #génère un graph simple des liens entre les tables sans tenir compte des colones
    graph = Graph()
    for x in df_edge.index:
        graph.add_edge(df_edge.loc[x, 'table_FK_id'],
                       df_edge.loc[x, 'tablePK_id'], {'cost': 1})
        graph.add_edge(df_edge.loc[x, 'tablePK_id'],
                       df_edge.loc[x, 'table_FK_id'], {'cost': 1})
    return graph
Пример #19
0
 def make_graph(self):
     graph = Graph()
     for x, y in product(range(self.width), range(self.height)):
         if isinstance(self.grid[x][y],
                       Floor) or isinstance(self.grid[x][y], Flag):
             for i, j in [(x + i, y + j) for i, j in
                          ((-1, 0), (1, 0), (0, 1), (0, -1))]:
                 if isinstance(self.grid[i][j],
                               Floor) or isinstance(self.grid[i][j], Flag):
                     graph.add_edge((x, y), (i, j), 1)
     return graph
Пример #20
0
 def test_initialise_dijk_grid_simple(self):
     self.grid.width, self.grid.height, self.grid.blocks = 2, 2, [(1, 1)]
     self.grid.initialise_dijk_grid()
     test_grid = Graph()
     for i in range(2):
         test_grid.add_edge((i, 0), (i, 1), 1)
         test_grid.add_edge((i, 1), (i, 0), 1)
         test_grid.add_edge((0, i), (1, i), 1)
         test_grid.add_edge((1, i), (0, i), 1)
     test_grid.remove_node((1, 1))
     self.assertEqual(self.grid.dijk_grid, test_grid)
Пример #21
0
def main():
    models.Base.metadata.create_all(engine)
    logging.info('Created database schema.')

    session = Session()

    add_random_animals_of_type_to_session(session, create_lion,
                                          config.LION_COUNT)
    add_random_animals_of_type_to_session(session, create_hippopotamus,
                                          config.HIPPOPOTAMUS_COUNT)
    add_random_animals_of_type_to_session(session, create_antelope,
                                          config.ANTELOPE_COUNT)
    add_random_animals_of_type_to_session(session, create_hyena,
                                          config.HYENA_COUNT)

    all_animals = session.query(models.Animal).all()

    friendship_count = 0
    while friendship_count < config.MAX_FRIENDSHIP_COUNT:
        random_animal_1 = all_animals[randint(0, len(all_animals) - 1)]
        random_animal_2 = all_animals[randint(0, len(all_animals) - 1)]
        if can_become_friends(random_animal_1, random_animal_2):
            logging.info('Creating friendship between: {}, {}'.format(
                random_animal_1, random_animal_2))
            random_animal_1.friends.append(random_animal_2)
            random_animal_2.friends.append(random_animal_1)
            session.add(random_animal_1)
            session.add(random_animal_2)
            friendship_count += 1

    session.commit()

    hungriest_lion = get_hungriest_lion(session)
    logging.info('Hungriest lion is: {}'.format(hungriest_lion))

    slowest_antelope = get_slowest_antelope(session)
    logging.info('Slowest antelope is: {}'.format(slowest_antelope))

    graph = Graph()
    for animal in all_animals:
        for friend in animal.friends:
            graph.add_edge(animal.id, friend.id)
    try:
        shortest_path = find_path(graph,
                                  hungriest_lion.id,
                                  slowest_antelope.id,
                                  cost_func=get_cost)
    except NoPathError:
        logging.warn('The lion stays hungry today')
    else:
        logging.info(
            'Shortest path to slowest antelope is through animal ids: {}'.
            format(shortest_path[0]))
Пример #22
0
def parse_distance_matrix(csv_reader, cost_function):
    # Ignore last column for trailhead.
    peaks = csv_reader.__next__()[1:-1]
    distances = {p: {p: None for p in peaks} for p in peaks}
    trailhead_distances = {p: None for p in peaks + [_TRAILHEAD_NAME]}
    for row in csv_reader:
        if not is_peak(row[0]):
            continue
        # Note: trailhead distances are read as Peak > Trailhead, but here
        # stored as Trailhead > Peak, so they need to be reversed below.
        trailhead_distances[row[0]] = _parse_distance_gain_loss_string(row[-1])
        i = 0
        for d in row[1:-1]:
            distances[row[0]][peaks[i]] = _parse_distance_gain_loss_string(d)
            i += 1

    # Add back distances
    for p1 in distances:
        for p2 in distances[p1]:
            if distances[p1][p2] is not None and distances[p2][p1] is None:
                distances[p2][p1] = (
                    distances[p1][p2][0],
                    distances[p1][p2][2],  # swap climb/desc
                    distances[p1][p2][1])
    # Build graph for shortest path computation
    g = Graph()
    for p1 in distances:
        for p2 in distances[p1]:
            g.add_edge(p1, p2, distances[p1][p2])

    def internal_cost_function(_0, _1, x, _2):
        return cost_function(x)

    for p1 in distances:
        for p2 in distances[p1]:
            distances[p1][p2] = tuple(
                sum(x) for x in zip(*find_path(
                    g, p1, p2, cost_func=internal_cost_function).edges))

    # Trailhead has to be added after shortest path computation to make sure
    # shortest parths don't go through the artificial trailhead.
    distances[_TRAILHEAD_NAME] = {}
    for p in trailhead_distances:
        d = trailhead_distances[p]
        if d is None:
            d = (1e9, 1e9, 1e9)
        distances[_TRAILHEAD_NAME][p] = (d[0], d[2], d[1])
        distances[p].update({_TRAILHEAD_NAME: (d[0], d[1], d[2])})

    # Add self distance
    for p1 in distances:
        distances[p1][p1] = (0, 0, 0)
    return distances
Пример #23
0
def find_closest(game_map, width, height, player, players):

    # is anyone in range if so don't move
    inRange = []
    for d in [(0, -1), (-1, 0), (1, 0), (0, 1)]:
        key = "{}_{}".format(player.x + d[0], player.y + d[1])
        if key in game_map and game_map[key] not in ['.', '#', player.type]:
            enemy = list(
                filter(
                    lambda x: x.x == player.x + d[0] and x.y == player.y + d[
                        1], players))[0]
            return [0, enemy.x, enemy.y, []]

    # build a map
    graph = Graph()
    for x in range(0, width):
        for y in range(0, height):
            for d in [(0, -1), (-1, 0), (1, 0), (0, 1)]:
                key = "{}_{}".format(x + d[0], y + d[1])
                if key in game_map and game_map[key] == '.':
                    graph.add_edge("{}_{}".format(x, y), key, 1)

    path_data = []
    for p in players:
        # don't attack your friends (or self)
        if p.type == player.type:
            continue
        # don't attack dead people
        if not p.alive:
            continue
        # look around the target for a free space
        for d in [(0, -1), (-1, 0), (1, 0), (0, 1)]:
            key = "{}_{}".format(p.x + d[0], p.y + d[1])
            # if this block is valid and empty
            if key in game_map and game_map[key] == '.':
                try:
                    pdata = find_path(graph,
                                      "{}_{}".format(player.x, player.y),
                                      "{}_{}".format(p.x + d[0], p.y + d[1]))
                    path_data.append([
                        pdata.total_cost, p.x + d[0], p.y + d[1], pdata.nodes,
                        p.reading
                    ])
                except:
                    continue

    # sort by the shortest distance, and then the lowest y value, and then the lowest x value
    path_data = sorted(path_data, key=lambda x: (x[0], x[4]))
    if len(path_data) > 0:
        return path_data[0]
    else:
        return None
Пример #24
0
def make_graph(grid):
    utils.logger.info("Making graph...")
    graph = Graph()
    for x in range(len(grid)):
        for y, cell in enumerate(grid[x]):
            neighbors = get_neighbors(grid, x, y)
            for neighbor in neighbors:
                graph.add_edge(
                    (x, y),
                    (neighbor["x"], neighbor["y"]),
                )

    return graph
def Init_Graph(Node_list, Edge_list):
  graph_orig = Graph()
  
  # Build Graph with specified Nodes & Edges
  for nodes in Node_list:
    graph_orig.add_node(nodes)

  for edges in Edge_list:
    n1, n2 = edges
    graph_orig.add_edge(n1, n2, {'cost':1})
    graph_orig.add_edge(n2, n1, {'cost':1})

  return graph_orig
Пример #26
0
def solve_graph():
    global maze, maze_cost, pacmanpos, world, goal_state
    graph = Graph()
    for j in range(0, height):
        for i in range(0, width):
            if maze[j][i] != 1:
                #arriba
                if (maze[j-1][i] != 1) and (j-1 > 0):
                    xi = str((j,i))
                    xj = str((j-1,i))
                    c = maze_cost[j-1][i]
                    graph.add_edge(xi,xj,{'cost':c})
                    #print(xi+' - '+xj)
                #abajo
                if (maze[j+1][i] != 1) and (j+1 < height):
                    xi = str((j,i))
                    xj = str((j+1,i))
                    c = maze_cost[j+1][i]
                    graph.add_edge(xi,xj,{'cost':c})
                    #print(xi+' - '+xj)
                #derecha
                if (maze[j][i+1] != 1) and (i+1 < width):
                    xi = str((j,i))
                    xj = str((j,i+1))
                    c = maze_cost[j][i+1]
                    graph.add_edge(xi,xj,{'cost':c})
                    #print(xi+' - '+xj)
                #izquierda
                if (maze[j][i-1] != 1) and (i-1 > 0):
                    xi = str((j,i))
                    xj = str((j,i-1))
                    c = maze_cost[j][i-1]
                    graph.add_edge(xi,xj,{'cost':c})
                    #print(xi+' - '+xj)

    cost_func = lambda u, v, e, prev_e: e['cost']
    pacman = worldToGrid(pacmanpos.pacmanPos.x, pacmanpos.pacmanPos.y)
    start_state = (pacman['y'], pacman['x'])
    #start_state = (25,14)
    # goal_state = closest_bonus()
    # print(goal_state)
    #goal_state = (25,26)
    path = find_path(graph, str(start_state), str(goal_state), cost_func=cost_func)
    # print(path.nodes)
    if len(path.nodes)>1:
        next_state = eval(path.nodes[1])
        aux = (start_state[0]-next_state[0], start_state[1]-next_state[1])
    else:
        print('yuca')
        return 4
    #print(str(start_state)+'  '+str(next_state))
    if aux == (1,0):
        return 0
    if aux == (-1,0):
        return 1
    if aux == (0,-1):
        return 2
    if aux == (0,1):
        return 3
Пример #27
0
def find_shortest_path(edges, vertex, s_i, e_i):
    # print(s_i)
    graph = Graph()
    for edge in edges:
        a = vertex.index(edge[0])
        b = vertex.index(edge[1])
        dist = np.linalg.norm(np.array(edge[0]) - np.array(edge[1]))
        # print(a, b, dist)
        graph.add_edge(a, b, {'cost': dist})
        graph.add_edge(b, a, {'cost': dist})
    cost_func = lambda u, v, e, prev_e: e['cost']
    shortest = find_path(graph, s_i, e_i, cost_func=cost_func)[0]
    for i in range(len(shortest) - 1):
        point1 = vertex[shortest[i]]
        point2 = vertex[shortest[i + 1]]
        plt.plot([point1[0], point2[0]], [point1[1], point2[1]], linewidth=2, color='g')
Пример #28
0
def dist_mat(train_mat, test_mat, k):
    # construct a weighted graph
    mat = graph_knn(train_mat, test_mat, k)
    n = mat.shape[0]
    graph = Graph()
    list((graph.add_edge(i, j, {'cost': mat[i, j]})
          for i, j in product(range(n), range(n))
          if i != j and mat[i, j] != max_dis))
    if graph is None:
        return
    cost_func = lambda u, v, e, prev_e: e['cost']
    mat = np.zeros((n, n))

    # the shortest path from node i to node j is the distance between i and j
    def dis(i):
        single_short_path = single_source_shortest_paths(graph,
                                                         i,
                                                         cost_func=cost_func)
        for j in range(n):
            if j != i:
                mat[i, j] = extract_shortest_path(single_short_path, j)
            else:
                mat[i, j] = 0

    list((dis(i) for i in range(n)))
    return mat
Пример #29
0
def index(request):
    if request.POST['tipo_algoritmo'] == 'dijkstra':
        graph = Graph()
        origen = request.POST['inicio']
        fin = request.POST['final']
        with open(
                '/Users/karelsalcedo/Code/AI/AlgoritmosPract2/BACKEND/practica2/Aristas.json'
        ) as file:
            data2 = json.load(file)
            aristas = data2
            for arti in aristas:
                graph.add_edge(arti[0], arti[1], {'cost': arti[2]})
            print(graph)
            cost_func = lambda u, v, e, prev_e: e['cost']
            final = find_path(graph,
                              int(origen),
                              int(fin),
                              cost_func=cost_func)

        data = {
            'status': '100',
            'ruta': final,
        }
        return JsonResponse(data)

    elif request.POST['tipo_algoritmo'] == 'prim':

        data = {
            'status': '100',
            'ruta': '',
        }
        return JsonResponse(data)

    elif request.POST['tipo_algoritmo'] == 'kruskal':
        data = {
            'status': '100',
            'ruta': ['a'],
        }
        return JsonResponse(data)

    else:
        data = {
            'status': '200',
            'msg': 'El algoritmo no esta registrado.',
        }
        return JsonResponse(data)
Пример #30
0
 def calculate_path(self):
     graph = Graph()
     for i in range(len(self.links)):
         graph.add_edge(self.links[i][0], self.links[i][1], {'cost': self.links[i][2]})
     cost_func = lambda u, v, e, prev_e: e['cost']
     #print self.links
     result =  find_path(graph, meta_data.source_id, meta_data.destination_id, cost_func=cost_func)
     route = result[0]
     # clear link list
     del self.crn_manager.role.links[:]
     # check and reply
     if meta_data.INF in result[2]:
         route = []
         self.routing_request_log.append([self.crn_manager.get_virtual_time(), 0])
     else: 
         self.routing_request_log.append([self.crn_manager.get_virtual_time(), 1])
     #self.crn_manager.route = route
     return route
Пример #31
0
def part_one(grid):
    from dijkstar import Graph, find_path
    mx, my = len(grid), len(grid[0])
    graph = Graph()

    for i in range(len(grid)):
        for j in range(len(grid[0])):
            ns = get_neighbors(i, j, mx, my, forward_only=False)
            for n in ns:
                graph.add_edge((i, j), n, grid[n[0]][n[1]])
    
    def cost_func(u, v, edge, prev_edge):
        return edge

    path = find_path(graph, (0, 0), (mx-1, my-1), cost_func=cost_func)

    #print_grid(grid, highlights=path.nodes)

    return path.total_cost
Пример #32
0
    def get_disktra(self, dict_probabilities, logaritmico):
        # Return a Graph to apply djistra algorithm
        grafo = Graph()
        dict_of_graphs = dict()

        for clave_transicion in dict_probabilities.keys():
            origen_destino = clave_transicion.split(";")
            origen = origen_destino[0]
            destino = origen_destino[1]
            valor_transicion = str(dict_probabilities.get(clave_transicion))
            grafo.add_edge(origen, destino, int(float(valor_transicion) * 100))

        for final_node in self.lista_nodos_finales:
            if logaritmico:
                dict_of_graphs[final_node] = find_path(grafo, "",
                                                       final_node + '_l')
            else:
                dict_of_graphs[final_node] = find_path(grafo, "", final_node)

        return dict_of_graphs
Пример #33
0
def compute_path(adj_matrix, tx_missing_matrix, node1, node2, nodes):
    graph = Graph()
    # Update tx_missing_matrix so that we don't count txs missing by node1
    source_missing_txs = tx_missing_matrix[node1]
    for cur_node in range(nodes):
        tx_missing_matrix[cur_node] = list(
            set(tx_missing_matrix[cur_node]) - set(source_missing_txs))

    cur_node = 0
    for cur_node in range(nodes):
        cur_node_peers = adj_matrix[cur_node]

        for peer in cur_node_peers:
            if tx_missing_matrix[peer] == []:
                graph.add_edge(cur_node, peer, {'cost': 0.5})
            else:
                graph.add_edge(cur_node, peer, {'cost': 1.5})
        cur_node += 1
    cost_func = lambda u, v, e, prev_e: e['cost']
    return find_path(graph, node1, node2,
                     cost_func=cost_func), tx_missing_matrix
def steiner_tree(reader_cost, reader_od):
    matrix_cost = np.array(reader_cost)
    matrix_od = np.array(reader_od)
    graph = Graph()
    # ایجاد گراف دایجسترا
    for item in range(matrix_cost.shape[0]):
        graph.add_edge(matrix_cost[item, 0], matrix_cost[item, 1],
                       matrix_cost[item, 2])
# ستون آخر ماتریس تقاضا(مجموع سفرهای تولید شده) را در لیست فرار بده
    list_max = []
    for item in matrix_od[:, -1]:
        list_max.append(item)
# ماکزیمم اول و دوم را در لیست ایجاد شده شناسایی کن
    first_max = max(list_max[0], list_max[1])
    second_max = 0
    for item in range(2, len(list_max)):
        if list_max[item] > first_max:
            second_max = first_max
            first_max = list_max[item]
        elif list_max[item] > second_max:
            second_max = list_max[item]


# بین دو نقطه ماکزیمم، کوتاهترین مسیر را ایجاد کن
    steiner_outcome = find_path(graph, list_max.index(first_max),
                                list_max.index(second_max))
    steiner_route = steiner_outcome.nodes
    steiner_network.append(steiner_route)
    # مقدار نقطه های اول و دوم تقاضا را در ماتریس تقاضا برابر با صفر قرار بده
    matrix_od[list_max.index(first_max), -1] = 0
    matrix_od[list_max.index(second_max), -1] = 0
    #reader_cost=matrix_cost
    #reader_od=matrix_od
    # مشخص کن که تمام نقاط ماتریس تقاضا بررسی شده و برابر صفر قرارداده شده
    # مشخص کن بیش از یک نقطه تقاضا برای ایجاد مسیر باقی مانده است
    if np.count_nonzero(matrix_od[:, -1]) > 1:
        steiner_tree(reader_cost, matrix_od)
    return steiner_network
Пример #35
0
Файл: run.py Проект: sheppard/bg
class Game(object):
    frame = 0
    sync_frame = 0

    def __init__(self):
        level_str = make_level()

        self.width = level_str.index('\n')
        self.height = level_str.count('\n')
        self.level = []
        self.teams = set()
        self.players = set()
        self.projectiles = set()
        self.graph = Graph()
        self.starts = set()

        for y in range(0, self.height):
            row = []
            for x in range(0, self.width):
                type_id = level_str[y * (self.width + 1) + x]
                row.append(Point(
                    x=x,
                    y=y,
                    type_id=type_id,
                    theme_id=('金' if type_id in 'iwX' else None),
                    orientation=None
                ))
            self.level.append(row)

        for pt in self.all_points:
            if pt.type_id == 'X':
                self.starts.add(pt)
            self.set_node(pt)

    def set_node(self, pt):
        ptype = ptypes[pt.type_id]
        if ptype.layer == 'e':
            cost = 50
        elif ptype.layer == 'd':
            cost = 6
        elif ptype.layer == 'c':
            cost = 0.99
        else:
            cost = 1
        neighbors = []
        if pt.x > 0:
            neighbors.append(self.level[pt.y][pt.x - 1])
        if pt.x < self.width - 1:
            neighbors.append(self.level[pt.y][pt.x + 1])
        if pt.y > 0:
            neighbors.append(self.level[pt.y - 1][pt.x])
        if pt.y < self.height - 1:
            neighbors.append(self.level[pt.y + 1][pt.x])
        for npt in neighbors:
            self.graph.add_edge((npt.x, npt.y), (pt.x, pt.y), cost)

    def find_path(self, pt1, pt2):
        def manhattan(u, v, edge, prev_edge):
            return abs(u[0] - v[0]) + abs(u[1] - v[1])
        try:
            nodes, edges, costs, final_cost = find_path(
                self.graph, pt1, pt2, heuristic_func=manhattan
            )
            result = []
            f = self.frame
            for i, (x, y) in enumerate(nodes):
                ptype = ptypes[self.level[y][x].type_id]
                if ptype.layer in ('d', 'e'):
                    return result
                if f > self.frame and self.check_conflict(f, x, y):
                    if self.check_conflict((f + 1) % 1000, x, y):
                        return result
                    else:
                        result.append(result[-1])
                        f += 1
                result.append((x, y))
                f += 1
            return result
        except NoPathError:
            return []

    def find_line(self, x, y, direction):
        path = []
        layer = None
        destroy = False
        while not layer or layer not in ('c', 'd', 'e'):
            path.append((x, y))
            if direction == 'l':
                x -= 1
            if direction == 'r':
                x += 1
            if direction == 'u':
                y -= 1
            if direction == 'd':
                y += 1
            layer = ptypes[self.level[y][x].type_id].layer
        if layer in ('c', 'd'):
            path.append((x, y))
            destroy = True
        return path, destroy

    def check_conflict(self, frame, x, y):
        for player in self.players:
            if not player.plan:
                if player.x == x and player.y == y:
                    return True
                else:
                    continue

            if frame in player.plan:
                if player.plan[frame] == (x, y):
                    return True

            elif player.target_x == x and player.target_y == y:
                return True

    def to_str(self):
        s = 'GRID '
        for row in self.level:
           for pt in row:
               s += pt.type_id
           s += '\n'
        return s

    @property
    def all_points(self):
        for row in self.level:
           for c in row:
               yield c

    @asyncio.coroutine
    def tick(self):
        yield from asyncio.sleep(0.1)
        self.frame += 1
        self.frame = self.frame % 1000
        if self.frame % 100 == 0:
            self.sync_frame += 1
            if self.sync_frame == 1:
                self.sync_frame = 0
                yield from self.broadcast("TICK %s" % self.frame)
        for player in list(self.players):
            yield from player.tick(self.frame)
            for proj in list(self.projectiles):
                yield from self.check_collide(proj, player)
        for proj in list(self.projectiles):
            yield from proj.tick(self.frame)
            for proj2 in list(self.projectiles):
                yield from self.check_collide(proj2, proj)
            for player in list(self.players):
                yield from self.check_collide(proj, player)

    @asyncio.coroutine
    def check_collide(self, proj, obj):
        if proj.x != obj.x or proj.y != obj.y or proj.id == obj.id:
            return
        if isinstance(obj, Player) and proj.player.id == obj.id:
            return
        yield from self.remove_projectile(proj)
        if isinstance(obj, Projectile):
            yield from self.remove_projectile(obj)
        else:
            yield from obj.add_hp(-1)

    @asyncio.coroutine
    def process(self, player, data):
        if not data:
            return
        vals = data.split(" ")
        cmd, args = vals[0], vals[1:]
        fn = getattr(self, 'process_%s' % cmd.lower(), None)
        if fn:
            yield from fn(player, *args)

    @asyncio.coroutine
    def process_point(self, player, x, y, type_id, theme_id=None, orientation=None):
        pt = self.level[int(y)][int(x)]
        pt.type_id = type_id
        pt.theme_id = theme_id
        pt.orientation = orientation
        self.set_node(pt)
        yield from self.broadcast(pt.to_str())

    @asyncio.coroutine
    def process_go(self, player, x, y):
        if player.set_target(self.frame, int(x), int(y)):
            yield from self.broadcast(player.path_str(self.frame))

    @asyncio.coroutine
    def broadcast(self, msg):
        print(msg)
        for player in self.players:
            print("Sending to Player ", player.id)
            yield from player.send(msg)

    @asyncio.coroutine
    def new_player(self, client):
        player = Player(client, self)
        self.players.add(player)

        tstr = "THEMES"
        for theme_id, theme in themes.items():
            if not theme.elemental:
                tstr += " %s" % theme_id
        yield from player.send(tstr)

        sstr = "SHIPS"
        for type_id, ptype in ptypes.items():
            if type_id.isdigit():
                sstr += " %s" % type_id
        yield from player.send(sstr)

        for team in self.teams:
            yield from player.send(team.to_str())

        for pl in self.players:
            if not pl.active:
                continue
            yield from player.send(pl.to_str())

        return player

    def start_player(self, player):
        player.active = True
        self.locate_player(player, initial=True)
        player.hp = 10
        player.lives = 3
        yield from self.send_initial(player)

    def locate_player(self, player, initial=False):
        if not initial:
            tx = player.x
            ty = player.y
            choices = player.team.starts
        else:
            if len(player.team.starts) == 0:
                ty = 0
                if player.team.id == 1:
                    tx = 0
                elif player.team.id == 2:
                    tx = self.width
                elif player.team.id == 3:
                    tx = self.width / 2
                else:
                    tx = random.randint(0, self.width)
            else:
                tx = 0
                ty = 0
                for start in player.team.starts:
                    tx += start.x
                    ty += start.y
                tx /= len(player.team.starts)
                ty /= len(player.team.starts)

            choices = []
            for i in range(0, int(len(self.starts) / 2)):
                start = None
                while start is None or start.theme_id != '金':
                    start = random.choice(list(self.starts))
                choices.append(start)
        choices = sorted(
            choices,
            key=lambda pt: abs(pt.x - tx) + abs(pt.y - ty)
        )
        player.x = choices[0].x
        player.y = choices[0].y

    @asyncio.coroutine
    def remove_player(self, player):
        if player in self.players:
            self.players.remove(player)

    @asyncio.coroutine
    def process_fire(self, player, direction):
        proj = Projectile(player, direction, self)
        self.projectiles.add(proj)
        yield from self.broadcast(proj.to_str())
        yield from self.broadcast(proj.path_str(self.frame))

    @asyncio.coroutine
    def process_team(self, player, name, theme_id):
        for team in self.teams:
            if team.theme_id == theme_id:
                return
        team = Team(self, name, theme_id)
        self.teams.add(team)
        yield from self.broadcast(team.to_str())

    @asyncio.coroutine
    def process_join(self, player, team_id):
        for team in self.teams:
            if str(team.id) == team_id:
                team.add_player(player)
                player.team = team

        if not player.team:
            raise Exception("Failed to find team")

    @asyncio.coroutine
    def process_ship(self, player, ship_id, name):
        player.type_id = ship_id
        player.name = name
        yield from self.start_player(player)

    @asyncio.coroutine
    def remove_projectile(self, projectile):
        if projectile in self.projectiles:
            self.projectiles.remove(projectile)

    @asyncio.coroutine
    def process_edit(self, player):
        yield from player.send(self.to_str())

    @asyncio.coroutine
    def send_initial(self, player):
        yield from player.send(self.to_str())
        yield from player.send(player.to_str(True))
        yield from player.send("TICK %s" % self.frame)
        for pt in self.all_points:
            if pt.theme_id or pt.orientation:
                 yield from player.send(pt.to_str())
        for pl in self.players:
            if not pl.active:
                continue
            yield from player.send(pl.path_str(self.frame))
        yield from self.broadcast(player.to_str())
def directed_and_guided_map_init ():
  graph = None
  cost_func = None
  graph = Graph()
# 4 real beacons
  graph.add_edge(1, 2, {'cost': 3})
  graph.add_edge(2, 1, {'cost': 3})
  graph.add_edge(2, 3, {'cost': 4})
  graph.add_edge(3, 2, {'cost': 4})
  graph.add_edge(3, 4, {'cost': 2})
  graph.add_edge(4, 3, {'cost': 2})
  graph.add_edge(1, 7, {'cost': 4})
  graph.add_edge(7, 1, {'cost': 4})
# 3 virtual beacons that does not actually exist
  graph.add_edge(3, 4, {'cost': 5})
  graph.add_edge(4, 3, {'cost': 5})
  graph.add_edge(3, 5, {'cost': 5})
  graph.add_edge(5, 3, {'cost': 5})
  graph.add_edge(2, 5, {'cost': 6})
  graph.add_edge(5, 2, {'cost': 6})
  graph.add_edge(1, 6, {'cost': 7})
  graph.add_edge(6, 1, {'cost': 7})

  cost_func = lambda u, v, e, prev_e: e['cost']

  return graph, cost_func
Пример #37
0
class House3DUtils():
    def __init__(
            self,
            env,
            rotation_sensitivity=9,
            move_sensitivity=0.5,
            build_graph=False,
            graph_dir='/path/to/3d-graphs',
            target_obj_conn_map_dir='/path/to/target_obj_connmaps',
            debug=True,
            load_semantic_classes=True,
            collision_reward=0.0,
            success_reward=1.0,
            dist_reward_scale=0.005,
            seeing_rwd=False):
        self.env = env
        self.debug = debug

        self.rotation_sensitivity = rotation_sensitivity
        self.move_sensitivity = move_sensitivity

        self.angles = [x for x in range(-180, 180, self.rotation_sensitivity)]
        self.angle_strings = {1: 'right', -1: 'left'}

        self.dirs, self.angle_map = self.calibrate_steps(reset=True)
        self.move_multiplier = self.move_sensitivity / np.array([np.abs(x).sum() for x in self.dirs]).mean()

        self.graph_dir = graph_dir
        self.graph = None

        self.target_obj_conn_map_dir = target_obj_conn_map_dir

        if build_graph == True:
            if os.path.exists(
                    os.path.join(graph_dir,
                                 self.env.house.house['id'] + '.pkl')):
                self.load_graph(
                    os.path.join(graph_dir,
                                 self.env.house.house['id'] + '.pkl'))
            else:
                self.build_graph(
                    save_path=os.path.join(
                        graph_dir, self.env.house.house['id'] + '.pkl'))

        self.rooms, self.objects = self._parse()

        self.collision_reward = collision_reward
        self.success_reward = success_reward
        self.dist_reward_scale = dist_reward_scale
        self.seeing_rwd = seeing_rwd

        if load_semantic_classes == True:
            self._load_semantic_classes()

    # Shortest paths are computed in 1000 x 1000 grid coordinates.
    # One step in the SUNCG continuous coordinate system however, can be
    # multiple grids in the grid coordinate system (since turns aren't 90 deg).
    # So even though the grid shortest path is fine-grained,
    # an equivalent best-fit path in SUNCG continuous coordinates
    # has to be computed by simulating steps. Sucks, but yeah.
    #
    # For now, we first explicitly calibrate how many steps in the gridworld
    # correspond to one step in continuous world, across all directions
    def calibrate_steps(self, reset=True):
        mults, angle_map = [], {}

        cx, cy = self.env.house.to_coor(50, 50)
        if reset == True:
            self.env.reset(x=cx, y=cy)

        for i in range(len(self.angles)):
            yaw = self.angles[i]

            self.env.cam.yaw = yaw
            self.env.cam.updateDirection()

            x1, y1 = self.env.house.to_grid(self.env.cam.pos.x,
                                            self.env.cam.pos.z)

            pos = self.env.cam.pos
            pos = pos + self.env.cam.front * self.move_sensitivity

            x2, y2 = self.env.house.to_grid(pos.x, pos.z)

            mult = np.array([x2, y2]) - np.array([x1, y1])
            mult = (mult[0], mult[1])

            angle_map[mult] = yaw
            mults.append(mult)

        return mults, angle_map

    # 0: forward
    # 1: left
    # 2: right
    # 3: stop
    #
    # returns observation, reward, done, info
    def step(self, action, step_reward=False):
        if action not in [0, 1, 2, 3]:
            raise IndexError

        if step_reward == True:
            pos = self.env.cam.pos
            x1, y1 = self.env.house.to_grid(self.env.cam.pos.x, self.env.cam.pos.z)
            init_target_dist = self.env.house.connMap[x1, y1]

        reward = 0
        done = False

        if action == 0:
            mv = self.env.move_forward(
                dist_fwd=self.move_sensitivity, dist_hor=0)
            obs = self.env.render()
            if mv == False:  # collision
                reward -= self.collision_reward
            elif mv != False and step_reward == True:
                # evaluate connMap dist here
                x2, y2 = self.env.house.to_grid(self.env.cam.pos.x,
                                                self.env.cam.pos.z)
                final_target_dist = self.env.house.connMap[x2, y2]
                reward += self.dist_reward_scale * ((init_target_dist - final_target_dist) / np.abs(
                    self.dirs[self.angles.index(self.env.cam.yaw % 180)]).sum())

        elif action == 1:
            self.env.rotate(-self.rotation_sensitivity)
            obs = self.env.render()

        elif action == 2:
            self.env.rotate(self.rotation_sensitivity)
            obs = self.env.render()

        elif action == 3:
            done = True
            obs = self.env.render()

        return obs, reward, done

    # pos: [x, y, z, yaw], or objrender.Vec3
    def get_dist_to_target(self, pos):
        if isinstance(pos, Vec3) == True:
            x, y = self.env.house.to_grid(pos.x, pos.z)
        else:
            x, y = self.env.house.to_grid(pos[0], pos[2])
        dist = self.env.house.connMap[x, y]
        return self.move_multiplier * dist

    def is_inside_room(self, pos, room):
        if isinstance(pos, Vec3) == True:
            x = pos.x
            y = pos.z
        else:
            x = pos[0]
            y = pos[2]
        if x >= room['bbox']['min'][0] and x <= room['bbox']['max'][0] and \
            y >= room['bbox']['min'][2] and y <= room['bbox']['max'][2]:
            return True
        return False

    # takes 200-300 seconds(!) when rotation_sensitivity == 9
    def build_graph(self, save_path=None):
        import time
        start_time = time.time()

        collide_res = self.env.house.n_row

        from dijkstar import Graph

        visit = dict()
        self.graph = Graph()

        self.mock_obs_map = np.zeros(
            (collide_res + 1, collide_res + 1), dtype=np.uint8)
        self.mock_obs_map[np.where(self.env.house.connMap == -1)] = 1

        for x in range(collide_res + 1):
            for y in range(collide_res + 1):
                pos = (x, y)
                if self.env.house.canMove(x, y) and pos not in visit:
                    que = [pos]
                    visit[pos] = True
                    ptr = 0
                    while ptr < len(que):
                        cx, cy = que[ptr]
                        ptr += 1

                        # add all angles for (cx, cy) here
                        # connect first and last
                        for ang in range(len(self.angles) - 1):
                            self.graph.add_edge((cx, cy, self.angles[ang]),
                                                (cx, cy, self.angles[ang + 1]),
                                                {
                                                    'cost': 1
                                                })
                            self.graph.add_edge((cx, cy, self.angles[ang + 1]),
                                                (cx, cy, self.angles[ang]), {
                                                    'cost': 1
                                                })
                        self.graph.add_edge((cx, cy, self.angles[-1]),
                                            (cx, cy, self.angles[0]), {
                                                'cost': 1
                                            })
                        self.graph.add_edge((cx, cy, self.angles[0]),
                                            (cx, cy, self.angles[-1]), {
                                                'cost': 1
                                            })

                        for deti in range(len(self.dirs)):
                            det = self.dirs[deti]
                            tx, ty = cx + det[0], cy + det[1]
                            if (self.env.house.inside(tx, ty) and
                                    self.mock_obs_map[min(cx, tx):max(cx, tx)+1,
                                                      min(cy, ty):max(cy, ty)+1].sum() == 0):
                                # make changes here to add edges for angle increments as well
                                #
                                # cost = 1 from one angle to the next,
                                # and connect first and last
                                # this would be for different angles for same tx, ty
                                #
                                # then there would be connections for same angle
                                # and from (cx, cy) to (tx, ty)
                                self.graph.add_edge(
                                    (cx, cy, self.angle_map[self.dirs[deti]]),
                                    (tx, ty, self.angle_map[self.dirs[deti]]),
                                    {
                                        'cost': 1
                                    })
                                tp = (tx, ty)
                                if tp not in visit:
                                    visit[tp] = True
                                    que.append(tp)

        if self.debug == True:
            print("--- %s seconds to build the graph ---" %
                  (time.time() - start_time))

        if save_path != None:
            start_time = time.time()

            print("saving graph to %s" % (save_path))
            self.graph.dump(save_path)

            if self.debug == True:
                print("--- %s seconds to save the graph ---" %
                      (time.time() - start_time))

    def load_graph(self, path):
        import time
        start_time = time.time()

        from dijkstar import Graph

        self.graph = Graph()
        self.graph.load(path)

        if self.debug == True:
            print("--- %s seconds to load the graph ---" %
                  (time.time() - start_time))

    # takes 1-5 seconds when rotation_sensitivity == 9
    def compute_shortest_path(self, source, target, graph=None):
        from dijkstar import find_path

        if graph == None:
            if self.graph == None:
                if os.path.exists(
                        os.path.join(self.graph_dir,
                                     self.env.house.house['id'] + '.pkl')):
                    self.load_graph(
                        os.path.join(self.graph_dir,
                                     self.env.house.house['id'] + '.pkl'))
                else:
                    self.build_graph(
                        save_path=os.path.join(
                            graph_dir, self.env.house.house['id'] + '.pkl'))
            graph = self.graph

        cost_func = lambda u, v, e, prev_e: e['cost']
        shortest_path = find_path(graph, source, target, cost_func=cost_func)

        return shortest_path

    def fit_grid_path_to_suncg(self, nodes, init_yaw=None, back_skip=2):

        # don't mess with the originals
        nodes = copy.deepcopy(nodes)

        # set initial position
        x, y = self.env.house.to_coor(nodes[0][0], nodes[0][1], True)
        x, y = x.astype(np.float32).item(), y.astype(np.float32).item()

        self.env.cam.pos.x, self.env.cam.pos.y, self.env.cam.pos.z = x, self.env.house.robotHei, y
        if init_yaw == None:
            self.env.cam.yaw = np.random.choice(self.angles)
        else:
            self.env.cam.yaw = init_yaw
        self.env.cam.updateDirection()

        pos_queue, action_queue = [], []

        current_pos = self._vec_to_array(self.env.cam.pos, self.env.cam.yaw)
        pos_queue = pos_queue + [current_pos]

        ptr = 0

        while ptr < len(nodes) - 1:
            turned = False

            # target rotation
            target_yaw = self.angle_map[tuple(
                np.array(nodes[ptr]) - np.array(nodes[ptr + 1]))]

            # turn
            if target_yaw != current_pos[3]:
                p_q, a_q = self.get_rotate_steps(current_pos, target_yaw)

                pos_queue = pos_queue + p_q
                action_queue = action_queue + a_q

                self.env.cam.yaw = target_yaw
                self.env.cam.updateDirection()

                turned = True
                current_pos = self._vec_to_array(self.env.cam.pos,
                                                 self.env.cam.yaw)

            # move
            cx, cz = self.env.house.to_coor(nodes[ptr + 1][0],
                                            nodes[ptr + 1][1], True)

            # if collision, find another sub-path, and delete that edge
            if self.env.move(cx, cz) == False:
                if nodes[ptr + 1] in self.graph[nodes[ptr]]:
                    del self.graph[nodes[ptr]][nodes[ptr + 1]]
                    print('deleted', nodes[ptr], nodes[ptr + 1])

                # delete the turns
                if turned == True:
                    pos_queue = pos_queue[:-len(p_q)]
                    action_queue = action_queue[:-len(a_q)]

                if back_skip != 0:
                    pos_queue = pos_queue[:-back_skip]
                    action_queue = action_queue[:-back_skip]

                dest_ptr = ptr + 1
                ptr = ptr - back_skip

                sub_shortest_path = self.compute_shortest_path(
                    nodes[ptr], nodes[dest_ptr])
                nodes = nodes[:ptr] + sub_shortest_path.nodes + nodes[dest_ptr
                                                                      + 1:]

                current_pos = pos_queue[-1]
            else:
                # this is the new position the agent moved to
                current_pos = self._vec_to_array(self.env.cam.pos,
                                                 self.env.cam.yaw)

                assert current_pos[3] == pos_queue[-1][3] and (
                    current_pos[0] != pos_queue[-1][0]
                    or current_pos[2] != pos_queue[-1][2])

                pos_queue = pos_queue + [current_pos]
                action_queue = action_queue + ['fwd']

                ptr = ptr + 1

        action_queue.append('stop')

        return pos_queue, action_queue

    # pos contains [x, y, z, yaw]
    # given a position and target yaw, this function
    # computes actions needed to turn there
    def get_rotate_steps(self, pos, target_yaw):

        direction = np.random.choice([1, -1])

        cur_yaw = pos[-1]
        ptr = self.angles.index(cur_yaw)
        pos_queue, action_queue = [], []

        while cur_yaw != target_yaw:
            if len(pos_queue) == len(self.angles) // 2:
                # reset
                direction = direction * -1
                cur_yaw = pos[-1]
                ptr = self.angles.index(cur_yaw)
                pos_queue, action_queue = [], []

            ptr = (ptr + direction) % len(self.angles)
            cur_yaw = self.angles[ptr]

            pos_queue.append([pos[0], pos[1], pos[2], self.angles[ptr]])
            action_queue.append(self.angle_strings[direction])

        return pos_queue, action_queue

    def _vec_to_array(self, pos, yaw):
        return [pos.x, pos.y, pos.z, yaw]

    # render images from camera position queue
    def render_images_from_pos_queue(self,
                                     pos_queue=[],
                                     img_dir='tmp/images',
                                     actions=None,
                                     values=None,
                                     rewards=None):
        if len(pos_queue) == 0:
            return False

        action_map = {0: 'FRWD', 1: 'LEFT', 2: 'RGHT', 3: 'STOP'}

        import scipy.misc

        sgx, sgy = self.env.house.to_grid(pos_queue[0][0], pos_queue[0][2])
        tgx, tgy = self.env.house.to_grid(pos_queue[-1][0], pos_queue[-1][2])

        for i in range(len(pos_queue)):
            # set position
            p = pos_queue[i]
            self.env.reset(x=p[0], y=p[2], yaw=p[3])

            # save image
            image = np.array(self.env.render(), copy=False)

            # put some text
            text = "[%02d]" % (i + 1)

            if actions != None and i < len(actions):
                text += "[%s]" % action_map[actions[i]]

            if values != None and i < len(values):
                text += "[V%.03f]" % values[i]

            if rewards != None and i > 0 and i <= len(rewards):
                text += "[R%.03f]" % rewards[i - 1]

            image = cv2.putText(
                img=np.copy(image),
                text=text,
                org=(20, 30),
                fontFace=3,
                fontScale=0.4,
                color=(255, 255, 255),
                thickness=1)

            scipy.misc.toimage(image).save(
                '%s/%s_%04d_%04d_%04d_%04d_%05d_%05d.jpg' %
                (img_dir, self.env.house.house['id'], sgx, sgy, tgx, tgy,
                 i + 1, len(pos_queue)))

        return True

    # render video from camera position queue
    #
    # NOTE: call `render_images_from_pos_queue` before calling this
    def render_video_from_pos_queue(self,
                                    pos_queue=[],
                                    img_dir='tmp/images',
                                    vid_dir='tmp/videos',
                                    fps=[5],
                                    tag_name='piano'):
        if len(pos_queue) == 0:
            return False

        import subprocess

        sgx, sgy = self.env.house.to_grid(pos_queue[0][0], pos_queue[0][2])
        tgx, tgy = self.env.house.to_grid(pos_queue[-1][0], pos_queue[-1][2])

        for fp in fps:
            subprocess.Popen([
                '/srv/share/abhshkdz/local/bin/ffmpeg', '-f', 'image2', '-r',
                str(fp), '-i',
                '%s/%s_%04d_%04d_%04d_%04d' %
                (img_dir, self.env.house.house['id'], sgx, sgy, tgx, tgy) +
                '_%05d_' + '%05d.jpg' % (len(pos_queue)), '-vcodec', 'libx264',
                '-crf', '25', '-y',
                '%s/%s_%04d_%04d_%s_%04d_%04d_%d.mp4' %
                (vid_dir, self.env.house.house['id'], sgx, sgy, tag_name, tgx,
                 tgy, fp)
            ])

            if self.debug == True:
                print('Rendered video to ' +
                      '%s/%s_%04d_%04d_%s_%04d_%04d_%d.mp4' %
                      (vid_dir, self.env.house.house['id'], sgx, sgy, tag_name,
                       tgx, tgy, fp))

        return True

    # Go over all nodes of house environment and accumulate objects room-wise.
    def _parse(self, levelsToExplore=[0]):
        rooms, objects = [], {}
        data = self.env.house.house

        modelCategoryMapping = {}

        import csv
        csvFile = csv.reader(open(self.env.house.metaDataFile, 'r'))
        headers = next(csvFile)

        for row in csvFile:
            modelCategoryMapping[row[headers.index('model_id')]] = {
                headers[x]: row[x]
                for x in range(2, len(headers))  # 0 is index, 1 is model_id
            }

        for i in levelsToExplore:
            for j in range(len(data['levels'][i]['nodes'])):
                assert data['levels'][i]['nodes'][j]['type'] != 'Box'

                if 'valid' in data['levels'][i]['nodes'][j]:
                    assert data['levels'][i]['nodes'][j]['valid'] == 1

                # Rooms
                if data['levels'][i]['nodes'][j]['type'] == 'Room':
                    if 'roomTypes' not in data['levels'][i]['nodes'][j]:
                        continue

                    # Can rooms have more than one type?
                    # Yes, they can; just found ['Living_Room', 'Dining_Room', 'Kitchen']
                    # assert len(data['levels'][i]['nodes'][j]['roomTypes']) <= 3

                    roomType = [
                        # ' '.join(x.lower().split('_'))
                        x.lower()
                        for x in data['levels'][i]['nodes'][j]['roomTypes']
                    ]

                    nodes = data['levels'][i]['nodes'][j][
                        'nodeIndices'] if 'nodeIndices' in data['levels'][i][
                            'nodes'][j] else []
                    rooms.append({
                        'type':
                        roomType,
                        'bbox':
                        data['levels'][i]['nodes'][j]['bbox'],
                        'nodes':
                        nodes,
                        'model_id':
                        data['levels'][i]['nodes'][j]['modelId']
                    })

                # Objects
                elif data['levels'][i]['nodes'][j]['type'] == 'Object':
                    if 'materials' not in data['levels'][i]['nodes'][j]:
                        material = []
                    else:
                        material = data['levels'][i]['nodes'][j]['materials']
                    objects[data['levels'][i]['nodes'][j]['id']] = {
                        'id':
                        data['levels'][i]['nodes'][j]['id'],
                        'model_id':
                        data['levels'][i]['nodes'][j]['modelId'],
                        'fine_class':
                        modelCategoryMapping[data['levels'][i]['nodes'][j][
                            'modelId']]['fine_grained_class'],
                        'coarse_class':
                        modelCategoryMapping[data['levels'][i]['nodes'][j][
                            'modelId']]['coarse_grained_class'],
                        'bbox':
                        data['levels'][i]['nodes'][j]['bbox'],
                        'mat':
                        material
                    }

        return rooms, objects

    # Spawn at a randomly selected point in a particular room
    def spawn_room(self, room=None):
        if room == None:
            return False, None

        target_room = '_'.join(room.lower().split(' '))

        if self.env.house.hasRoomType(target_room) == False:
            return False, None

        rooms = self.env.house._getRooms(target_room)
        room = np.random.choice(rooms)

        gx1, gy1, gx2, gy2 = self.env.house._getRoomBounds(room)

        available_coords = []
        for x in range(gx1, gx2 + 1):
            for y in range(gy1, gy2 + 1):
                if self.env.house.moveMap[x, y] > 0:
                    available_coords.append((x, y))

        # print(available_coords)
        spawn_coord_idx = np.random.choice(len(available_coords))
        spawn_coord = available_coords[spawn_coord_idx]

        return spawn_coord, room

    # Spawn close to an object
    # If room given, look for object within room
    def spawn_object(self, obj=None, room=None):
        if object == None:
            return False, None

        if isinstance(obj, list) == False:
            obj = [obj]

        is_door = False
        if 'door' in obj:
            is_door = True

        target_obj = ['_'.join(x.lower().split(' ')) for x in obj]

        if room != None:
            if 'nodeIndices' in room:
                objs = [
                    self.objects['0_' + str(x)] for x in room['nodeIndices']
                    if self.objects['0_' + str(x)]['fine_class'] in target_obj
                ]
            else:
                objs = [
                    self.objects['0_' + str(x)] for x in room['nodes']
                    if self.objects['0_' + str(x)]['fine_class'] in target_obj
                ]
        else:
            obj_id_list = list(
                itertools.chain.from_iterable(
                    [x['nodes'] for x in self.rooms if x['type'] != []]))
            objs = [
                self.objects['0_' + str(x)] for x in obj_id_list
                if self.objects['0_' + str(x)]['fine_class'] in target_obj
            ]

        if len(objs) == 0:
            return False, None, None

        obj_idx = np.random.choice(len(objs))
        obj = objs[obj_idx]

        self.target_obj_class = obj['fine_class'].lower()

        gx1, gy1, gx2, gy2 = self.env.house._getRoomBounds(obj)

        if room == None:
            obj_node_idx = int(obj['id'][2:])
            room = [
                x for x in self.env.house.all_rooms
                if 'nodeIndices' in x and obj_node_idx in x['nodeIndices']
            ][0]

        self.set_target_object(obj, room)

        available_x, available_y = np.where(self.env.house.connMap == 0)

        if len(available_x) == 0:
            return False, None, None

        spawn_coords = []
        for i in range(len(available_x)):
            spawn_coords.append((available_x[i], available_y[i]))

        return spawn_coords, obj, room

    # analogous to `setTargetRoom` in the House3D API
    def set_target_object(self, obj, room):
        object_tp = room['id'] + '_' + obj['id'] + '_' + obj['fine_class'].lower(
        )
        # Caching
        if object_tp in self.env.house.connMapDict:
            self.env.house.connMap, self.env.house.connectedCoors, self.env.house.inroomDist, self.env.house.maxConnDist = self.env.house.connMapDict[
                object_tp]
            return True  # object changed!
        elif os.path.exists(
                os.path.join(
                    self.target_obj_conn_map_dir,
                    self.env.house.house['id'] + '_' + object_tp + '.npy')):
            self.env.house.connMap = np.load(
                os.path.join(
                    self.target_obj_conn_map_dir,
                    self.env.house.house['id'] + '_' + object_tp + '.npy'))

            if self.env.house.connMap.shape[0] == self.env.house.n_row+1:
                self.env.house.connectedCoors, self.env.house.inroomDist, self.env.house.maxConnDist = None, None, None
                return True

        self.env.house.connMap = connMap = np.ones(
            (self.env.house.n_row + 1, self.env.house.n_row + 1),
            dtype=np.int32) * -1
        self.env.house.inroomDist = inroomDist = np.ones(
            (self.env.house.n_row + 1, self.env.house.n_row + 1),
            dtype=np.float32) * -1
        dirs = [[0, 1], [1, 0], [-1, 0], [0, -1]]
        que = []
        flag_find_open_components = True

        _ox1, _, _oy1 = obj['bbox']['min']
        _ox2, _, _oy2 = obj['bbox']['max']
        ocx, ocy = (_ox1 + _ox2) / 2, (_oy1 + _oy2) / 2
        ox1, oy1, ox2, oy2 = self.env.house.rescale(_ox1, _oy1, _ox2, _oy2)

        for _ in range(2):
            _x1, _, _y1 = room['bbox']['min']
            _x2, _, _y2 = room['bbox']['max']
            cx, cy = (_x1 + _x2) / 2, (_y1 + _y2) / 2
            x1, y1, x2, y2 = self.env.house.rescale(_x1, _y1, _x2, _y2)

            curr_components = self.env.house._find_components(
                x1,
                y1,
                x2,
                y2,
                dirs=dirs,
                return_open=flag_find_open_components
            )  # find all the open components
            if len(curr_components) == 0:
                print('No space found! =(')
                raise ValueError('no space')
            if isinstance(curr_components[0],
                          list):  # join all the coors in the open components
                curr_major_coors = list(itertools.chain(*curr_components))
            else:
                curr_major_coors = curr_components
            min_dist_to_center, min_dist_to_edge = 1e50, 1e50
            for x, y in curr_major_coors:
                ###
                # Compute minimum dist to edge here
                if x in range(ox1, ox2):
                    dx = 0
                elif x < ox1:
                    dx = ox1 - x
                else:
                    dx = x - ox2

                if y in range(oy1, oy2):
                    dy = 0
                elif y < oy1:
                    dy = oy1 - y
                else:
                    dy = y - oy2

                assert dx >= 0 and dy >= 0

                if dx != 0 or dy != 0:
                    dd = np.sqrt(dx**2 + dy**2)
                elif dx == 0:
                    dd = dy
                else:
                    dd = dx

                if dd < min_dist_to_edge:
                    min_dist_to_edge = int(np.ceil(dd))
                ###
                tx, ty = self.env.house.to_coor(x, y)
                tdist = np.sqrt((tx - ocx)**2 + (ty - ocy)**2)
                if tdist < min_dist_to_center:
                    min_dist_to_center = tdist
                inroomDist[x, y] = tdist
            margin = min_dist_to_edge + 1
            for x, y in curr_major_coors:
                inroomDist[x, y] -= min_dist_to_center
            for x, y in curr_major_coors:
                if x in range(ox1 - margin, ox2 + margin) and y in range(
                        oy1 - margin, oy2 + margin):
                    connMap[x, y] = 0
                    que.append((x, y))
            if len(que) > 0: break
            if flag_find_open_components:
                flag_find_open_components = False
            else:
                break
            raise ValueError

        ptr = 0
        self.env.house.maxConnDist = 1
        while ptr < len(que):
            x, y = que[ptr]
            cur_dist = connMap[x, y]
            ptr += 1
            for dx, dy in dirs:
                tx, ty = x + dx, y + dy
                if self.env.house.inside(tx, ty) and self.env.house.canMove(
                        tx, ty) and not self.env.house.isConnect(tx, ty):
                    que.append((tx, ty))
                    connMap[tx, ty] = cur_dist + 1
                    if cur_dist + 1 > self.env.house.maxConnDist:
                        self.env.house.maxConnDist = cur_dist + 1
        self.env.house.connMapDict[object_tp] = (connMap, que, inroomDist,
                                                 self.env.house.maxConnDist)
        np.save(
            os.path.join(
                self.target_obj_conn_map_dir,
                self.env.house.house['id'] + '_' + object_tp + '.npy'),
            connMap)
        self.connectedCoors = que
        print(' >>>> ConnMap Cached!')
        return True  # room changed!

    def _load_semantic_classes(self, color_file=None):
        if color_file == None:
            color_file = self.env.config['colorFile']

        self.semantic_classes = {}

        with open(color_file) as csv_file:
            reader = csv.DictReader(csv_file)
            for row in reader:
                c = np.array((row['r'], row['g'], row['b']), dtype=np.uint8)
                fine_cat = row['name'].lower()
                self.semantic_classes[fine_cat] = c

        return self.semantic_classes

    def _get_best_yaw_obj_from_pos(self, obj_id, grid_pos, height=1.0):
        obj = self.objects[obj_id]
        obj_fine_class = obj['fine_class']

        cx, cy = self.env.house.to_coor(grid_pos[0], grid_pos[1])

        self.env.cam.pos.x = cx
        self.env.cam.pos.y = height
        self.env.cam.pos.z = cy

        best_yaw, best_coverage = None, 0

        for yaw in self.angles:
            self.env.cam.yaw = yaw
            self.env.cam.updateDirection()

            seg = self.env.render(mode='semantic')
            c = self.semantic_classes[obj_fine_class.lower()]
            mask = np.all(seg == c, axis=2)
            coverage = np.sum(mask) / (seg.shape[0] * seg.shape[1])

            if best_yaw == None:
                best_yaw = yaw
                best_coverage = coverage
            else:
                if coverage > best_coverage:
                    best_yaw = yaw
                    best_coverage = coverage

        return best_yaw, best_coverage

    def _get_best_view_obj(self,
                           obj,
                           coverage_thres=0.5,
                           dist_add=0.5,
                           robot_height=False):
        bbox = obj['bbox']
        obj_fine_class = obj['fine_class']

        obj_max = np.asarray(bbox['max'])
        obj_min = np.asarray(bbox['min'])
        obj_center = (obj_min + obj_max) / 2

        c_x, c_y, c_z = obj_center
        max_radius = np.sqrt(
            (obj_max[0] - obj_min[0]) * (obj_max[0] - obj_min[0]) +
            (obj_max[2] - obj_min[2]) * (obj_max[2] - obj_min[2])) / 2.0
        max_radius += dist_add

        best_pos = None
        best_coverage = 0

        returned_pos_cov = []

        for yaw in self.angles:
            pos = [
                c_x - max_radius * np.cos(yaw * (2 * np.pi) / 360.0), c_y,
                c_z - max_radius * np.sin(yaw * (2 * np.pi) / 360.0), yaw
            ]

            if robot_height == True:
                pos[1] = min(max(0.75, c_y), 2.00)

            self.env.cam.pos.x = pos[0]
            self.env.cam.pos.y = pos[1]
            self.env.cam.pos.z = pos[2]
            self.env.cam.yaw = pos[3]

            self.env.cam.updateDirection()

            seg = self.env.render(mode='semantic')
            c = self.semantic_classes[obj_fine_class.lower()]
            mask = np.all(seg == c, axis=2)
            coverage = np.sum(mask) / (seg.shape[0] * seg.shape[1])

            returned_pos_cov.append([pos, coverage])

            if coverage > coverage_thres:
                return pos, coverage, returned_pos_cov
            elif coverage > best_coverage:
                best_coverage = coverage
                best_pos = pos

        return best_pos, best_coverage, returned_pos_cov