From d8ceecf22d1b2f94635d3bd7fdf0521ac4de169a Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Sat, 27 Jun 2020 09:58:58 -0700
Subject: [PATCH] yq edited A*3D

---
 Search-based Planning/Astar_3D/Astar3D.py     |  38 -------
 .../Astar_3D/__pycache__/env3D.cpython-37.pyc | Bin 1865 -> 0 bytes
 .../__pycache__/utils3D.cpython-37.pyc        | Bin 2838 -> 0 bytes
 Search-based Planning/Astar_3D/utils3D.py     |  66 ------------
 Search-based Planning/Search_3D/Astar3D.py    |  94 ++++++++++++++++++
 .../__pycache__/Astar3D.cpython-37.pyc        | Bin
 .../__pycache__/env3D.cpython-37.pyc          | Bin 0 -> 1866 bytes
 .../__pycache__/plot_util3D.cpython-37.pyc    | Bin 0 -> 4729 bytes
 .../__pycache__/queue.cpython-37.pyc          | Bin
 .../__pycache__/utils3D.cpython-37.pyc        | Bin 0 -> 3692 bytes
 .../{Astar_3D => Search_3D}/env3D.py          |  14 +--
 .../{Astar_3D => Search_3D}/plot_util3D.py    |   6 +-
 .../{Astar_3D => Search_3D}/queue.py          |   0
 Search-based Planning/Search_3D/utils3D.py    |  90 +++++++++++++++++
 14 files changed, 194 insertions(+), 114 deletions(-)
 delete mode 100644 Search-based Planning/Astar_3D/Astar3D.py
 delete mode 100644 Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc
 delete mode 100644 Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc
 delete mode 100644 Search-based Planning/Astar_3D/utils3D.py
 create mode 100644 Search-based Planning/Search_3D/Astar3D.py
 rename Search-based Planning/{Astar_3D => Search_3D}/__pycache__/Astar3D.cpython-37.pyc (100%)
 create mode 100644 Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
 create mode 100644 Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc
 rename Search-based Planning/{Astar_3D => Search_3D}/__pycache__/queue.cpython-37.pyc (100%)
 create mode 100644 Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
 rename Search-based Planning/{Astar_3D => Search_3D}/env3D.py (81%)
 rename Search-based Planning/{Astar_3D => Search_3D}/plot_util3D.py (95%)
 rename Search-based Planning/{Astar_3D => Search_3D}/queue.py (100%)
 create mode 100644 Search-based Planning/Search_3D/utils3D.py

