diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc
index bfd65b2..0c955c0 100644
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/queue.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/queue.cpython-37.pyc
new file mode 100644
index 0000000..e4fd9fc
Binary files /dev/null and b/Sampling-based Planning/rrt_2D/__pycache__/queue.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc
index af6845f..5181573 100644
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py b/Sampling-based Planning/rrt_2D/optimal_bi_rrt.py
new file mode 100644
index 0000000..e69de29
diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling-based Planning/rrt_2D/plotting.py
index 37a3178..f4e4eb5 100644
--- a/Sampling-based Planning/rrt_2D/plotting.py	
+++ b/Sampling-based Planning/rrt_2D/plotting.py	
@@ -80,7 +80,8 @@ class Plotting:
                 if node.parent:
                     plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
                     plt.gcf().canvas.mpl_connect('key_release_event',
-                                                 lambda event: [exit(0) if event.key == 'escape' else None])
+                                                 lambda event:
+                                                 [exit(0) if event.key == 'escape' else None])
                     if count % 10 == 0:
                         plt.pause(0.001)
         else:
@@ -110,6 +111,7 @@ class Plotting:
 
     @staticmethod
     def plot_path(path):
-        plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
-        plt.pause(0.01)
+        if len(path) != 0:
+            plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2)
+            plt.pause(0.01)
         plt.show()
diff --git a/Sampling-based Planning/rrt_2D/queue.py b/Sampling-based Planning/rrt_2D/queue.py
new file mode 100644
index 0000000..8f481ae
--- /dev/null
+++ b/Sampling-based Planning/rrt_2D/queue.py	
@@ -0,0 +1,62 @@
+import collections
+import heapq
+
+
+class QueueFIFO:
+    """
+    Class: QueueFIFO
+    Description: QueueFIFO is designed for First-in-First-out rule.
+    """
+
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)  # enter from back
+
+    def get(self):
+        return self.queue.popleft()  # leave from front
+
+
+class QueueLIFO:
+    """
+    Class: QueueLIFO
+    Description: QueueLIFO is designed for Last-in-First-out rule.
+    """
+
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)  # enter from back
+
+    def get(self):
+        return self.queue.pop()  # leave from back
+
+
+class QueuePrior:
+    """
+    Class: QueuePrior
+    Description: QueuePrior reorders elements using value [priority]
+    """
+
+    def __init__(self):
+        self.queue = []
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, item, priority):
+        heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+
+    def get(self):
+        return heapq.heappop(self.queue)[1]  # pop out the smallest item
+
+    def enumerate(self):
+        return self.queue
diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling-based Planning/rrt_2D/rrt.py
index 80f42a9..d7a0269 100644
--- a/Sampling-based Planning/rrt_2D/rrt.py	
+++ b/Sampling-based Planning/rrt_2D/rrt.py	
@@ -54,6 +54,7 @@ class Rrt:
 
                 if dist <= self.step_len:
                     self.new_state(node_new, self.s_goal)
+                    print(i)
                     return self.extract_path(node_new)
 
         return None
diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py
index ad066e3..90735f4 100644
--- a/Sampling-based Planning/rrt_2D/rrt_star.py	
+++ b/Sampling-based Planning/rrt_2D/rrt_star.py	
@@ -7,6 +7,8 @@ import os
 import sys
 import math
 import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Sampling-based Planning/")
@@ -14,30 +16,33 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
 from rrt_2D import env
 from rrt_2D import plotting
 from rrt_2D import utils
+from rrt_2D import queue
 
 
 class Node:
     def __init__(self, n):
         self.x = n[0]
         self.y = n[1]
-        self.cost = 0.0
         self.parent = None
 
 
 class RrtStar:
     def __init__(self, x_start, x_goal, step_len,
                  goal_sample_rate, search_radius, iter_max):
-        self.xI = Node(x_start)
-        self.xG = Node(x_goal)
+        self.s_start = Node(x_start)
+        self.s_goal = Node(x_goal)
         self.step_len = step_len
         self.goal_sample_rate = goal_sample_rate
         self.search_radius = search_radius
         self.iter_max = iter_max
