Funktion zur Findung von Sensor Aufpunkten auf Racks implementiert. Dazu unittest.

lib/linesweep_circle.py

@@ -68,7 +68,10 @@ class RackIDs():
 
 
     def add_point_to_rack(self, point:Point, name:str):
-        pass
+        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]
@@ -95,8 +98,7 @@ class RackIDs():
             return True
         else:
             return False
-    
-
+ 
 
 class PointIndex2D:
     def __init__(self):
@@ -196,6 +198,41 @@ def increase_circle(tol, tol_step, line, pt, rack_id, idx, other_rack_id, other_
             break
         radius += tol_step
 
+def find_nearest_rack_from_sensor(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)
+
+
 
 
 # === 3. Verbindungen suchen ===
@@ -357,12 +394,48 @@ class TestLinesweep(unittest.TestCase):
                         'Rack_2-1': [Point(0, 10), Point(5, 10)]}
         
 
-        allids = RackIDs(res_rack_seg)
+        point2rack = RackIDs(res_rack_seg)
 
-        self.assertEqual(allids.get_racks_from_point(Point(1, 0)), ["Rack_1-0", "Rack_2-0"])
-        self.assertEqual(allids.get_racks_from_point(Point(5, 6)), ["Rack_1-0"])
-        self.assertEqual(allids.get_points_from_rack("Rack_2-0"), [Point(1, 0), Point(1, 8)])
+        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)}
+        
+        point2rack = RackIDs(rack_segs)
+
+        sensor_points = {}
+        for s, p in sensors.items():
+            onpoint, rack_name = find_nearest_rack_from_sensor(2, 0.5, p, rack_segs)
+            sensor_points[s] = ( onpoint, rack_name)
+            point2rack.add_point_to_rack(onpoint, rack_name)
+
+        plist = point2rack.get_points_from_rack("Rack_1-0")
+
+        self.assertEqual(plist, [Point(0, 0), Point(0,1), Point(0, 10)])
+
+        
+