import json from shapely.geometry import LineString, Point from shapely.ops import nearest_points import unittest from collections import defaultdict import bisect class PointSorter: def __init__(self): self._points_by_x = [] # [(x, y)] self._points_by_y = [] # [(y, x)] def add_point(self, x, y): bisect.insort(self._points_by_x, (x, y)) bisect.insort(self._points_by_y, (y, x)) def add_points(self, points:list[Point]): for p in points: self.add_point(p.x, p.y) def query_box(self, x1, x2, y1, y2): # Suche nach x-Grenzen ix1 = bisect.bisect_left(self._points_by_x, (x1, -float('inf'))) ix2 = bisect.bisect_right(self._points_by_x, (x2, float('inf'))) candidates = self._points_by_x[ix1:ix2] # Filtere nach y ret = list() for (x,y) in candidates: if y1 <= y <= y2: ret.append(Point(x,y)) return ret def get_sorted_by_x(self): # Sortiere nach x ret = list() for (x,y) in self._points_by_x: ret.append(Point(x,y)) return ret def get_sorted_by_y(self): # Sortiere nach y ret = list() for (y,x) in self._points_by_y: ret.append(Point(x,y)) return ret def to_json(d, pretty: bool = True) -> str: return json.dumps(d, indent=2 if pretty else None, default=str) #ensure_ascii false für darstellung von "ue" class NodeIDs(): def __init__(self, points=[]): self._counter = 0 self._cord2id = dict() self._id2cord = dict() self.add_points(points) def add_point(self, point:Point): self._counter += 1 self._cord2id[f"{point.x} {point.y}"] = self._counter self._id2cord[f"{self._counter}"] = point def add_points(self, points): for p in points: self.add_point(p) def get_id(self, point:Point) -> int: return self._cord2id[f"{point.x} {point.y}"] def get_point(self, nid:int) -> Point: return self._id2cord[f"{nid}"] def get_ids(self, points:list[Point]) -> list[int]: ret = list() for p in points: nid = self.get_id(p) ret.append(nid) return ret def get_points(self, nids:list[int]) -> list[Point]: ret = list() for n in nids: c = self.get_point(n) ret.append(c) return ret class RackIDs(): def __init__(self, racks=dict()): self._point2rack = dict() self._rack2begend = dict() self.add_racks(racks) def add_rack(self, beg:Point, end:Point, name): #Hier wird Rack nur mit Anfang und Ende hinzugefügt -> wie macht man Zwischenpunkte? if beg in self._point2rack: self._point2rack[beg].append(name) else: self._point2rack[beg] = [name] if end in self._point2rack: self._point2rack[end].append(name) else: self._point2rack[end] = [name] self._rack2begend[name] = [beg, end] # Anfangs und Endpunkte zu Rack Namen merken def get_racks_borders(self) -> dict: ''' Gibt Rack nur mit Anfangs und Endpunkt zurück. {Rack_1_0: "Point(0, 0), Point(0,15)", ... } ''' return self._rack2begend def get_racks_from_all_points(self) -> dict: ''' Gibt zu einem Punkt, diejenigen Racks zurück, auf denen der Punkt liegt. {Point(0, 0): ["Rack_1-0", "Rack_2-0", ...]} ''' return self._point2rack def add_racks(self, racks:dict): for name,v in racks.items(): if len(v) != 2: raise AttributeError self.add_rack(v[0], v[1], name) def add_point_to_rack(self, point:Point, name:str): if point in self._point2rack: self._point2rack[point].append(name) else: self._point2rack[point] = [name] def get_racks_from_point(self, point:Point) -> list[str]: return self._point2rack[point] def get_points_from_rack(self, name:str) -> list[Point]: ''' Gibt zu Namen von Rack zugehörige Punkte aus und sortiert Punkte''' ret = list() pin = PointSorter() for p, l_racks in self._point2rack.items(): if name in l_racks: ret.append(p) pin.add_points(ret) ret_sorted = list() [pa, pe] = self._rack2begend[name] if self.rack_is_horizontal(name): ret_sorted = pin.get_sorted_by_x() else: ret_sorted = pin.get_sorted_by_y() return ret_sorted def rack_is_horizontal(self, name): [pa, pe] = self._rack2begend[name] if pa.y == pe.y: return True else: return False class Anlage(): def __init__(self, ): self._points = PointSorter() self._racks = RackIDs() self._nodeids = NodeIDs() self._sensors = dict() self._sensor_onpoints = dict() def set_racks(self, racks:dict[str, list[Point]]): return self._racks.add_racks(racks) def get_racks(self) -> dict: return self._racks._point2rack def add_point_to_rack(self, point:Point, rname:str): return self._racks.add_point_to_rack(point, rname) def get_points_from_rack(self, rname:str): return self._racks.get_points_from_rack(rname) def add_sensor(self, sname: str, pos:Point): self._sensors[sname] = pos def add_sensors(self, sensors:dict): for sname,pos in sensors.items(): self.add_sensor(sname, pos) def connect_sensors_to_racks(self): for sname, pos in self._sensors.items(): rack_borders = self._racks.get_racks_borders() onpoint, rack_name = self.find_nearest_rack_from_sensor(2, 0.5, pos, rack_borders) self._sensor_onpoints[sname] = (onpoint, rack_name) self.add_point_to_rack(onpoint, rack_name) return self._sensor_onpoints def rack_segmentation(self, racks:dict): ''' Racks werden zu LineString konvertiert. Racks bestehend aus Polylinine werden in einzelne Segmente zerlegt und in Liste gesammelt. ''' rack_segments = [] for rack_id, nodes in racks.items(): # Sortiere Node_1, Node_2, ... sorted_keys = sorted(nodes.keys(), key=lambda k: int(k.split("_")[1])) coords = [tuple(nodes[k]) for k in sorted_keys] for i in range(len(coords) - 1): p1, p2 = coords[i], coords[i+1] line = LineString([p1, p2]) rack_segments.append((rack_id, i, line)) return(rack_segments) def find_rack_endpoints(self, rack_segments): ''' Endpunkte der Racks-Segmente werden in Points konvertiert und in Liste gesammelt''' segment_endpoints = [] for rack_id, idx, line in rack_segments: for pt in [line.coords[0], line.coords[1]]: segment_endpoints.append((rack_id, idx, Point(pt))) return(segment_endpoints) def increase_circle(self, tol, tol_step, line, pt, rack_id, idx, other_rack_id, other_idx, verbindungen, endpoint_pinned): ''' vergrößere Kreis bis Schnittpunkt mit Rack entsteht. Argumente: tol, tol_step -- Toleranz und Schittweite line -- linestring der entlang gelaufen wird rack_id, idx -- Rack_id und index von dem linestring stammt pt -- Punkt der Überprüft wird other_rack_id, other_idx -- Rack zu welchem der zu untersuchende Punkt gehört verbindungen -- Liste an die angefügt wird und die verbindungspunkte speichert endpoint_pinned -- Liste, die Rack und index von dem untersuchten Punkt und den neuen angepinnten Punkt speichert ''' radius = tol_step while radius <= tol: circle = pt.buffer(radius) if circle.intersects(line): contact = circle.intersection(line) if contact.geom_type == "Point": nearest = contact else: nearest = nearest_points(pt, contact)[1] #print(f" 🟡 Kreisberührung bei {nearest} mit {rack_id}_{idx}") verbindungen.append((rack_id, idx, other_rack_id, other_idx, nearest)) # Füge verschobenen Endpunkt zu Liste hinzu. [Punkt gehört zu Rack_Nr, alter Punkt, neuer Punkt, gepinnt an Target_Rack] endpoint_pinned.append((other_rack_id, other_idx, pt, nearest, rack_id)) break radius += tol_step def find_nearest_rack_from_sensor(self, max_dist, coarse_step, sensor:Point, racks:dict) -> tuple[Point, str]: # 1. grobe Kandidatensuche candidate_lines = [] radius = coarse_step rack_lines = dict() while radius <= max_dist: circle = sensor.buffer(radius) for r_name, pts in racks.items(): line = LineString([pts[0], pts[-1]]) #Linestring aus erstem und letzten Eintrag in Rack dict erzeugen if circle.intersects(line): candidate_lines.append((r_name, line)) if candidate_lines: break radius += coarse_step if not candidate_lines: return None, None # 2. Feinbestimmung über Distanz candidates_distance = [ (r_name, line, line.distance(sensor)) for r_name, line in candidate_lines ] # Sortieren nach Abstand candidates_distance.sort(key=lambda x: x[2]) '''# Theoretisch könnten mehrere ähnlich naheliegende Racks zurückgegeben werden.''' r_best, line_best, _ = candidates_distance[0] # Hier wird nur das tatsächlich dem Senso nächste Rack gegriffen # Aufpunkt bestimmen nearest_point = line_best.interpolate(line_best.project(sensor)) return (nearest_point, r_best) def search_connections(self, rack_segments, segment_endpoints, tol, tol_step): ''' Aus Rack Segmenten und Endpunkten der Racks wird unter Berücksichtigung von Toleranz naheliegende Endpunkte gefunden. Zuerst echte Schnittpunkte und im Anschluss via Kreissuche neheliegende Punkte und deren gepinnte Berührpunkte ''' verbindungen = [] endpoint_pinned = [] # === A: Echte Schnittpunkte zwischen Linien finden === ''' Alle Segmente mit allen überprüfen, um echte SP zu finden''' for i, (rack_id1, idx1, line1) in enumerate(rack_segments): #print(f"\n=== Prüfe {rack_id1}_{idx1} auf echte Schnittpunkte") for j, (rack_id2, idx2, line2) in enumerate(rack_segments): if i >= j: continue # keine Duplikate / sich selbst if line1.intersects(line2): inter = line1.intersection(line2) if inter.geom_type == "Point": #print(f"✅ Exakter Schnittpunkt {inter} zwischen {rack_id1}_{idx1} und {rack_id2}_{idx2}") verbindungen.append((rack_id1, idx1, rack_id2, idx2, inter)) # === B: Näherungsweise Verbindung durch Toleranz-Kreise === ''' Entlanglaufen der Racks und Scan nach Endpunkten im Toleranzbereich''' for rack_id, idx, line in rack_segments: #print(f"\n=== Prüfe {rack_id}_{idx1} auf Punkte im Toleranzbereich") for other_rack_id, other_idx, pt in segment_endpoints: if rack_id == other_rack_id: continue # ignoriere eigene Endpunkte # Exakte Schnittpunkte ignorieren if line.intersects(pt): continue dist = line.distance(pt) if dist < tol: self.increase_circle(tol, tol_step, line, pt, rack_id, idx, other_rack_id, other_idx, verbindungen, endpoint_pinned) #print(f"🔍 Punkt {pt} liegt {dist:.2f} von Linie {rack_id}_{idx} entfernt" # === Endpunkte aktualisieren === # Dict erstellen, dass mit dem Key "Rack_id - index" dahinter die Koordinaten von Anfang und Endpunkt speichert d_racks_segments = dict() for rack_id, idx, linestring in rack_segments: key = f"{rack_id}-{idx}" d_racks_segments[key] = [Point(linestring.coords[0]), Point(linestring.coords[1])] #Alle Racks in ihrer eingelesenen Form zum Dict hinzufügen for rack_id, idx, old_pt, new_pt, taget_rack in endpoint_pinned: #Durch verschobene Endpunkte laufen... key = f"{rack_id}-{idx}" coords = d_racks_segments.get(key) if coords: #...und bei Übereinstimmung von Start oder Endkoordinate die ursprüngliche (eingelesene) mit der gepinnten überschreiben # Vergleich mit Startpunkt if Point(coords[0]).equals(old_pt): coords[0] = Point(new_pt.x, new_pt.y) #.x bzw .y übergibt x bzw y Koordinate von Objekt POINT # Vergleich mit Endpunkt elif Point(coords[1]).equals(old_pt): coords[1] = Point(new_pt.x, new_pt.y) d_racks_segments[key] = coords # aktualisieren #Dict erstellen, dass alle Punkte die an einem Rack anschließen speichert d_rack_conn_points = dict() for conn_to_rack, conn_to_idx, conn_from_rack, conn_from_idx, conn_point in verbindungen: key = f"{conn_to_rack}-{conn_to_idx} + {conn_from_rack}-{conn_from_idx}" d_rack_conn_points[key] = [conn_point] d_rack_to_points = dict() #neues Dict für Rack_id - Idx: Alle Punkte auf dem Rack for key, coords in d_racks_segments.items(): # Erst Anfangs und Endpunkt aus d_racks_segments holen # coords = [start_point end_point] d_rack_to_points[key] = coords.copy() for key, point in d_rack_conn_points.items(): # Dann aus d_rack_conn_points alle verbindungspunkte holen und dazu speichern to_rack = key.split(" + ")[0] if to_rack in d_rack_to_points: d_rack_to_points[to_rack].extend(point) for key in d_rack_to_points: unique_points = list({(pt.x, pt.y): pt for pt in d_rack_to_points[key]}.values()) d_rack_to_points[key] = unique_points return [d_racks_segments, d_rack_conn_points] class TestLinesweep(unittest.TestCase): def test_linesweep(self): # === Konfiguration === tol = 200 tol_step = 10 racks_json_str= '''{ "Rack_1": { "Node_1": [ 4946.5, 15774.4 ], "Node_2": [ 4946.5, 3879.4 ] }, "Rack_2": { "Node_1": [ 0.1, 57.6 ], "Node_2": [ 0.1, 3777.6 ], "Node_3": [ 14755.1, 3777.6 ] }, "Rack_3": { "Node_1": [ 185.1, 15865.5 ], "Node_2": [ 12450.7, 15865.5 ] }, "Rack_4": { "Node_1": [ 2866.6, 15774.4 ], "Node_2": [ 2866.6, 3880.4 ] }, "Rack_5": { "Node_1": [ 8866.1, 15774.4 ], "Node_2": [ 8866.1, 3878.4 ] }}''' racks_json = json.loads(racks_json_str) an = Anlage() # === 1. Racks in Segmente zerlegen === ''' Hier werden Racks, die aus "echter" Polylinie bestehen (mehrere Nodes, z.B. Rack 2 in easy.dxf) in einzelne Segmente zerlegt (Node1 -> Node2, Node2 -> Node3)''' rack_segments = an.rack_segmentation(racks_json) # === 2. Alle Endpunkte sammeln === ''' Alle Endpunkte aller Racks als Point gespeichert, um shapely funktionen verwenden zu können''' segment_endpoints = an.find_rack_endpoints(rack_segments) d_racks_segments, d_rack_conn_points = an.search_connections(rack_segments, segment_endpoints, tol, tol_step) res_rack_seg = {'Rack_1-0': [Point(4946.5, 15865.5), Point(4946.5, 3777.6)], 'Rack_2-0': [Point(0.1, 57.6), Point(0.1, 3777.6)], 'Rack_2-1': [Point(0.1, 3777.6), Point(14755.1, 3777.6)], 'Rack_3-0': [Point(185.1, 15865.5), Point(12450.7, 15865.5)], 'Rack_4-0': [Point(2866.6, 15865.5), Point(2866.6, 3777.6)], 'Rack_5-0': [Point(8866.1, 15865.5), Point(8866.1, 3777.6)] } log_res = to_json(res_rack_seg) self.assertEqual(d_racks_segments, res_rack_seg) def test_ids_to_point(self): res_rack_seg = {'Rack_1-0': [Point(1, 0), Point(5, 6)], 'Rack_2-0': [Point(1, 8), Point(1, 0)], 'Rack_2-1': [Point(0, 10), Point(5, 10)]} point2rack = RackIDs(res_rack_seg) self.assertEqual(point2rack.get_racks_from_point(Point(1, 0)), ["Rack_1-0", "Rack_2-0"]) self.assertEqual(point2rack.get_racks_from_point(Point(5, 6)), ["Rack_1-0"]) self.assertEqual(point2rack.get_points_from_rack("Rack_2-0"), [Point(1, 0), Point(1, 8)]) def test_add_point_interim(self): res_rack_seg = {'Rack_1-0': [Point(1, 0), Point(5, 6)], 'Rack_2-0': [Point(1, 8), Point(1, 0)], 'Rack_2-1': [Point(0, 10), Point(5, 10)]} point2rack = RackIDs(res_rack_seg) point2rack.add_point_to_rack(Point(1,4), "Rack_2-0") self.assertEqual(point2rack.get_points_from_rack("Rack_2-0"), [Point(1, 0), Point(1,4), Point(1, 8)]) def test_add_sensor(self): rack_segs = {'Rack_1-0': [Point(0, 0), Point(0, 10)], 'Rack_2-0': [Point(10, -2), Point(10, 5)], 'Rack_2-1': [Point(0, 3), Point(10, 3)]} sensors = {'Sens_1': Point(1, 1), 'Sens_2': Point(2, 4), 'Sens_3': Point(9, 2)} an = Anlage() point2rack = an.set_racks(rack_segs) an.add_sensors(sensors) plist1 = an.get_points_from_rack("Rack_1-0") an.connect_sensors_to_racks() plist2 = an.get_points_from_rack("Rack_1-0") self.assertEqual(plist1, [Point(0, 0), Point(0, 10)]) self.assertEqual(plist2, [Point(0, 0), Point(0,1), Point(0, 10)]) if __name__ == '__main__': unittest.main()