diff --git a/compiler/router/navigation_graph.py b/compiler/router/navigation_graph.py
index 9985f1dd..0a1e6462 100644
--- a/compiler/router/navigation_graph.py
+++ b/compiler/router/navigation_graph.py
@@ -4,7 +4,10 @@
 # All rights reserved.
 #
 import heapq
+from copy import deepcopy
 from openram import debug
+from openram.base.pin_layout import pin_layout
+from openram.base.vector import vector
 from openram.base.vector3d import vector3d
 from .direction import direction
 from .navigation_node import navigation_node
@@ -19,32 +22,57 @@ class navigation_graph:
         self.router = router
 
 
+    def is_on_same_layer(self, point, shape):
+        """ Return if the point is on the same layer as the shape. """
+
+        return point.z == self.router.get_zindex(shape.lpp)
+
+
     def create_graph(self, layout_source, layout_target):
-        """  """
+        """ Create the Hanan graph to run routing on later. """
         debug.info(0, "Creating the navigation graph for source '{0}' and target'{1}'.".format(layout_source, layout_target))
 
         # Find the region to be routed and only include objects inside that region
-        s_ll, s_ur = layout_source.rect
-        t_ll, t_ur = layout_target.rect
-        region = (s_ll.min(t_ll), s_ur.min(t_ur))
-        debug.info(0, "Routing region is ll: '{0}' ur: '{1}'".format(region[0], region[1]))
+        region = deepcopy(layout_source)
+        region.bbox([layout_source, layout_target])
+        debug.info(0, "Routing region is {}".format(region.rect))
 
         # Find the blockages that are in the routing area
         self.graph_blockages = []
         for blockage in self.router.blockages:
-            ll, ur = blockage.rect
-            if is_in_region(ll, region) or is_in_region(ur, region):
+            if region.overlaps(blockage):
                 self.graph_blockages.append(blockage)
         debug.info(0, "Number of blockages detected in the routing region: {}".format(len(self.graph_blockages)))
 
-        # Obtain the x and y points for Hanan grid
+        # Obtain the x and y values for Hanan grid
         x_values = []
         y_values = []
         offset = max(self.router.horiz_track_width, self.router.vert_track_width) / 2
+        # Add the source and target pins first
         for shape in [layout_source, layout_target]:
-            center = shape.center()
-            x_values.append(center.x)
-            y_values.append(center.y)
+            aspect_ratio = shape.width() / shape.height()
+            # If the pin is tall or fat, add two points on the ends
+            if aspect_ratio <= 0.5: # Tall pin
+                uc = shape.uc()
+                bc = shape.bc()
+                points = [vector(uc.x, uc.y - offset),
+                          vector(bc.x, bc.y + offset)]
+                for p in points:
+                    x_values.append(p.x)
+                    y_values.append(p.y)
+            elif aspect_ratio >= 2: # Fat pin
+                lc = shape.lc()
+                rc = shape.rc()
+                points = [vector(lc.x + offset, lc.y),
+                          vector(rc.x - offset, rc.y)]
+                for p in points:
+                    x_values.append(p.x)
+                    y_values.append(p.y)
+            else: # Square-like pin
+                center = shape.center()
+                x_values.append(center.x)
+                y_values.append(center.y)
+        # Add corners for blockages
         for blockage in self.graph_blockages:
             ll, ur = blockage.rect
             x_values.extend([ll.x - offset, ur.x + offset])
@@ -61,7 +89,7 @@ class navigation_graph:
         for point in hanan_points.copy():
             for blockage in self.graph_blockages:
                 ll, ur = blockage.rect
-                if self.router.get_zindex(blockage.lpp) == point.z and is_in_region(point, blockage.rect):
+                if self.is_on_same_layer(point, blockage) and is_in_region(point, blockage):
                     hanan_points.remove(point)
                     break
 
