diff --git a/lib/plant.py b/lib/plant.py
index 778ad9e..2b1baf8 100644
--- a/lib/plant.py
+++ b/lib/plant.py
@@ -9,21 +9,27 @@ import matplotlib.pyplot as plt
 from itertools import pairwise, combinations, permutations
 import re
 from shapely.strtree import STRtree
+import math
+import shapely
 
 # Globale Variable, die in main aufgerufen wird und steuert ob Graphen in unittests gezeichnet werden
 draw = False
 class PointSorter:
     def __init__(self):
-        self._points_by_x = []  # [(x, y)]
-        self._points_by_y = []  # [(y, x)]
+        #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))
+        self.points = []
+
+    def add_point(self, point:Point):
+        # bisect.insort(self._points_by_x, (x, y))
+        # bisect.insort(self._points_by_y, (y, x))
+
+        self.points.append(point)
 
     def add_points(self, points:list[Point]):
         for p in points:
-            self.add_point(p.x, p.y)
+            self.add_point(p)
 
     def query_box(self, x1, x2, y1, y2):
         # Suche nach x-Grenzen
@@ -40,17 +46,19 @@ class PointSorter:
     
     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
+        # ret = list()
+        # for (x,y) in self._points_by_x:
+        #     ret.append(Point(x,y))
+        # return ret
+        return sorted(self.points, key = lambda p: p.x)
     
     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
+        # # Sortiere nach y
+        # ret = list()
+        # for (y,x) in self._points_by_y:
+        #     ret.append(Point(x,y))
+        # return ret
+        return sorted(self.points, key = lambda p: p.y)
     
 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"
@@ -290,6 +298,8 @@ class Anlage():
         self._tunnel_lengths = dict()
         #Container für alle Wege
         self._sensor2dist = dict()
+        # Container für Sensor Artikelnummern
+        self._sensor_artnrs = dict()
         # Toleranzen zur Rack anbindung aneinander (Rack Snap)
         self._tol_snap = tol_snap 
         # Toleranzen zur Anbindung von Sensoren / Verteilern zu Racks
@@ -543,7 +553,16 @@ class Anlage():
                 nid_start = self._nodeids.get_id(start)
                 nid_end = self._nodeids.get_id(end)
 
-                weight=start.distance(end)
+                dx = start.x - end.x
+                dy = start.y - end.y  
+                
+                if shapely.has_z(start) and shapely.has_z(end):
+                    dz = (getattr(start, "z", 0) - getattr(end, "z", 0))
+                else:
+                    dz = 0.0
+
+                weight = math.sqrt(dx**2 + dy**2 + dz**2)
+                #weight=start.distance(end)
 
                 if re.match("v-.*", rname): 
                     color = "red"
@@ -602,7 +621,8 @@ class Anlage():
             ld = dict()
                         
             ld['id'] = f"{dname}-{sname}"
-            ld['s_artinr'] = self._sensor_artnrs.get(sname, "")
+            if len(self._sensor_artnrs) > 0:
+                ld['s_artinr'] = self._sensor_artnrs.get(sname, "")
             ld["coords"]= [{"x":round(p.x,1), "y":round(p.y,1)} for p in pfad_coords]
             ld["length"]= round(pfad_length,1)
             #ld["nodes"]=pfad_nodes
@@ -696,12 +716,28 @@ class TestPlant(unittest.TestCase):
                         'Rack_2-0': [Point(1, 8), Point(1, 0)], 
                         'Rack_2-1': [Point(0, 10), Point(5, 10)]}
         
+        # 10 |R2-1: ####################
+        #  9 |
+        #  8 |     |
+        #  7 |     |
+        #  6 |     |     #### R1-0
+        #  5 |     |    #
+        #  4 |     |   #
+        #  3 |     |  #
+        #  2 |     | #
+        #  1 |     |#
+        #  0 |-----#--------------------
+        #    0     1    2    3    4    5
+
 
         point2rack = RackIDs()
         point2rack.add_racks(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"])
+        t1 = set(point2rack.get_racks_from_point(Point(1, 0)))
+        self.assertEqual(t1, set(["Rack_1-0", "Rack_2-0"]))
+
+        t2 = point2rack.get_racks_from_point(Point(5, 6))
+        self.assertEqual(t2, set(["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):
diff --git a/lib/routing.py b/lib/routing.py
index 25ec199..0316bb1 100644
--- a/lib/routing.py
+++ b/lib/routing.py
@@ -5,11 +5,13 @@ import heapq
 import math
 import matplotlib.pyplot as plt
 import networkx as nx
+import shapely
 from shapely.geometry import LineString, Point
 from shapely.ops import nearest_points
 from plant import Anlage
 import configparser
 
+
 # Funktionen
 def load_json(jsonfilename):
     with open(jsonfilename, encoding='utf-8') as fh:
@@ -127,7 +129,11 @@ def prepare_data(rawdata:dict):
     for rname,lp in racks.items():
         ltemp = list()
         for p in lp:
-            ltemp.append(Point(p))
+            if len(p) == 3:
+                pt = Point(p[0], p[1], p[2])
+            else:
+                pt = Point(p[0], p[1], 0.0)
+            ltemp.append(pt)
         dracks[rname] = ltemp
         
     mapping = rawdata["mappings"]