diff --git a/main.py b/main.py
index cb71a71..eb6fe96 100644
--- a/main.py
+++ b/main.py
@@ -264,3 +264,6 @@ block_list = ["blue_concrete", "red_concrete", "green_concrete",
 
 # # polyline._alpha_assign(1, polyline.length_polyline-1)
 # print(polyline.alpha_radii)
+
+print(
+    Point2D(-2, -5).optimized_path([Point2D(0, 0), Point2D(10, 5), Point2D(1, 3)]))
diff --git a/networks/geometry/Point2D.py b/networks/geometry/Point2D.py
index a251545..7b229e6 100644
--- a/networks/geometry/Point2D.py
+++ b/networks/geometry/Point2D.py
@@ -62,6 +62,32 @@ class Point2D:
         """
         return min(points, key=lambda point: self.distance(point))
 
+    def optimized_path(self, points: List["Point2D"]):
+        """Get an optimized ordered path starting from the current point.
+
+        From: https://stackoverflow.com/questions/45829155/sort-points-in-order-to-have-a-continuous-curve-using-python
+
+        Args:
+            points (List[Point2D]): List of 2d-points. Could contain the current point.
+
+        Returns:
+            List[Point2D]: Ordered list of 2d-points starting from the current point.
+
+        >>> Point2D(-2, -5).optimized_path([Point2D(0, 0), Point2D(10, 5), Point2D(1, 3)])
+        [Point2D(x: -2, y: -5), Point2D(x: 0, y: 0), Point2D(x: 1, y: 3), Point2D(x: 10, y: 5)]
+        """
+        start = self
+        if start not in points:
+            points.append(start)
+        pass_by = points
+        path = [start]
+        pass_by.remove(start)
+        while pass_by:
+            nearest = min(pass_by, key=lambda point: point.distance(path[-1]))
+            path.append(nearest)
+            pass_by.remove(nearest)
+        return path
+
     def angle(self, xy1, xy2):
         """
         Compute angle (in degrees). Corner in current point.
diff --git a/networks/geometry/Point3D.py b/networks/geometry/Point3D.py
index ddfc80b..1429e9d 100644
--- a/networks/geometry/Point3D.py
+++ b/networks/geometry/Point3D.py
@@ -34,6 +34,32 @@ class Point3D:
         """
         return min(points, key=lambda point: self.distance(point))
 
+    def optimized_path(self, points: List["Point3D"]):
+        """Get an optimized ordered path starting from the current point.
+
+        From: https://stackoverflow.com/questions/45829155/sort-points-in-order-to-have-a-continuous-curve-using-python
+
+        Args:
+            points (List[Point2D]): List of 3d-points. Could contain the current point.
+
+        Returns:
+            List[Point2D]: Ordered list of 3d-points starting from the current point.
+
+        >>> Point3D(-2, -5, 6).optimized_path([Point3D(0, 0, 7), Point3D(10, 5, 1), Point3D(1, 3, 3)])
+        [Point3D(x: -2, y: -5, z: 6), Point3D(x: 0, y: 0, z: 7), Point3D(x: 1, y: 3, z: 3), Point3D(x: 10, y: 5, z: 1)]
+        """
+        start = self
+        if start not in points:
+            points.append(start)
+        pass_by = points
+        path = [start]
+        pass_by.remove(start)
+        while pass_by:
+            nearest = min(pass_by, key=lambda point: point.distance(path[-1]))
+            path.append(nearest)
+            pass_by.remove(nearest)
+        return path
+
     def round(self, ndigits: int = None) -> "Point3D":
         self.x = round(self.x, ndigits)
         self.y = round(self.y, ndigits)
diff --git a/networks/geometry/point_tools.py b/networks/geometry/point_tools.py
index bd9bca0..eb4b46b 100644
--- a/networks/geometry/point_tools.py
+++ b/networks/geometry/point_tools.py
@@ -3,20 +3,6 @@ import numpy as np
 from networks.geometry.segment_tools import discrete_segment, middle_point, parallel
 
 
-def optimized_path(points, start=None):
-    # https://stackoverflow.com/questions/45829155/sort-points-in-order-to-have-a-continuous-curve-using-python
-    if start is None:
-        start = points[0]
-    pass_by = points
-    path = [start]
-    pass_by.remove(start)
-    while pass_by:
-        nearest = min(pass_by, key=lambda x: distance(path[-1], x))
-        path.append(nearest)
-        pass_by.remove(nearest)
-    return path
-
-
 def sort_by_clockwise(points):
     """
     Sort point in a rotation order. Works in 2d but supports 3d.