diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py
index a79a8ea..9fe608b 100644
--- a/Search-based Planning/Search_3D/Astar3D.py	
+++ b/Search-based Planning/Search_3D/Astar3D.py	
@@ -12,7 +12,7 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
-from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
     cost
 from Search_3D.plot_util3D import visualization
 import queue
@@ -27,7 +27,7 @@ class Weighted_A_star(object):
                                   [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]])
 
         self.env = env(resolution=resolution)
-        self.Space = StateSpace(self)  # key is the point, store g value
+        self.Space = g_Space(self)  # key is the point, store g value
         self.start, self.goal = getNearest(self.Space, self.env.start), getNearest(self.Space, self.env.goal)
         # self.AABB = getAABB(self.env.blocks)
         self.Space[hash3D(getNearest(self.Space, self.start))] = 0  # set g(x0) = 0
@@ -109,7 +109,7 @@ class Weighted_A_star(object):
 
     # utility used in LRTA*
     def reset(self, xj):
-        self.Space = StateSpace(self)  # key is the point, store g value
+        self.Space = g_Space(self)  # key is the point, store g value
         self.start = xj
         self.Space[hash3D(getNearest(self.Space, self.start))] = 0  # set g(x0) = 0
         self.x0 = hash3D(xj)
diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py
index 3edf606..ebb0cfd 100644
--- a/Search-based Planning/Search_3D/Dstar3D.py	
+++ b/Search-based Planning/Search_3D/Dstar3D.py	
@@ -8,24 +8,9 @@ from collections import defaultdict
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D import Astar3D
-from Search_3D.utils3D import getDist, getRay, isinbound, isinball
+from Search_3D.utils3D import StateSpace, getDist, getRay, isinbound, isinball
 import pyrr
 
-def StateSpace(env, factor = 0):
-    boundary = env.boundary
-    resolution = env.resolution
-    xmin,xmax = boundary[0]+factor*resolution,boundary[3]-factor*resolution
-    ymin,ymax = boundary[1]+factor*resolution,boundary[4]-factor*resolution
-    zmin,zmax = boundary[2]+factor*resolution,boundary[5]-factor*resolution
-    xarr = np.arange(xmin,xmax,resolution).astype(float)
-    yarr = np.arange(ymin,ymax,resolution).astype(float)
-    zarr = np.arange(zmin,zmax,resolution).astype(float)
-    g = set()
-    for x in xarr:
-        for y in yarr:
-            for z in zarr:
-                g.add((x,y,z))
-    return g
 
 def getNearest(Space,pt):
     '''get the nearest point on the grid'''
diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py
index e465ed0..25ae599 100644
--- a/Search-based Planning/Search_3D/LP_Astar3D.py	
+++ b/Search-based Planning/Search_3D/LP_Astar3D.py	
@@ -7,7 +7,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D import Astar3D
-from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isinbound, isinball, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, hash3D, dehash, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
@@ -23,10 +23,10 @@ class Lifelong_Astar(object):
                                   [1, -1, 0], [-1, 1, 0], [1, 0, -1], [-1, 0, 1], [0, 1, -1], [0, -1, 1],
                                   [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]])
         self.env = env(resolution=resolution)
-        self.g = StateSpace(self)
+        self.g = g_Space(self)
         self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
         self.x0, self.xt = hash3D(self.start), hash3D(self.goal)
-        self.v = StateSpace(self) # rhs(.) = g(.) = inf
+        self.v = g_Space(self) # rhs(.) = g(.) = inf
         self.v[hash3D(self.start)] = 0 # rhs(x0) = 0
         self.h = Heuristic(self.g, self.goal)
         
diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py
index d2dd224..6dd9632 100644
--- a/Search-based Planning/Search_3D/LRT_Astar3D.py	
+++ b/Search-based Planning/Search_3D/LRT_Astar3D.py	
@@ -13,7 +13,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D import Astar3D
-from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search-based Planning/Search_3D/RTA_Astar3D.py
index aaf41aa..ca6d299 100644
--- a/Search-based Planning/Search_3D/RTA_Astar3D.py	
+++ b/Search-based Planning/Search_3D/RTA_Astar3D.py	
@@ -13,7 +13,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D import Astar3D
-from Search_3D.utils3D import getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
index 26a51e0..e71e672 100644
Binary files a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
index 95abe6a..9b09abd 100644
Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_3D/bidirectional_Astar3D.py b/Search-based Planning/Search_3D/bidirectional_Astar3D.py
index cd05ff7..26a5522 100644
--- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py	
+++ b/Search-based Planning/Search_3D/bidirectional_Astar3D.py	
@@ -13,7 +13,7 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
-from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, cost
 from Search_3D.plot_util3D import visualization
 import queue
 
@@ -25,9 +25,8 @@ class Weighted_A_star(object):
                       [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
                       [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
         self.env = env(resolution = resolution)
-        self.Space = StateSpace(self) # key is the point, store g value
+        self.Space = g_Space(self) # key is the point, store g value
         self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
-        self.AABB = getAABB(self.env.blocks)
         self.Space[hash3D(self.start)] = 0 # set g(x0) = 0
         self.Space[hash3D(self.goal)] = 0 # set g(x0) = 0
         self.OPEN1 = queue.QueuePrior() # store [point,priority]
@@ -60,7 +59,7 @@ class Weighted_A_star(object):
             self.CLOSED2.add(strxi2)
             self.V.append(xi1)
             self.V.append(xi2)
-            # visualization(self)
+            visualization(self)
             allchild1,  allchild2 = self.children(xi1), self.children(xi2)
             self.evaluation(allchild1,strxi1,xi1,conf=1)
             self.evaluation(allchild2,strxi2,xi2,conf=2)
diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py
index 1e2330b..715ba0f 100644
--- a/Search-based Planning/Search_3D/utils3D.py	
+++ b/Search-based Planning/Search_3D/utils3D.py	
@@ -44,24 +44,31 @@ def isinball(i, x):
         return True
     return False
 
-def StateSpace(initparams,factor=0):
-    '''This function is used to get nodes and discretize the space.
-       State space is by x*y*z,3 where each 3 is a point in 3D.'''
-    boundary = initparams.env.boundary
-    resolution = initparams.env.resolution
+def StateSpace(env, factor = 0):
+    boundary = env.boundary
+    resolution = env.resolution
     xmin,xmax = boundary[0]+factor*resolution,boundary[3]-factor*resolution
     ymin,ymax = boundary[1]+factor*resolution,boundary[4]-factor*resolution
     zmin,zmax = boundary[2]+factor*resolution,boundary[5]-factor*resolution
     xarr = np.arange(xmin,xmax,resolution).astype(float)
     yarr = np.arange(ymin,ymax,resolution).astype(float)
     zarr = np.arange(zmin,zmax,resolution).astype(float)
-    V = np.meshgrid(xarr,yarr,zarr)
-    VV = np.reshape(V,[3,len(xarr)*len(yarr)*len(zarr)]) # all points in 3D
-    Space = {}
-    for v in VV.T:
-        Space[hash3D(v)] = np.inf # this hashmap initialize all g values at inf
+    Space = set()
+    for x in xarr:
+        for y in yarr:
+            for z in zarr:
+                Space.add((x,y,z))
     return Space
 
+def g_Space(initparams):
+    '''This function is used to get nodes and discretize the space.
+       State space is by x*y*z,3 where each 3 is a point in 3D.'''
+    g = {}
+    Space = StateSpace(initparams.env)
+    for v in Space:
+        g[hash3D(v)] = np.inf # this hashmap initialize all g values at inf
+    return g
+
 def isCollide(initparams, x, direc):
     '''see if line intersects obstacle'''
     resolution = initparams.env.resolution