-        self.vertex = [self.xI]
+        self.vertex = [self.s_start]
+        self.path = []
+        self.visited = []
 
         self.env = env.Env()
         self.plotting = plotting.Plotting(x_start, x_goal)
         self.utils = utils.Utils()
+        # self.fig, self.ax = plt.subplots()
 
         self.x_range = self.env.x_range
         self.y_range = self.env.y_range
@@ -47,23 +52,69 @@ class RrtStar:
 
     def planning(self):
         for k in range(self.iter_max):
-            if k % 500 == 0:
-                print(k)
-
             node_rand = self.generate_random_node(self.goal_sample_rate)
             node_near = self.nearest_neighbor(self.vertex, node_rand)
             node_new = self.new_state(node_near, node_rand)
 
+            if k % 500 == 0:
+                print(k)
+
             if node_new and not self.utils.is_collision(node_near, node_new):
-                self.vertex.append(node_new)
                 neighbor_index = self.find_near_neighbor(node_new)
+                self.vertex.append(node_new)
+
+                # if k % 20 == 0:
+                #     self.visited.append([[[node.x, node.parent.x], [node.y, node.parent.y]]
+                #                          for node in self.vertex[1: len(self.vertex)]])
+
                 if neighbor_index:
-                    node_new = self.choose_parent(node_new, neighbor_index)
-                    self.vertex.append(node_new)
+                    self.choose_parent(node_new, neighbor_index)
                     self.rewire(node_new, neighbor_index)
 
         index = self.search_goal_parent()
-        return self.extract_path(self.vertex[index])
+        self.path = self.extract_path(self.vertex[index])
+
+        self.plotting.animation(self.vertex, self.path, "rrt*")
+
+    def new_state(self, node_start, node_goal):
+        dist, theta = self.get_distance_and_angle(node_start, node_goal)
+
+        dist = min(self.step_len, dist)
+        node_new = Node((node_start.x + dist * math.cos(theta),
+                         node_start.y + dist * math.sin(theta)))
+
+        node_new.parent = node_start
+
+        return node_new
+
+    def choose_parent(self, node_new, neighbor_index):
+        cost = [self.get_new_cost(self.vertex[i], node_new) for i in neighbor_index]
+
+        cost_min_index = neighbor_index[int(np.argmin(cost))]
+        node_new.parent = self.vertex[cost_min_index]
+
+    def rewire(self, node_new, neighbor_index):
+        for i in neighbor_index:
+            node_neighbor = self.vertex[i]
+
+            if self.cost(node_neighbor) > self.get_new_cost(node_new, node_neighbor):
+                node_neighbor.parent = node_new
+
+    def search_goal_parent(self):
+        dist_list = [math.hypot(n.x - self.s_goal.x, n.y - self.s_goal.y) for n in self.vertex]
+        node_index = [dist_list.index(i) for i in dist_list if i <= self.step_len]
+
+        if node_index:
+            cost_list = [dist_list[i] + self.cost(self.vertex[i]) for i in node_index
+                         if not self.utils.is_collision(self.vertex[i], self.s_goal)]
+            return node_index[int(np.argmin(cost_list))]
+
+        return len(self.vertex) - 1
+
+    def get_new_cost(self, node_start, node_end):
+        dist, _ = self.get_distance_and_angle(node_start, node_end)
+
+        return self.cost(node_start) + dist
 
     def generate_random_node(self, goal_sample_rate):
         delta = self.utils.delta
@@ -72,79 +123,108 @@ class RrtStar:
             return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
                          np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
 
-        return self.xG
+        return self.s_goal
 
-    def nearest_neighbor(self, node_list, n):
-        return self.vertex[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
-                                          for nd in node_list]))]
-
-    def new_state(self, node_start, node_goal):
-        dist, theta = self.get_distance_and_angle(node_start, node_goal)
-
-        dist = min(self.step_len, dist)
-        node_new = Node((node_start.x + dist * math.cos(theta),
-                         node_start.y + dist * math.sin(theta)))
-        node_new.parent = node_start
-
-        return node_new
+    @staticmethod
+    def nearest_neighbor(node_list, n):
+        return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
+                                        for nd in node_list]))]
 
     def find_near_neighbor(self, node_new):
         n = len(self.vertex) + 1
         r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len)
 
         dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.vertex]
