def add_edge(self, p1: Vector, p2: Vector):
# Loop through all existing Vectors, checking
# if new Vectors already exists close
# Then return indices or add new
if p2 < p1:
p1, p2 = p2, p1
intersections: List[Tuple[Edge, Vector]] = []
new_edges: List[Any] = []
clear = True
for other in self._edges:
p3, p4 = self._vertices[other.a], self._vertices[other.b]
if p4 < p3:
p3, p4 = p4, p3
i = line_intersection(p1, p2, p3, p4)
if i is not None:
clear = False
if isinstance(i, Vector):
intersections.append((other, i))
elif len(i) == 2:
new_edge_length = p1.dist(p2)
prev_edge_length = p3.dist(p4)
if i[0].dist(i[1]) < self.tolerance:
# Most likely end-to-end
intersections.append((other, i[0]))
elif ((p1.dist(i[0]) < self.tolerance
and p2.dist(i[1]) < self.tolerance)
or (p3.dist(i[0]) < self.tolerance
and p4.dist(i[1]) < self.tolerance)):
# This is one contained inside the other
if new_edge_length > prev_edge_length:
p3 = self._vertices[other.a]
p4 = self._vertices[other.b]
if p4 < p3:
p3, p4 = p4, p3
if p2 < p1:
p1, p2 = p2, p1
self.add_to_segment_queue((p1, p3))
self.add_to_segment_queue((p4, p2))
else:
# Overlap
if p1.x < p3.x:
self._edges.remove(other)
self.add_to_segment_queue(
(p1, self._vertices[other.a]))
self.add_to_segment_queue(
(self._vertices[other.a], p2))
self.add_to_segment_queue(
(p2, self._vertices[other.b]))
else:
self._edges.remove(other)
self.add_to_segment_queue(
(self._vertices[other.a], p1))
self.add_to_segment_queue(
(p1, self._vertices[other.b]))
self.add_to_segment_queue(
(self._vertices[other.b], p2))
else:
raise Exception("WTF")
if len(new_edges) > 0:
for e in new_edges:
self._add_edge(e)
if len(intersections) > 0:
p1_i = self.add_vertex(p1)
p2_i = self.add_vertex(p2)
if p1_i is None or p2_i is None:
return
edge = Edge(p1_i, p2_i)
px_i: List[int] = []
for other, p0 in intersections:
self._edges.remove(other)
p3_i = other.a
p4_i = other.b
p0_i = self.add_vertex(p0)
if p0_i is not None:
px_i.append(p0_i)
if p3_i != p0_i and p0_i is not None:
self._add_edge(Edge(p3_i, p0_i))
if p4_i != p0_i and p0_i is not None:
self._add_edge(Edge(p4_i, p0_i))
sort_key: Callable[[int], float] = lambda x: self._vertices[
x].dist(self.vertices[p1_i if p1_i is not None else x])
px_sorted = sorted(px_i, key=sort_key)
prev_x = p1_i
for px in px_sorted:
if prev_x != px:
self._add_edge(Edge(prev_x, px))
prev_x = px
if prev_x != p2_i:
self._add_edge(Edge(prev_x, p2_i))
if clear:
p1_i = self.add_vertex(p1)
p2_i = self.add_vertex(p2)
if p2_i is None or p1_i is None:
return
edge = Edge(p1_i, p2_i)
self._add_edge(edge)
def test_dist(self):
a = Vector(0, 0)
b = Vector(3, 4)
self.assertEqual(a.dist(b), 5)
