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)