-        dist_table_index = [dist_table.index(d) for d in dist_table if d <= r]
-        dist_table_index = [ind for ind in dist_table_index
-                            if not self.utils.is_collision(node_new, self.vertex[ind])]
+        dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and
+                            not self.utils.is_collision(node_new, self.vertex[ind])]
 
         return dist_table_index
 
-    def choose_parent(self, node_new, neighbor_index):
-        cost = []
+    @staticmethod
+    def cost(node_p):
+        node = node_p
+        cost = 0.0
 
-        for i in neighbor_index:
-            node_neighbor = self.vertex[i]
-            cost.append(self.get_new_cost(node_neighbor, node_new))
+        while node.parent:
+            cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y)
+            node = node.parent
 
-        cost_min_index = neighbor_index[int(np.argmin(cost))]
-        node_new = self.new_state(self.vertex[cost_min_index], node_new)
-        node_new.cost = min(cost)
+        return cost
 
-        return node_new
+    def animation(self, name):
+        self.plot_grid(name)
+        plt.pause(4)
+        for edge_set in self.visited:
+            plt.cla()
 
-    def search_goal_parent(self):
-        dist_list = [math.hypot(n.x - self.xG.x, n.y - self.xG.y) for n in self.vertex]
-        node_index = [dist_list.index(i) for i in dist_list if i <= self.step_len]
+            self.plot_grid(name)
+            for edges in edge_set:
+                plt.plot(edges[0], edges[1], "-g")
 
-        if node_index:
-            cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index
-                         if not self.utils.is_collision(self.vertex[i], self.xG)]
-            return node_index[int(np.argmin(cost_list))]
+            plt.pause(0.0001)
 
-        return None
+        if self.path:
+            plt.plot([x[0] for x in self.path], [x[1] for x in self.path], '-r', linewidth=2)
 
-    def rewire(self, node_new, neighbor_index):
-        for i in neighbor_index:
-            node_neighbor = self.vertex[i]
-            new_cost = self.get_new_cost(node_new, node_neighbor)
+        plt.pause(0.5)
+        plt.show()
 
-            if node_neighbor.cost > new_cost:
-                self.vertex[i] = self.new_state(node_new, node_neighbor)
-                self.propagate_cost_to_leaves(node_new)
+    def plot_grid(self, name):
 
-    def get_new_cost(self, node_start, node_end):
-        dist, _ = self.get_distance_and_angle(node_start, node_end)
-        return node_start.cost + dist
+        for (ox, oy, w, h) in self.obs_boundary:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='black',
+                    fill=True
+                )
+            )
 
