diff --git a/camlib.py b/camlib.py
index 92b56aab..1531dc81 100644
--- a/camlib.py
+++ b/camlib.py
@@ -77,10 +77,6 @@ class Geometry(object):
         # Flattened geometry (list of paths only)
         self.flat_geometry = []
 
-        # Flat geometry rtree index
-        # When geometry gets flattened it is indexed here.
-        self.flat_geometry_rtree = rtindex.Index()
-
     def add_circle(self, origin, radius):
         """
         Adds a circle to the object.
@@ -362,7 +358,14 @@ class Geometry(object):
                 inner_edges.append(y)
         geoms += outer_edges + inner_edges
 
-        return geoms
+        # Optimization
+        # TODO: Re-architecture?
+        g = Geometry()
+        g.solid_geometry = geoms
+        g.path_connect()
+        return g.flat_geometry
+
+        #return geoms
 
     def scale(self, factor):
         """
@@ -391,9 +394,66 @@ class Geometry(object):
         :return:
         """
 
-        self.flatten(pathonly=True)
+        flat_geometry = self.flatten(pathonly=True)
 
+        ## Index first and last points in paths
+        def get_pts(o):
+            return [o.coords[0], o.coords[-1]]
 
+        storage = FlatCAMRTreeStorage()
+        storage.get_points = get_pts
+
+        for shape in flat_geometry:
+            if shape is not None:  # TODO: This shouldn't have happened.
+                storage.insert(shape)
+
+        optimized_geometry = []
+        path_count = 0
+        current_pt = (0, 0)
+        pt, geo = storage.nearest(current_pt)
+        try:
+            while True:
+                path_count += 1
+                try:
+                    storage.remove(geo)
+                except Exception, e:
+                    log.debug('path_connect(), geo not in storage:')
+                    log.debug(str(e))
+
+                left = storage.nearest(geo.coords[0])
+
+                if left.coords[0] == geo.coords[0]:
+                    storage.remove(left)
+                    geo = geo.union(left)
+                    continue
+
+                if left.coords[-1] == geo.coords[0]:
+                    storage.remove(left)
+                    geo.union(left)
+                    continue
+
+                right = storage.nearest(geo.coords[-1])
+
+                if right.coords[0] == geo.coords[-1]:
+                    storage.remove(right)
+                    geo = geo.union(right)
+                    continue
+
+                if right.coords[-1] == geo.coords[-1]:
+                    storage.remove(right)
+                    geo.union(right)
+                    continue
+
+                # No matches on either end
+                optimized_geometry.append[geo]
+
+                # Next
+                geo = storage.nearest(geo.coords[0])
+
+        except StopIteration:  # Nothing found in storage.
+            pass
+
+        self.flat_geometry = optimized_geometry
 
     def convert_units(self, units):
         """