Esempio n. 1
0
    def calculate_edge_to_vertex_conflicts(self, robot_radius, nn):
        print("calculate_edge_to_vertex_conflicts")
        for edge in self.edges:
            sure_s = nn.neighbors_in_radius(edge.src.point,
                                            KER.FT(2) * robot_radius)
            sure_d = nn.neighbors_in_radius(edge.dest.point,
                                            KER.FT(2) * robot_radius)
            sure_p = set(
                [self.points_to_nodes[point] for point in sure_s + sure_d])

            for node in sure_p:
                node.edge_conflicts.append(edge)
                edge.node_conflicts.append(node)

            x1 = nn.neighbors_in_radius(
                edge.src.point,
                KER.FT(2) * robot_radius + Config.connection_radius)
            x2 = nn.neighbors_in_radius(
                edge.dest.point,
                KER.FT(2) * robot_radius + Config.connection_radius)
            points = set([self.points_to_nodes[point] for point in x1 + x2])
            for node in points - sure_p:
                if KER.squared_distance(
                        point_d_to_point_2(node.point), edge.segment
                ) < KER.FT(4) * robot_radius * robot_radius:
                    node.edge_conflicts.append(edge)
                    edge.node_conflicts.append(node)
Esempio n. 2
0
 def __init__(self, src, dest, eid):
     self.src = src
     self.dest = dest
     self.name = "e" + str(eid)
     self.segment = KER.Segment_2(src.point_2, dest.point_2)
     self.node_conflicts = []
     self.edge_conflicts = set()
     self.cost = sqrt(
         KER.squared_distance(src.point_2, dest.point_2).to_double())
Esempio n. 3
0
    def calculate_edge_to_edge_conflicts(
        self, robot_radius, nn
    ):  # Dror: this can be improved by looking at edges of "near" vertices
        print("calculate_edge_to_edge_conflicts")
        i = 0
        for edge1 in self.edges:
            if i % 1000 == 0:
                gc.collect()
            print(i)
            i += 1

            sure_s = nn.neighbors_in_radius(edge1.src.point,
                                            KER.FT(2) * robot_radius)
            sure_d = nn.neighbors_in_radius(edge1.dest.point,
                                            KER.FT(2) * robot_radius)
            sure_p = set(
                [self.points_to_nodes[point] for point in sure_s + sure_d])
            sure_e = set()
            for node in sure_p:
                sure_e.update(
                    list(node.in_connections.values()) +
                    list(node.out_connections.values()))

            for edge2 in sure_e:
                if edge1 == edge2 or edge2 in edge1.edge_conflicts:
                    continue
                edge1.edge_conflicts.add(edge2)
                edge2.edge_conflicts.add(edge1)

            maybe_s = nn.neighbors_in_radius(
                edge1.src.point,
                KER.FT(2) * robot_radius +
                KER.FT(2) * Config.connection_radius)
            maybe_d = nn.neighbors_in_radius(
                edge1.dest.point,
                KER.FT(2) * robot_radius +
                KER.FT(2) * Config.connection_radius)
            maybe_p = set(
                [self.points_to_nodes[point] for point in maybe_s + maybe_d])
            maybe_e = set()
            for node in maybe_p:
                maybe_e.update(
                    list(node.in_connections.values()) +
                    list(node.out_connections.values()))

            for edge2 in maybe_e - sure_e:
                if edge1 == edge2 or edge2 in edge1.edge_conflicts:
                    continue
                else:
                    if KER.squared_distance(
                            edge1.segment, edge2.segment
                    ) < KER.FT(4) * robot_radius * robot_radius:
                        edge1.edge_conflicts.add(edge2)
                        edge2.edge_conflicts.add(edge1)
Esempio n. 4
0
 def get_neighbors(points):
     connections_list = [
         list(p.out_connections.keys()) + list(p.in_connections.keys())
         for p in points
     ]
     res = []
     for i in range(len(points)):
         is_good = True
         for next_point in connections_list[i]:
             for j in range(len(points)):
                 if i == j:
                     continue
                 if KER.squared_distance(
                         next_point.point_2, points[j].point_2
                 ) < KER.FT(4) * robot_radius * robot_radius:
                     is_good = False
                     break
             if not is_good:
                 continue
             seg = points[i].in_connections[
                 next_point] if next_point in points[
                     i].in_connections else points[i].out_connections[
                         next_point]
             for j in range(len(points)):
                 if i == j:
                     continue
                 if KER.squared_distance(
                         seg.segment, points[j].point_2
                 ) < KER.FT(4) * robot_radius * robot_radius:
                     is_good = False
                     break
             if not is_good:
                 continue
             res.append((tuple([
                 points[k] if k != i else next_point
                 for k in range(len(points))
             ]), seg))
     return res
Esempio n. 5
0
 def h(p):
     res = 0
     for p1, p2 in zip(p, goal):
         res += sqrt(
             KER.squared_distance(p1.point_2, p2.point_2).to_double())
     return res