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)
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())
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)
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
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