@@ -116,30 +144,37 @@ class navigation_graph:
         """
 
         # Find source and target nodes
-        source_center = source.center()
-        source_center = vector3d(source_center.x, source_center.y, self.router.get_zindex(source.lpp))
-        target_center = target.center()
-        target_center = vector3d(target_center.x, target_center.y, self.router.get_zindex(target.lpp))
+        sources = []
         for node in self.nodes:
-            if node.center == source_center:
-                source = node
-            if node.center == target_center:
-                target = node
+            if self.is_on_same_layer(node.center, source) and is_in_region(node.center, source):
+                sources.append(node)
+        targets = []
+        for node in self.nodes:
+            if self.is_on_same_layer(node.center, target) and is_in_region(node.center, target):
+                targets.append(node)
 
         # Heuristic function to calculate the scores
-        h = lambda node: target.center.distance(node.center) + abs(target.center.z - node.center.z)
+        def h(node):
+            """ Return the estimated distance to closest target. """
+            min_dist = float("inf")
+            for t in targets:
+                dist = t.center.distance(node.center) + abs(t.center.z - node.center.z)
+                if dist < min_dist:
+                    min_dist = dist
+            return min_dist
 
+        # Initialize data structures to be used for A* search
         queue = []
         close_set = set()
         came_from = {}
         g_scores = {}
         f_scores = {}
 
-        # Initialize score values for the source node
-        g_scores[source.id] = 0
-        f_scores[source.id] = h(source)
-
-        heapq.heappush(queue, (f_scores[source.id], source.id, source))
+        # Initialize score values for the source nodes
+        for node in sources:
+            g_scores[node.id] = 0
+            f_scores[node.id] = h(node)
+            heapq.heappush(queue, (f_scores[node.id], node.id, node))
 
         # Run the A* algorithm
         while len(queue) > 0:
@@ -152,7 +187,7 @@ class navigation_graph:
             close_set.add(current)
 
             # Check if we've reached the target
-            if current == target:
+            if current in targets:
                 path = []
                 while current.id in came_from:
                     path.append(current)
diff --git a/compiler/router/navigation_node.py b/compiler/router/navigation_node.py
index e8acdd9b..21b10757 100644
--- a/compiler/router/navigation_node.py
+++ b/compiler/router/navigation_node.py
@@ -24,6 +24,12 @@ class navigation_node:
         self.neighbors = []
 
 
+    def __lt__(self, other):
+        """  """
+
+        return self.center < other.center
+
+
     def add_neighbor(self, node):
         """ Connect two nodes. """
 
diff --git a/compiler/router/navigation_router.py b/compiler/router/navigation_router.py
index a6228d43..01147a43 100644
--- a/compiler/router/navigation_router.py
+++ b/compiler/router/navigation_router.py
@@ -52,8 +52,17 @@ class navigation_router(router_tech):
         self.find_blockages()
 
         # Create the navigation graph
-        pin_iter = iter(self.pins["vdd"])
+        # TODO: Remove this part later and route all pins
+        vdds = list(self.pins["vdd"])
+        vdds.sort()
+        pin_iter = iter(vdds)
         vdd_0 = next(pin_iter)
+        next(pin_iter)
+        next(pin_iter)
+        next(pin_iter)
+        next(pin_iter)
+        next(pin_iter)
+        next(pin_iter)
         vdd_1 = next(pin_iter)
         self.nav = navigation_graph(self)
         self.nav.create_graph(vdd_0, vdd_1)
@@ -62,7 +71,11 @@ class navigation_router(router_tech):
         path = self.nav.find_shortest_path(vdd_0, vdd_1)
 
         # Create the path shapes on layout
-        self.add_path(path)
+        if path:
+            self.add_path(path)
+            debug.info(0, "Successfully routed")
+        else:
+            debug.info(0, "No path was found!")
 
         self.write_debug_gds(gds_name="after.gds", source=vdd_0, target=vdd_1)
 
@@ -101,16 +114,17 @@ class navigation_router(router_tech):
                 rect = [ll, ur]
                 new_shape = pin_layout("blockage{}".format(len(self.blockages)),
                                        rect,
-                                       lpp).inflated_pin(multiple=1)
+                                       lpp)
                 # If there is a rectangle that is the same in the pins,
                 # it isn't a blockage
                 if new_shape not in self.all_pins and not self.pin_contains(new_shape):
+                    new_shape = new_shape.inflated_pin(multiple=1)
                     self.blockages.append(new_shape)
 
 
     def pin_contains(self, shape):
         for pin in self.all_pins:
-            if pin.contains(shape):
+            if pin.contains(shape) or shape.contains(pin):
                 return True
         return False
 
@@ -118,17 +132,10 @@ class navigation_router(router_tech):
     def add_path(self, path):
         """  """
 
-        for i in range(len(path) - 1):
-            self.connect_nodes(path[i], path[i + 1])
-
-
-    def connect_nodes(self, a, b):
-        """ Connect nodes 'a' and 'b' with a wire. """
-
-        if a.center.x == b.center.x and a.center.y == b.center.y:
-            self.design.add_via_center(self.layers, vector(a.center.x, b.center.y))
-        else:
-            self.design.add_path(self.get_layer(a.center.z), [a.center, b.center])
+        coordinates = [x.center for x in path]
+        self.design.add_route(layers=self.layers,
+                              coordinates=coordinates,
+                              layer_widths=self.layer_widths)
 
 
     def write_debug_gds(self, gds_name="debug_route.gds", source=None, target=None):
diff --git a/compiler/router/navigation_utils.py b/compiler/router/navigation_utils.py
index 9ce2cba5..30f8460a 100644
--- a/compiler/router/navigation_utils.py
+++ b/compiler/router/navigation_utils.py
@@ -31,9 +31,10 @@ def is_probe_blocked(p1, p2, blockages):
 
 
 def is_in_region(point, region):
-    """"""
+    """ Return if a point is in the given region. """
 
-    if is_between(region[0].x, region[1].x, point.x) and is_between(region[0].y, region[1].y, point.y):
+    ll, ur = region.rect
+    if is_between(ll.x, ur.x, point.x) and is_between(ll.y, ur.y, point.y):
         return True
     return False