-    def propagate_cost_to_leaves(self, parent_node):
-        for node in self.vertex:
-            if node.parent == parent_node:
-                node.cost = self.get_new_cost(parent_node, node)
-                self.propagate_cost_to_leaves(node)
+        for (ox, oy, w, h) in self.obs_rectangle:
+            self.ax.add_patch(
+                patches.Rectangle(
+                    (ox, oy), w, h,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        for (ox, oy, r) in self.obs_circle:
+            self.ax.add_patch(
+                patches.Circle(
+                    (ox, oy), r,
+                    edgecolor='black',
+                    facecolor='gray',
+                    fill=True
+                )
+            )
+
+        plt.plot(self.s_start.x, self.s_start.y, "bs", linewidth=3)
+        plt.plot(self.s_goal.x, self.s_goal.y, "gs", linewidth=3)
+
+        plt.title(name)
+        plt.axis("equal")
+
+    def update_cost(self, parent_node):
+        OPEN = queue.QueueFIFO()
+        OPEN.put(parent_node)
+
+        while not OPEN.empty():
+            node = OPEN.get()
+
+            if len(node.child) == 0:
+                continue
+
+            for node_c in node.child:
+                node_c.cost = self.get_new_cost(node, node_c)
+                OPEN.put(node_c)
 
     def extract_path(self, node_end):
-        path = [[self.xG.x, self.xG.y]]
+        path = [[self.s_goal.x, self.s_goal.y]]
         node = node_end
+
         while node.parent is not None:
             path.append([node.x, node.y])
             node = node.parent
@@ -163,13 +243,8 @@ def main():
     x_start = (2, 2)  # Starting node
     x_goal = (49, 24)  # Goal node
 
-    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 10000)
-    path = rrt_star.planning()
-
-    if path:
-        rrt_star.plotting.animation(rrt_star.vertex, path, "RRT*")
-    else:
-        print("No Path Found!")
+    rrt_star = RrtStar(x_start, x_goal, 10, 0.10, 20, 15000)
+    rrt_star.planning()
 
 
 if __name__ == '__main__':
diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py
index 318f38a..f712bf0 100644
--- a/Sampling-based Planning/rrt_3D/rrtstar3D.py	
+++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py	
@@ -13,7 +13,8 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/")
 from rrt_3D.env3D import env
-from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, hash3D, dehash
+from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset, \
+    hash3D, dehash
 
 
 class rrtstar():
@@ -23,44 +24,44 @@ class rrtstar():
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 10000 # at least 4000 in this env
+        self.maxiter = 10000  # at least 4000 in this env
         self.stepsize = 0.5
         self.gamma = 500
-        self.eta = 2*self.stepsize
+        self.eta = 2 * self.stepsize
         self.Path = []
         self.done = False
 
-    def wireup(self,x,y):
-        self.E.add_edge([x,y]) # add edge
+    def wireup(self, x, y):
+        self.E.add_edge([x, y])  # add edge
         self.Parent[hash3D(x)] = y
 
-    def removewire(self,xnear):
+    def removewire(self, xnear):
         xparent = self.Parent[hash3D(xnear)]
-        a = [xnear,xparent]
-        self.E.remove_edge(a) # remove and replace old the connection
+        a = [xnear, xparent]
+        self.E.remove_edge(a)  # remove and replace old the connection
 
     def reached(self):
         self.done = True
-        xn = near(self,self.env.goal)
-        c = [cost(self,x) for x in xn]
+        xn = near(self, self.env.goal)
+        c = [cost(self, x) for x in xn]
         xncmin = xn[np.argmin(c)]
-        self.wireup(self.env.goal,xncmin)
+        self.wireup(self.env.goal, xncmin)
         self.V.append(self.env.goal)
-        self.Path,self.D = path(self)
+        self.Path, self.D = path(self)
 
     def run(self):
         self.V.append(self.env.start)
         self.ind = 0
         xnew = self.env.start
         print('start rrt*... ')
-        self.fig = plt.figure(figsize = (10,8))
+        self.fig = plt.figure(figsize=(10, 8))
         while self.ind < self.maxiter:
-            xrand    = sampleFree(self)
-            xnearest = nearest(self,xrand)
-            xnew     = steer(self,xnearest,xrand)
-            if not isCollide(self,xnearest,xnew):
-                Xnear = near(self,xnew)
-                self.V.append(xnew) # add point
+            xrand = sampleFree(self)
+            xnearest = nearest(self, xrand)
+            xnew = steer(self, xnearest, xrand)
+            if not isCollide(self, xnearest, xnew):
+                Xnear = near(self, xnew)
+                self.V.append(xnew)  # add point
                 visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
@@ -80,14 +81,13 @@ class rrtstar():
             self.ind += 1
         # max sample reached
         self.reached()
-        print('time used = ' + str(time.time()-starttime))
-        print('Total distance = '+str(self.D))
+        print('time used = ' + str(time.time() - starttime))
+        print('Total distance = ' + str(self.D))
         visualization(self)
         plt.show()
-        
+
 
 if __name__ == '__main__':
     p = rrtstar()
     starttime = time.time()
     p.run()
-