diff --git a/Search-based Planning/Astar_3D/Astar3D.py b/Search-based Planning/Astar_3D/Astar3D.py
deleted file mode 100644
index ef72dd7..0000000
--- a/Search-based Planning/Astar_3D/Astar3D.py	
+++ /dev/null
@@ -1,38 +0,0 @@
-# this is the three dimensional A* algo
-# !/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: yue qi
-"""
-import numpy as np
-
-import os
-import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
-from Astar_3D.env3D import env
-from Astar_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest
-import queue
-
-
-class Weighted_A_star(object):
-    def __init__(self):
-        self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
-                      [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
-                      [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()
-        self.Space = StateSpace(self.env.boundary) # key is the point, store g value
-        self.OPEN = queue.QueuePrior() # store [point,priority]
-        self.start = getNearest(self.Space,self.env.start)
-        self.goal = getNearest(self.Space,self.env.goal)
-        self.h = Heuristic(self.Space,self.goal)
-        self.Parent = {}
-        self.CLOSED = {}
-        
-
-    def run(self):
-        pass
-if __name__ == '__main__':
-    Astar = Weighted_A_star()
-    
\ No newline at end of file
diff --git a/Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc
deleted file mode 100644
index a3ab9d0f7f20e55685a0233fdb12120ba7874b72..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 1865
zcma)6&2Jk;6rb5|FOCDDlvGJkD5YW#iBWSyQCO9zT#!Ku5|X8Mwf1b*+pKptv+H78
z>r*1Z5iUq@N{;+5aN{&5BqSvN0~~tdz1ffUKqa2_-p<S$zxU?%elw3YHf#d&!yo@V
z{mCHYFPt=@!oUU1AA@0JNEl^GOos|nnRZ2nD$|((U+r_kOlDmXW?d>nJuq13T;Y!b
z{gMpLzzWP~ol8myQ%;FvFW<6Ve^yLV{>k2A7VKR__8P*I!I<I)L<8-BV)}du2Ge3n
zOcxrE3TIY5>%dH5Hq12ey&-4aY8GlM8j_VVKPc%5dGcVT7UY_i>J43~rE;x&N6skQ
zV7)7=R8Pnyy@4Vx9)CmRF6D+oxtoLV_uo(~H-t*0)GNdd52b82u3ODuyK&uf>&+iD
zD<3s0jvM~+GsvBXZg__B_d~aZK!HRE!`q<wEifS|$&>Dimh^kFLw-~)v{Ujc;keIH
zgmUn)pwApCY#!ukJS(C!k-isS_b2Hz==*Un@1F$ud68!QFe)aqQ9tvG$$l35NfIUD
zz8{Av2gfw;p9Vf3PxeQC9<aTy>J9zFlYWpqKRh|e7BB3hILeE0I?ax6V})3TL6&#J
zpcuvJ_&h&2f{!2P$1rGiI;v2TOz1x6{z4djmIVoOG_J#+g)aXmI$w_R!XL*$E@)Jp
zj>-)jxJg8(O@O4$x4;mkM|)b2Zc&Ze@Y|)l2dgcTh$8-1lIaT=G-L{qP6{)CQd+3g
zM7b}CvMcF9Bt4MVuhQcl(1TtpWkrw9VErb<tf`^7YfQ67IM!I<8Y$hD^1qrY4_)__
zV>FH@aB37VgoJ{dFuDucV84sd_;LIi=sA|dEi`zwYS@Qno=pPa4DX_60}ZBV(n-**
zCh{Hd<npzk+Y)WS8o4<JZ3XLcLG$;(1Y}6z-B93K!nM7k@M2i3gD(cN;c|6_3Y6jq
z(i#J;kqT=}%8~eEEEfg8sGou6Pr;OCNxZ5Lv{Xv9)JnaCr{RW{<__ExPc5|4yreMF
z3!@&H*te?tAv^)#v*d~fEKuu}4kJ*PKByb|=cjFF>%r~jiXXa;T`{u-Ram2RmN1_$
zBn5?0-35se5FDE$bf7mchFX~Vv117>58^YS&8JZ!w1u=wY3Eaajw@)FV5?NJ{VfNC
z<#|yO6`uD#0)s6i)PNg+_h5@Iy8!M%v!!JrqKzuSL~h9=(?QdCB-Kk2*7xptl+XM)
zS_TeyCFO{^+`#y)XW*E!ldl+j8~D3uw$Wfu9UZGq{Au8M!uGsr%4RW+yPkIeDJl=Y
zkNZDF^YX~;x%oTb*aOY6e2dzaT*1IoKT15$v4!b*EFD8!J(*3jMa6u47-To(e*OTa
qLn8^kg>wi!<55!Z9o+ICIfZppm3oZy%rTNiE!Enz%-*I2sQ(LewT|)t

diff --git a/Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc
deleted file mode 100644
index aae19a23a65de8f8fbbff6570b1544d1f40a0fd5..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2838
zcmai0-EP~+6`mRXh@vh3#k+CbwBVv!GzvEMcD)yi!U&R7D2k#D7IlyULbnF3nMhnq
zBt0B$C4ssqlwS6tZ(!dPXdj>t&<E%P@TM2Nsb8QMx$1X@veq>MloHRJGcz2X-|z56
zs}(X_pa1hezx{iIv44?pb9rE%;V%9PBAMhv*5y)2=Pm0B=}HfyBYoMx=*mDgF?uqT
zYZ!gGE?XEIazk!n4CI#F#@LkKkvsC<Tiy-Tn%q4X`p-D|gW>w9)opR6TJl4A|Bt-e
zQlV;ny&>5`bT<{h+>(3pf!yyqSG>Eee6^{zv9hD?sU6h-&mXV_=g{$lwI5zXU-q&m
zXu##*CR=`jyEq0Zxy1ewyC1MUl7y8&t>jn<bdU9&xiIXKm#h?haYd)QFEz&**Ot&L
z+M*I^UU{+B@wDw!d{Xf#9Fb_%`wP<nw!HiO;UF8S!+5ABhaFX%n=C)<CuT4{JIrG<
zc#;p}G)>a}lX%$AG!{n1;facMZ}8+SE|iSETy8jgUYJ;)9(4{64(R`XHjPa(ERH${
z`Ly!;%KR#xk~0bygz*EeDfm6Igf3cI!)^Wab1;22=aXk8>#(oBn2VBMGQ%$gZl@Fy
zUi$+)Ci&IP3^L<phJ1B}cwB_PhJSB)joxwXgS6eskMmrmQj<2C0#rG%l<=T`Hq3hG
zMddz!{^CW&6YIm8G-+D{rqB`+mKEe*#$6urFSy<Tzlu8rhgPjU9WWG0BkH)<Ik3m1
zxJQ{lG3P{AGn5lHGwPYXvw%h4TB6AkZR77)3o5s`(57;V@u)4jZk`oCvh=g}Y2Qf-
z)4~CT%~7sD0Kd|k%p#vyy<dQl{5N7Q=Fa5vl3lVZipQh5Yk27{_}qg<Udb0??w5Wk
z&Y9l9=m|zoT<pOA4zw1tkKnY(3{;dNuT){8JWEm&WvQL)>qNFi@^4u65)p9E)wHO5
zsRnT|u-dg7m3NZIJ=GPt>H4E2l}XX{Fp^jpotvWTfK(0^?%EvU<uB{^Zo!~p1B8iy
zZ}0$tqfzf;X5|7hpR&Zd;4?4@%$Yn|ng4L^7z!mfPKm(mq4*cP6s2>_E;eAYfXRHe
z{mXa~rI{W@P|3t;PqkhBAx^8DQ9MDmofY~%M9l8dEH@P&RQ&v|_RXKEvBn8WkCd!4
zPPkC|A=eRRRux2*w2Is(5X=%~7RaRgwS*j7@cYQSrR>%+6!vOldB-E0S2n}i92sSF
zn`?5%UB!F~&r#<iEZt<%_rd><J}Wnp?o<I(xo@G@CF=Excc9HOuUC&zQj!@bxQkto
zKDK|gKIbq8e*TlVa8GJopq}?;n8k)b@ZeLujm>wId>0-M;k#Zo%Afv#4y4n4Hrub0
z>);r*9uJGBU~aZ)GN@gbRSgVYkq;B|elOS%etdH><5pf}61@#cU1yR3bG!UqES)2U
zm_xavbn?{YEd9l8Kv?eSU=E>L?LjU|&q4&~a?b#B{;fT(Y@9F|AhgXow3^`62KSo$
zYa@J3$oJ}})@yB1d+>d+ZYD*No@L`y{tyoow1x2@fKNXLx3YH=0s)AvoPU6UPSos>
za^Nhe?XemhAY0h8E3mCZ9bv_tdswly*%er{T=C}(+YC`>NF8S$SjjCtD>bd;mkle0
zkVF}j%`(K!HAqgmCWUd!V8KC~3%<)RhBTsGi1|8dyfxV`*T41va9i^Y$;+1U%Z&v`
zRsRiD-GV0@v+~s-DWW&yv}Y2a7x*!CNRvf`aA_u05yh#bn(t|4l9{pyR}gp|gi(E+
znAoVu&d~{H(`fQ|`gr!qQS`fk(kfDMZx9{PQoIDpB#qEWZSVg!jQSBgT(^QYxOSs%
z1%9mKw6CgQq>2GSYt;bc4&q!@&QPTlf2FBT+nZHjBN}V;=QnZBWV&)Ez*M3@D0gc4
zspV&upMmeX6MSrR-KnKB(2BqAir24~F!s%z60AaNu7z(xrGn6oIs2S`8_VCvNTqeY
z#r2P|Rt2Z0qc}-VPYHlj`c2*Gcoo8qY>3Z_EbY@F=&;Y0%+?LF`wWFjP}ttBymUOu
zr*(^`j}6<QZ9}nuZ{gGW!d9(K<&^?yJW{8pm50|Kbu@jr=}(D}_7)cOcZvHRNaf`k
u-Rzd#)cCdiS_qCunH&$*Q&OpjKmrlCfgd<Q7&Jhgzzeqm7#~^(g#5p_W=xa-

diff --git a/Search-based Planning/Astar_3D/utils3D.py b/Search-based Planning/Astar_3D/utils3D.py
deleted file mode 100644
index 5db2fd9..0000000
--- a/Search-based Planning/Astar_3D/utils3D.py	
+++ /dev/null
@@ -1,66 +0,0 @@
-import numpy as np
-
-def getRay(x, y):
-    direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
-    return np.array([x, direc])
-
-def getAABB(blocks):
-    AABB = []
-    for i in blocks:
-        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
-    return AABB
-
-def getDist(pos1, pos2):
-    return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
-
-def getNearest(Space,pt):
-    '''get the nearest point on the grid'''
-    mindis,minpt = 1000,None
-    for strpts in Space.keys(): 
-        pts = dehash(strpts)
-        dis = getDist(pts,pt)
-        if dis < mindis:
-            mindis,minpt = dis,pts
-    return minpt
-
-def Heuristic(Space,t):
-    '''Max norm distance'''
-    h = {}
-    for k in Space.keys():
-        h[k] = max(abs(t-dehash(k)))
-    return h
-
-def hash3D(x):
-    return str(x[0])+' '+str(x[1])+' '+str(x[2])
-
-def dehash(x):
-    return np.array([float(i) for i in x.split(' ')])
-
-def isinbound(i, x):
-    if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
-        return True
-    return False
-
-def StateSpace(boundary,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.'''
-    xmin,xmax = boundary[0]+factor,boundary[3]-factor
-    ymin,ymax = boundary[1]+factor,boundary[4]-factor
-    zmin,zmax = boundary[2]+factor,boundary[5]-factor
-    xarr = np.arange(xmin,xmax,1)
-    yarr = np.arange(ymin,ymax,1)
-    zarr = np.arange(zmin,zmax,1)
-    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)] = 0 # this hashmap initialize all g values at 0
-    return Space
-
-if __name__ == "__main__":
-    from env3D import env
-    env = env(resolution=1)
-    Space = StateSpace(env.boundary,0)
-    t = np.array([3.0,4.0,5.0])
-    h = Heuristic(Space,t)
-    print(h[hash3D(t)])
\ No newline at end of file
diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py
new file mode 100644
index 0000000..c83db3a
--- /dev/null
+++ b/Search-based Planning/Search_3D/Astar3D.py	
@@ -0,0 +1,94 @@
+# this is the three dimensional A* algo
+# !/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: yue qi
+"""
+import numpy as np
+
+import os
+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.plot_util3D import visualization
+import queue
+
+
+class Weighted_A_star(object):
+    def __init__(self,resolution=0.2):
+        self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\
+                      [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\
+                      [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.start = getNearest(self.Space,self.env.start)
+        self.goal = getNearest(self.Space,self.env.goal)
+        self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # set g(x0) = 0
+        self.OPEN = queue.QueuePrior() # store [point,priority]
+        self.h = Heuristic(self.Space,self.goal)
+        self.Parent = {}
+        self.CLOSED = {}
+        self.V = []
+        self.done = False
+        self.Path = []
+
+    def children(self,x):
+        allchild = []
+        for j in self.Alldirec:
+            collide,child = isCollide(self,x,j)
+            if not collide:
+                allchild.append(child)
+        return allchild
+
+    def run(self):
+        x0 = hash3D(self.start)
+        xt = hash3D(self.goal)
+        self.OPEN.put(x0,self.Space[x0] + self.h[x0]) # item, priority = g + h
+        self.ind = 0
+        while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty
+            strxi = self.OPEN.get()           
+            xi = dehash(strxi)
+            self.V.append(xi)
+            visualization(self)
+            self.CLOSED[strxi] = [] # add the point in CLOSED set
+            allchild = self.children(xi)
+            for xj in allchild:
+                strxj = hash3D(xj)
+                if strxj not in self.CLOSED:
+                    gi,gj = self.Space[strxi], self.Space[strxj]
+                    a = gi + cost(xi,xj)
+                    if a < gj:
+                        self.Space[strxj] = a
+                        self.Parent[strxj] = xi
+                        if strxj in self.OPEN.enumerate():
+                            #TODO: update priority of xj
+                            # self.OPEN.put(strxj, a+1*self.h[strxj])
+                            pass
+                        else:
+                            #TODO: add xj in to OPEN set
+                            self.OPEN.put(strxj, a+1*self.h[strxj])
+            if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
+            self.ind += 1
+        self.done = True
+        #self.Path = self.path()
+        #visualization(self)
+
+    def path(self):
+        path = [self.goal]
+        strx = hash3D(self.goal)
+        strstart = hash3D(self.start)
+        while strx != strstart:
+            path.append(self.Parent[strx])
+            strx = hash3D(self.Parent[strx])
+        path = np.array(np.flip(path,axis=0))
+        return path
+
+
+if __name__ == '__main__':
+    Astar = Weighted_A_star(1)
+    Astar.run()
+    PATH = Astar.path()
+    print(PATH)
\ No newline at end of file
diff --git a/Search-based Planning/Astar_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Astar_3D/__pycache__/Astar3D.cpython-37.pyc
rename to Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..73e363c10b84b46a633890de35a088c42fffd9a9
GIT binary patch
literal 1866
zcma)6UvJ|?5Z_(@$;Q{FP>!RAI8@3DMrwktZ>TEfs16=bK&k{%q{DKuKF3$bc6Xi9
zM2>h!zW|T$bbaKzz!&fn5)x8A0n$A&v$5MA2yyG|?9S|DW_IQ``)G61Auzh+ukXIJ
z3Hbw=W*D&XV5-MJ1Q`%P1xx6F2}78dWMBwO*sz+P6VVd<k_f(J11q#e`;^H?p|vD~
zR>;HFv-XlwBG?IWo%!pI7fg$BCO_VtO~c)@*ja&j(jOtkfo|I{KZB{xfuLJ-iS8mB
zTp`D6t_>U$4sa%%-O=2}nv2W@iO7Opd{~lW@?>wp3i6zm?20Z}NuM*ZDLR*=WXEJl
zuOPs)$KMjI0UU(7)sKGr6$kesh(eEALcHiekFDnDZnN2Gj`qA-`Ta)nK_hX!=;xnc
z+<)jrPjUSD&>PX(1%nk+Z~{}k1|%XSdBPX8q(8DV^Mw3DB<i@VV%?7_`b=}0y!fFv
z&L&|mNWzQWai~s<Ebm2eF`f>4c~Fe^^CU>qIF0s$B+4WlOjPeA4CH9MKMYhTcE7F_
zdY`Hykp97OuiM4n|0sp&`N45FpS^GnlUNm_Y?2?{!}u{!o6H|XVKGdy(W&Yl!V)Cv
z2sVwTYg8=FD=UyPm{oR==V2;9Xz9OH?XQLq%qR&}MdO>xN(*dP?Q6@3W3>&0unz5*
z9lA|T>cF={<t9j9!WaHm<Le99H28`TVNBS7GBaW|c6K#(b~IvYnBPLo9v}vgRIU-z
z?yuGb1EYkP-U{ou!W^tHx)qlA)$4q7RhLP$Buyk}ZcXqK5}KAgfZ3$_9ZX1&B>$0V
ziJc`|NH`GJ7_?MA4rQq19TaXLdD%t98<Y3pQ0eyql8VWN)V=|->(s)YpluJPdKXAY
z1{7Wn28{`gdr9HV;G&IhhH#+8HYx+4BQY<lyGZa#E4-4#N{%o@9Ju1tCHn?8Wve89
zo%)iMMroE-36H}S1-Vg}OXwNjDr~*Bute)3M~2qmT4F-6jat$c&~UERuRHzsPwI`?
z^!2@FbKtp7<v_x+WLm^odXvnG53^}10y)!w8`x{e>l6W)u_%>=eN|Lulm&^)D^rEZ
z)5^S<#Oc7CX*vgrlHdX*NasN14&3@*h4KA3jSJs@4-OTUj!+wl!r|Md^9{&tw^6yq
z9VQ#6&LtvB-<<ZfahFO2(?1=x&to+Wl6W4vaH^q3jQJ+ozdq@^EnUc4<V`e%?|^&@
z36|8gD(?GfFbRFXa(sW1iD`oS4c|Wl7wZ$bi~4x0Tajo~YRn^4P(xw(oH|^h1PxDu
zIQ4xfP|Np4HUhg=I-TURb&>EH(ACiTau3*nord4TJt`}gaazc?QRN2z*FGzLxDNFQ
T(WyE>Ov(+u#ao>%4q5*j+E<h-

literal 0
HcmV?d00001

diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..637b3cd172846ef2e614d4fb4ea2e33eaa907ca3
GIT binary patch
literal 4729
zcmai1-ESmE5ufh)*xA_+f33gn0$Bn??CH*qaEPMgNJ!!Y1hF~gLV|{XGv1zEk3GAy
z*E9C*u4ePVCA#Cm5`TjIMqUs?{0IC8JhlWv@PPb?cmN?J_*Ku?TuumPcWSDuYkIn?
ztA17edZXbPxPCYJ+mHX+GK@b_<MJ}Wd=qz?g9t;g*cdRMGvaw{4lLi&ycydA$9FW(
zAg}nY<}L75zgo&Yzb0(qd~Eo2Q4ubl4Y4FVQTv$rP0<n!(F8Xq=EOYu=EZ_o#B)Kg
zdq!vZ0y8jnJ4`vRjl*<n8{GWeB%W+-zmddo*v+D3NWCMO9Bzp&Cv{wv@_qx)^p_x+
zamEA_JTne?%Mj+dIRj5JlIXUHHr-av&mldtbMu^?^O;r3?Wg2+DR=O!P@jIwD&KN)
zmRl_&hxOKtRRb+sx2&?|=oY7zX>em^&Fq<zTe+P(JrgUpcO}ETFK9&<pC>}ij;r`^
zq}(_frlX)6Dn5#oog8PORAx6xm6=9EWhKKfRaP2J!%jsx-Ec_#4F6-rP81s}Hc@Q)
zsUcS}=06^79wdWsGl;|S=60AKWyxr>A7ux}`<tU6JGeE9gW)h5_HPAoKauDdq?`A`
zKz0vq?FVTn+ILHZQo6Uby}7YL|No~oij!>bIE&(~?Tyhy)!&d|kcIa~2cZnBm|EHd
zF<6Vuvo-ua&t7D59g?ekcF=Hf>wSJ5%=k{uwv8XXNV}VJ+VM|dV{qoW@sNLD&P=j&
zZXB}QBx?(ceDl<1#(n2~_%|$HDJ@S?aPco({Vjd{=2gY^J6zd8JUR%J-A&>|cDSC9
z-_%Qa5XrD7gF)DF6c5JA-Vfq9RV-3`oGL!SJx#UI3$lh!sJf8BgS}$W=^`X4t;%2?
zdxps`gZuO4_q!H#;A-Hm;nvIk1snrTOzw?e-xRd$;t39I7@>TEEBlKl*KO)YyYC89
zSf_=<(^#BL)3@+M*a{n5MN|>EF1RZC9pQl^J;=d1(057Jcg`7%%n(>`ez-LgHBtM7
z<1P0S-lGk<l+;AEkgOTA3I&v?=Lii^?IECMF4koVS4*582UO7fs^d{+<<3LeXfgA!
zjg{P6HxN)P2drF<h@f-Pu0!q#zG@JMIp~@6_#FeGrhB1R1?OJrfqPQ#{@>p>GPuk`
zmN`f6&zSrur+|0!>bh~xm@&MWIYlcc+c(ABsWq$RwKJDMfdK-}s(B5osps{3hN!{C
z=BFL6jf-|3fz{3q!uEcgbdS<@GSscU3`Pf9N5g<5KvQ-vPJ#?@XNzny3NLI3FUTjP
z=E!a2HIk9`?ga&gvZG=4(u;~6bxip=2z(FN5k81Sc98mBDEguH<Fw<+LO+l)m?%3v
z2u7i@r=bL@*fJRQLuJudCB=nbO~Y*OB$Qd|H;C;4jPyYMd?3Ug*b<z#M5iI2rI|Rz
z3MGM=o{C%|G3zNdR808%B=&hPmYXy_A9WQwcsf+(bg1ZY_Tnhb2)@!bh+)0OT=o`X
z<|}NGt*|D0ncW1P=Pk%QCT~MW8}TZJ2w$$C<0ok3tZy8d@}qI4AtatzIlOya12{N%
zEwy?ao?Xbvj|(|C$N}G4A5c@u!9fncK13`Kw8K+5yu>=SXD<B0%w6)3`^^%1O3>F6
z)q4tfp-s~^>sZPj#o3nd?LgAQc^f;qwe7oPaoW{RRa@FO@4fl1@0~<xv>(M$HYrU0
z1sdo%B3~r(B@kuNYfoFbs;x{tx1dvEunQ{^2Cu=o@*9wMDId`_hx^Kqe}ok13RA71
zRygA63SljR=Jy%La*aa^fxt@M2G1dL4l6V2a}Hf(+v=f5KqPF0x}DpHH2@0$=oIxw
zZlBf94Q6Bw7?bpy4<XIw%GUg4s~q1sT<9Y!BPgq<CiE7e$Eot57DiqI3<2{I?3bWa
zwCW{96|Dxfs7gU#BTwMHmJal~&MW{d(I~kFjRjp@!-%C*R7P5xAhgi7uCy(TSZKGX
z;wiOhO$+2|dUXq8d7WwlXI3fO^{bv0-Q&)xWl!U(XI1xjfS^k5tP!}mptamvD=Llp
ztTAh5%gB8IBtUIFZ-6$4K14>`Tgj?tt745D6Wp3uFS&JbjX0enukT(^`B;=F2{@l4
z(3$U19adCjeMt3n;p2h4X@zlkqO4#V4LYoF>jmmzg?kcaX`ffD4{erevCmO*+@vmo
zg%0<<K_HJpNwtkXSMXOL=5v{70F*cAjSdmiwuW2=>HqW7zx;M*|L&`uYs!p<LRkW3
zk_J8b0{RtuPg!?S?<g}Io&W$dM47VsNf0X&apzak<9*8Ve)S{@AM8cLC_@ntqY%<r
zTDIv+8fU&n!@2v(@lXVEqN4?X*$pz3VTf0YMp2DHa1`$Kft_SC<z8~WGmeA(F!r5^
zej@0D<23BdDUUum3Q%99@>}$=Zxi_rk?#`GC~J)eQ~{0$C_k);<|mq;YJN&Q7j#d2
zE~Zb%egmj+9K_Kypvw9s44ZC%7>N2huLEOUvk6>vInisNFS8A{!LKp;J%gV{+aKbM
z?q#t;X^D8s8!6cbl^G)#e27{M$(vm_ehR?MIi9GQa}!Vg$ausa@keG4kSh4cE+GLD
zr#bz(PKxh@*|8k9gZ4=vBRy40oVR<&!|v@5KN^hUy(~%MqbS<|pJZPZ8vz|3#71$7
z*nqLg>K`K42ALj_b|WnkaWZZP=?GrjmNaxbybp`Cv!s1Pj&IA!Z8^Phvwe(0yWb8l
z2MpX!_Q~tpH_|Zf-Pq6rcCk`@QIaBjq->Y|sw~zXoBAArwNr9H%R~&Kg=jZsXrEc<
z#s_@HXC`=C^Qc?FJDLX~WECnfiYk^8%egai1&Sk*KSp&Z%vIyD@tBnyd(2CYL(@a4
zRRt>P!Vf5naz)@|0;!i5=NK<Xanmv|vl<GcY76HS6fiZaqoGy*A6gA)*`*f7_{<1R
z`fd)y$(uc#u4Z%j++%YOZ{%}p_|{$%+R!<iJDVr&92OyIwxG)|Dst`X#*u}5@{V2s
z6`J$IVl@@w>1#DTU*c%qu;{{U5ms5u=kT3*ocxy1TFMvlMV#(h7^jslY3d;zbI6u;
ztCcT<U&&YCckb@Rud#h4rv2g7B12OKEYh@DRB2jp9a0oqn%9Jot+QMpjYiGa=sh~E
z_#Q?5IF1HL=bE29!A~_WDR%tUgG9z+FGC@g_7XYJKCf|0*3l0zLpdbrTmbl*guZoh
z5_Xk?B#uLyvW9p?nZbVQFKO`l>^Oc4WnDUYYIJ3aB<rmDwj|)jgOFR^!zbjsM7~et
zJtFTD`2mRU=xOedG9t&X6mhhp*gH4@jeI`AJsm0brUo$Inf7Ed%zS&gA9Rm=o94FT
zJ9;)dKHqsku^p5>{0Mk;G{HT^sTLvjpVO1Pi_d<DJAE0%V9x=h@H1Uz0jw+ll*gLp
zJb#`&4{)024%#cc#p@j940`K0T-Mq2+0TIS#vRn+L7cuqIHBPgHMa@m>^SleUqPN3
z;|74rDei%ugq~B!lH?=ufJj7y^d+71<o7_xW+`o$;Q&kl3$C@;(LEi$E1!oP6(9M_
zZfDV7z%CPd#nC=a$2#>m#Z$fw6~FZa&ZPy=Z3)S#>)L<o+R$ZVQNLF-E<*k5P}eS|
z?BVfXG%4Ja4%tOk(+Q30JxQRhr}{+BAZ*jCQ|_HXB97zm75aX<0-`s9HUrtht-7|i
J;#J*e{|&mRXjK3J

literal 0
HcmV?d00001

diff --git a/Search-based Planning/Astar_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc
similarity index 100%
rename from Search-based Planning/Astar_3D/__pycache__/queue.cpython-37.pyc
rename to Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc
diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..8a0c0dfcefa809ea3fd4bbcbc6e804453890d7cd
GIT binary patch
literal 3692
zcmai1TW=f36`t7}mlSoQ6<c;(bd#oq>BdqUr9d7G*B2s4i&h9C*9M5X^op}0S6VJt
zGoyqQmIYe6XkPmdB)|2&zh+<iQoj^^E8wSoXO^TT!9Z8qxty6jb8hpU!^g|ZHHN2t
z?Z1Ec#}Z@zq{;lTF?fhq{TYp5f+wuaO>ea=VF~*cYumyRE_z3JqJrKPzNn)2L`^KA
zuZX%>M(>Liv5LMb*2Fscnz$tz;@T_TUJ@JP15oPXy10RUIa(2$N0$5}PP&<_4%XUh
z9B1#b=7(<}2ixsh)ZrodT)a>4s{3eiF8EEBWA{zANi)mL(3+WTW@tBA&z@MCo$;LI
zR?j*o?#`~{;PJ*ZbVSoCtTZd!P|9%Jv<p5e__%OIETit<nXF>V+dpmhhl6N4Orp_t
zD^f>#m~HoB-9Opi&O+V4mnC7E#%b?fnDmAc3j?*i7lpFhzqcQ%NCaQcHf%pqI+UHA
z*7nvG{r{g$Cpu2lPHQV07hW&Y--Kfa;wWku-{KOkWL?sP9<mb=jGcasL61%N=wZ%U
z?8O%oE9YlS^D_&tom&D{`wJ{4`1zayPtPf2t8>oTaqT(md%<dS#v2b!(<!_#%c4|B
zl17q)3Of`67WDR$VfRS2okx!zKQ4G|Y*;oFg+&Fb(2|ddqK0vY*Z3D)HZWfJot#6f
zMxPc2<VkInelKHSlL_k@MFRPpQ@fy0Oqj?hDLs1%joxV@NfT-OZyOB?M;%LD*y?1^
zwAxNKR6jTEXYZ4~6)U|02Fk=Jmmgrfkh_8j*~G~G7z4q7Z%wR;J^DOnXY8E(acAOa
zo;y=MaiNi$^QkrQaxb@zm~5bTEk<7)H(-AoQd_4V!DxZ*M?s3Xij)qrVVvq<m>ObF
z#-eG(|AJP#@PKm^jaA`^s2{4nk*--O+`TO9Mr|w8ZEp~#B35k|Jx!ICnO1EZO<`l<
zs?IeWzFVHV4uury&Mcp=a37ANUfu#_VFFU0qQscsGYn#=GrF@-|MtYz<VvpX9FEy!
zQ$FRnmD~5(@d{M7pfW#Q|7|!5(xDs#5J`t=H)=ZaI=B_~ARHmu_LaPWDX5!7<+|Yg
zf*)O#zWP;kBEcc<eu!;l#90o6zRhI-%HjsmO<H*FQ%v*>Wu}Ov8|4f!Hsv=EceA-u
z&LOZ{BFkGIn7A?#*5rsNEt_1DIj#!kk$ZMpA7g1ANk78)k~|ADl7Z<4(9L}Xxz2E3
z&v^^d%;I|S83iS&v4>Y}py^@zi~58^9oYGA*0Hl!$^!A+gJc+L0>Q1HNpkKLA>Y>S
zC$L?2ILIFSgcuTckDcBsgKO(P?s}N02N=w^$&aw5SzfCW7+jSlv3|c5j0=A}=ghd7
zFCvLfLr|BIq%pX7{T(bF!H1yXy5s6(sl!?Nn~Q)j+tUIKu3GFtEDF~^1lrl2K4{*B
zJ&vgCG2z3t)zY=H4grjF-;jNchu;wLJ^j*Ht!b4Oe4nqYv5M3E;Ylj)-~&Zl7*7Cv
z@-9XTeaQm^Am--O5Rf@LWQQE^O+6ripqu2*?G1JVH^n4BGuvbU4QJwlX5etK1A3SH
z%?@u;!49MQVh6Oou*08JwSDH~4uHu%x6fSdo%2b}(7e2Ifwp95eqOymtAkd_Yxz=M
zKes2#nECm#uyi%AO*z`7Dc@k|>(nFBSd$f?!|LdEzH(RtG&Uw{+CE&$S9LvKn{pt+
ze}D+9@IvJ@f7*{#aBz}#bqwUg_=K{n9tMPt=}<%}2vb3L(Up;oPa}hlihysc7L?DP
z4s{e58gbYk2cx^=yQiP-1mE=|83j?;?FT!w6wW|4P6H%iQ#ZT|0ZD!=(?}De=5k?2
z>9>V%d>qPgQ3Fg56QuBAT4oq8lwsP7kT{hdXOa8_pB4TfQhkE&qJsVXFpCO1iPC~U
zE$lcwkOT<L^`aK1vChC?pxWL+*wsTR38CaYV&aSd<kSKdov|5@&G^)ePcd#gBUCtT
zXKdP2v;}|Gww^tkVduA3?tM`+b}~7G&QQ0}Fxv;(e{B9XIQ^~gPY}a(-r({9*1L39
z>7hb|l0D7UztPO%*w)sWtsRZDcj$f3FhevVdaS8Q&`68^_lRnwLswTZawswCn)!N(
zX6hS(#Cd=WF=`<~=vFLG0fR3-pH%e9;p*8MqPBvl@^fEUM^*XK@bST?ia7Ov7cb`=
zj$b}!BcKI%tb@bD8HdI@4n1~c%h!Kp2PiIt^9no(aB&x|A;Q6H%H`_?x7W{sDWz;p
z@AQW%ih}qcNa7TIsw1hQu2#WtAGxfXL{E1w0XD(NH8K_@I0M)#1lTYEYJ*FVUFLI3
z5@r_s@Di$$$tA(`*Ldep{exIN874_AqW7Wom(30NQ~Hd&e4&_g2+t|gW^xrP2-K5A
z$5}EqRi_ccKvx-tRJqAD+UI0r#Mx>uLdC3Q-0f&oE2`Z<b=_H*p%aGt`-Qt7CW&fq
zTwYQcT|=bzBxx?mUl3yhs8kD<;qG?(aU$9_zAGH64GiqI9n~Ld{PX~!^Bum%uMZ4W
zhfXQ+d6tGv6$(I}S>=~ttjK6o(gY}IYNoJhn3l?$I!09LG%%`j`4x690Q*vcBF0oR
zkk6=u*@bhcHk>C0I_c^?bGk#Ne}{#63Gge73%}DDgmK#GfCG649aSel0yjMwz{Dm2
zJprk7hp|5$WePD<))?q8*|hW%-A^OCoS%?}gQ(LfT;SGDs|*7AxpP%Q|NdYoPLk*W
cNnmgQC}8=H@A<Z0^DAg=->o(L>!9)f0q`9WW&i*H

literal 0
HcmV?d00001

diff --git a/Search-based Planning/Astar_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py
similarity index 81%
rename from Search-based Planning/Astar_3D/env3D.py
rename to Search-based Planning/Search_3D/env3D.py
index 977aa8d..d71b524 100644
--- a/Search-based Planning/Astar_3D/env3D.py	
+++ b/Search-based Planning/Search_3D/env3D.py	
@@ -7,7 +7,7 @@
 import numpy as np
 
 
-def getblocks(resolution):
+def getblocks():
     # AABBs
     block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00],
              [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00],
@@ -19,22 +19,22 @@ def getblocks(resolution):
     Obstacles = []
     for i in block:
         i = np.array(i)
-        Obstacles.append([j/resolution for j in i])
+        Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
-def getballs(resolution):
+def getballs():
     spheres = [[16,2.5,3,2],[10,2.5,1,1]]
     Obstacles = []
     for i in spheres:
-        Obstacles.append([j/resolution for j in i])
+        Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
 class env():
     def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
         self.resolution = resolution
-        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution
-        self.blocks = getblocks(resolution)
-        self.balls = getballs(resolution)
+        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
+        self.blocks = getblocks()
+        self.balls = getballs()
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
 
diff --git a/Search-based Planning/Astar_3D/plot_util3D.py b/Search-based Planning/Search_3D/plot_util3D.py
similarity index 95%
rename from Search-based Planning/Astar_3D/plot_util3D.py
rename to Search-based Planning/Search_3D/plot_util3D.py
index b164fd6..0da691c 100644
--- a/Search-based Planning/Astar_3D/plot_util3D.py	
+++ b/Search-based Planning/Search_3D/plot_util3D.py	
@@ -54,11 +54,11 @@ def draw_line(ax,SET,visibility=1,color=None):
 def visualization(initparams):
     if initparams.ind % 10 == 0 or initparams.done:
         V = np.array(initparams.V)
-        E = initparams.E
+        # E = initparams.E
         Path = np.array(initparams.Path)
         start = initparams.env.start
         goal = initparams.env.goal
-        edges = E.get_edge()
+        # edges = E.get_edge()
         # generate axis objects
         ax = plt.subplot(111, projection='3d')
         ax.view_init(elev=0., azim=90)
@@ -67,7 +67,7 @@ def visualization(initparams):
         draw_Spheres(ax, initparams.env.balls)
         draw_block_list(ax, initparams.env.blocks)
         draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
-        draw_line(ax,edges,visibility=0.25)
+        # draw_line(ax,edges,visibility=0.25)
         draw_line(ax,Path,color='r')
         ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
         ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
diff --git a/Search-based Planning/Astar_3D/queue.py b/Search-based Planning/Search_3D/queue.py
similarity index 100%
rename from Search-based Planning/Astar_3D/queue.py
rename to Search-based Planning/Search_3D/queue.py
diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py
new file mode 100644
index 0000000..d34aaa2
--- /dev/null
+++ b/Search-based Planning/Search_3D/utils3D.py	
@@ -0,0 +1,90 @@
+import numpy as np
+import pyrr
+
+def getRay(x, y):
+    direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
+    return np.array([x, direc])
+
+def getAABB(blocks):
+    AABB = []
+    for i in blocks:
+        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
+    return AABB
+
+def getDist(pos1, pos2):
+    return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
+
+def getNearest(Space,pt):
+    '''get the nearest point on the grid'''
+    mindis,minpt = 1000,None
+    for strpts in Space.keys(): 
+        pts = dehash(strpts)
+        dis = getDist(pts,pt)
+        if dis < mindis:
+            mindis,minpt = dis,pts
+    return minpt
+
+def Heuristic(Space,t):
+    '''Max norm distance'''
+    h = {}
+    for k in Space.keys():
+        h[k] = max(abs(t-dehash(k)))
+    return h
+
+def hash3D(x):
+    return str(x[0])+' '+str(x[1])+' '+str(x[2])
+
+def dehash(x):
+    return np.array([float(i) for i in x.split(' ')])
+
+def isinbound(i, x):
+    if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
+        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
+    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
+    return Space
+
+def isCollide(initparams, x, direc):
+    '''see if line intersects obstacle'''
+    resolution = initparams.env.resolution
+    child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
+    ray = getRay(x, direc)
+    dist = getDist(x, child)
+    if not isinbound(initparams.env.boundary,child):
+        return True, child
+    for i in getAABB(initparams.env.blocks):
+        shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
+        if shot is not None:
+            dist_wall = getDist(x, shot)
+            if dist_wall <= dist:  # collide
+                return True, child
+    for i in initparams.env.balls:
+        shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)
+        if shot != []:
+            dists_ball = [getDist(x, j) for j in shot]
+            if all(dists_ball <= dist):  # collide
+                return True, child
+    return False, child
+
+def cost(i,j):
+    return getDist(i,j)
+    
+
+if __name__ == "__main__":
+    from env3D import env
\ No newline at end of file