From b25bfec8d3a3b44984cfb171ab147add1da6b725 Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Fri, 26 Jun 2020 23:05:10 -0700
Subject: [PATCH] A*changed

---
 .../rrt_3D/__pycache__/env3D.cpython-37.pyc   | Bin 1933 -> 1908 bytes
 .../__pycache__/plot_util3D.cpython-37.pyc    | Bin 4829 -> 4804 bytes
 .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 5349 -> 5324 bytes
 Sampling-based Planning/rrt_3D/rrtstar3D.py   |   4 +-
 Search-based Planning/Astar_3D/Astar3D.py     |  38 ++++++++++
 .../__pycache__/Astar3D.cpython-37.pyc        | Bin 0 -> 2798 bytes
 .../Astar_3D/__pycache__/env3D.cpython-37.pyc | Bin 0 -> 1865 bytes
 .../__pycache__/utils3D.cpython-37.pyc        | Bin 0 -> 2838 bytes
 .../{3D => Astar_3D}/env3D.py                 |   0
 .../{3D => Astar_3D}/plot_util3D.py           |   0
 Search-based Planning/Astar_3D/utils3D.py     |  66 ++++++++++++++++++
 11 files changed, 106 insertions(+), 2 deletions(-)
 create mode 100644 Search-based Planning/Astar_3D/Astar3D.py
 create mode 100644 Search-based Planning/Astar_3D/__pycache__/Astar3D.cpython-37.pyc
 create mode 100644 Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc
 create mode 100644 Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc
 rename Search-based Planning/{3D => Astar_3D}/env3D.py (100%)
 rename Search-based Planning/{3D => Astar_3D}/plot_util3D.py (100%)
 create mode 100644 Search-based Planning/Astar_3D/utils3D.py

diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc
index 03312dddc8e16234be4ab1342b0a4aa710aacbdd..d34d29617282354742d7eaeb24c201b9d0a3d08c 100644
GIT binary patch
delta 84
zcmeC>|H8-Z#LLUY00fLPzHQ{b%E(wh`5~h)WBTN;jPjdxnHDlj-eRfD&CI*SQUxX}
gauX{+B0zHTbJj0xDnPA8s*@kG#WSi+c4YSf0E#FajQ{`u

delta 84
zcmeyu*UQiC#LLUY00j2>Up8`IWn`Q&`5~h)qx<BqjPjd*Gwx!RjAE(G&CH8psREL>
iSSoT8D?lPZa`HLWFKh}xtwoBHPq4)^Doxg4_W=N7?i%O-

diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc
index b0f8a3b12d763a0397a45656fbba76787ac233e0..231d1af8fead0655796278af8e41a47a861b8a3a 100644
GIT binary patch
delta 680
zcmY+CKX21O7{+^!?YMCwrJ)rlf`$}=Y6w`g2&w84pq;2dLZxaCq4W}8T@(M~wMi2X
z6b4usZeig&fHJXB2`0XQCB6ewC78H3EMV(?e$LP9*M9E&qxH3A-mO$hGx8wsPd}MN
zS$Ja~Mgg~(&qwwKPX=ie+pcITjme%;A%qIIuuEE#FNKeaoIKa(NkhEV+r$<VeVcTI
zQT%>^Y7Ej<hw58i$X7RVgE>yum)e?zhWJ^mSDW&Uu!OjTxGd_%I@z4;7@<P$iEpKA
zdK>TC;<VJ(ucEZXvT2bO@yJ}eO=pK5Kk9m+m*!gE=M12QH5U;b#F|8I^r9D`!(a@X
za!SRC`LMZ-?qkFbVi&O|k!##>c_KgWX89e7Cnx2_!ts4{w<KuE<!N{1dx1<f)#HD3
zHr1eF;rvkqzSB*Cg~>q#0ZY9ogcWR5mdI7dOH!zyEbuX>>Tt;Wxh@xn`EIU;@{zp2
zdI>!<^lCEW|Ba}KtLLv%4Y<#T^1?DN02iAdB0R)1!~o$-Q1Q9Cv`CdK{+k-}Q$0I?
zC`_rA^;myE#jolu!o-ca=;>>;6&T4emLtm*2!LU;vl5mHFHs*OULmj^&LQ?Ca9tCO
zIir_}F(Xg4W=$PSc>q_$?>S3*B3&w$YwIR02C*N>{{-%cuvTA~Zed5-)9r#7)xP*p
G>-+&-V6*%H

delta 727
zcmZ9K&2G~`5XZ3{n|w5CQd-&yDKtP*nHHr|5K@VdP$-I8L5oBWB?3iGya`rquNk{;
z;tLn<NGx19aBUA9KzRTz+<1eLD!~(Q;?9n_04wd!|DBorm>GYqoz)8MVljVC4!-m2
zb3suTJ~W<lk2NfpjT>7mJdAkI=!mL%dAh3B6$QnvcA(U!C)y|VB9<)M?hUx>vO(l<
zziFFASP`DFu55_+#=g=PKaFo4)GdgbHX08cpEb8IOS@dD%^a-AtGn9c`}>%WozUq!
zt`kk5BCT+RaFuY4P!meFrEE{{WPMfXh>!U-;|AF`#94mbSR;8|%oa?gDK-n6w=fqw
z?6~LnPK5fvWfnl4Vyc8Tp(TOYK0oqp3ns8lOL1D*s~(W|3871PN_ZxLI<rSClo>9~
zyjJ4r=}bl2*(2{G^5#b@>Wy8;lc}b9@|R9h4H*iYKjz@ty$CEn9CGk1svR^%skHPG
zGjYrYXvp$d?g%r!^rL3=L%fjY^Y12{4;X~Oe@CHa^T=F89hl2v`LPnm1CJi+6F4Ct
zydZ!C8tI!Jij&f%3aUx)Cx5C08p#m2FRPgJt-&FR@1>i{NGz0j>z#CWRT#@TkuyOR
zJQ&fLixOrA-jEiAw*-oZCBjgADKF=SQkv>6tco9HQ}4+YirKl=Z9MM<E|<Ry+@s*M
cXu5yo1rx}UVVRJ2Gi@Yw4r0PkyqdfF8-V!M0{{R3

diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc
index 530e52aed392b15bbdcdd71df5b2a33e22f530fa..6e99341e006217282fa6c0df80a5ce0927b4eb70 100644
GIT binary patch
delta 209
zcmaE=c}A1liI<m)0SFjpeA~!<kx{f!KO;XkRX;H&wL;$|wK%&Zzd%2I@<&Gb&6-S2
z+>BA1*YK(_GP-QO%;(L>=sH<mz=6?obAo^|Bd<5mlp+Bjp~+I@GkJmFaw)KgJy7Hp
zTTyCmepxC=2BgYwa-z^$A-IMjPna$R;W7bVkP3eg5eOneCr=a+XDafVykA(9(QEQm
cVPi&@$($lpd>~D?*z%LIQj<%7(i25O00p`>V*mgE

delta 235
zcmX@3`BanJiI<m)0SIn~eBH=>kx_J(enx(7s(xZlYK6W_YH@Z+eu2LG<d2N<o4+&8
z=4QOLc`~mWBcto)U3}h*jBb<J1RNN>HoFTLGxGWXO(_xp5}GVUzLVPpm&<}h?13V;
z*osnf^UG2}G9Xp{Kw|O)L8-~9LKB4G`ii_@n%RZR1pGh>13*L&hzOh9D=Z=I4P>?h
xaWMyw;9%rn=3wVwDe|7YN7$Irb@B({Dn5`Ix7hNNvQm>vfD*mJ;*<MDYylr_I?@0D

diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py
index dadc702..3734e8b 100644
--- a/Sampling-based Planning/rrt_3D/rrtstar3D.py	
+++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py	
@@ -23,7 +23,7 @@ class rrtstar():
         self.E = edgeset()
         self.V = []
         self.i = 0
-        self.maxiter = 4000 # at least 4000 in this env
+        self.maxiter = 10000 # at least 4000 in this env
         self.stepsize = 0.5
         self.gamma = 500
         self.eta = 1.1*self.stepsize
@@ -61,7 +61,7 @@ class rrtstar():
             if not isCollide(self,xnearest,xnew):
                 Xnear = near(self,xnew)
                 self.V.append(xnew) # add point
-                visualization(self)
+                # visualization(self)
                 # minimal path and minimal cost
                 xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew)
                 # connecting along minimal cost path
diff --git a/Search-based Planning/Astar_3D/Astar3D.py b/Search-based Planning/Astar_3D/Astar3D.py
new file mode 100644
index 0000000..9ebe7a7
--- /dev/null
+++ b/Search-based Planning/Astar_3D/Astar3D.py	
@@ -0,0 +1,38 @@
+# 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
+from queue import QueuePrior
+
+
+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(env.boundary) # key is the point, store g value
+        self.OPEN = QueuePrior() # store [point,priority]
+        self.start = getNearest(self.Space,env.start)
+        self.goal = getNearest(self.Space,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__/Astar3D.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/Astar3D.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cf152275033871ae2590872fde16b5789509c063
GIT binary patch
literal 2798
zcmZ`*TW=#t6|U-=+a2F>wKIfKgg_!l6R&n8c7>v0GLu<&K$$E-Gs0SEb=)1ho%SWw
zWpJYMllOJe2qB)6x8+ap2lxl|gv86vGZGKHu-~bUqfxZ9UFT9&=k%#_tA0_f1`N+X
z{`sHZzpOI$FJhKY3B+@Z{LcU}>#&%|R>nJ)!FI<s*y%WlGjMOb#0D%G=qKe{e#>LK
z$!<9(IVTH-E1eZXyIEye?No=gPHkB4)Q62uW4PK`HEUkBHeBzlbM^}smty}*7W>J@
z!tQJ)TXFfq5`RiI7p&8aty8uaT(1Vtqp9qV#m{!HrpfMQ8oY<8?D+co?frfFpC*y$
z_Me<Zc@poQWYK7pj(Y7quI*&>hdo!9dWk$deDT5%M`<p#hnZhTS2{SAkxWh}Q8&@$
zUnWz5#k32_;PYuRO-@8Q7F~MT^07cX$H;#OkgyJi$Bd`GWbl+7JHSrt#vZU6mteoI
z>u-~^*Oy5g9)`JusT{KO5PppD5+nZ<oMbng+#Opz8xWMipn%Pa!39t2%Xz{iQS1kG
zCuf`af~R%j0SzVz;t@cz-vrw5#Xj0edg(n&x#E3G`C^{*EbUn8E9^1$^&Z`F#X?Kk
z3kGaUM<Rt5H0Q~Zne$~?v4MyAU}0fCxaU_4zalHNj#2Q{lr+^0tj7#ac8=eZGX;V7
z|HHG_(AH?8-AD+8*+0y(I2B1(Q2c}kp#8J)bQDM8O1mch!lzm1^~tMa9Uwi9kyHs%
z#ifu*?exY`Ca736?`!V_izDRG%U``deRYI{aq=WP{~HtA*zw;l+Wqk`X-8QyYab=~
zg&a@Xy;Sz6XYENO`%fl!Ir=2ZdSiiwVg7%T_>kTnK0Rt1;_1=;1g8nZbd<_4T*tfe
z769W1{8RoBhR-V)4fFH(b?ZUu?Jq^|bCB#UW0)NL#^j*n3qI!(5ksuph5dy!x8}C=
zl%<@71<sv2hVu+pntLgm`?JTgBz<KqAbrGsSDTk*c@QXHl@HkEXK&FSK$i}fTq*R-
z16iS5MDaUc@%ajLRA|RGQvtyyQ(n(fD<p%pYRu;~S&fl7oW0|-mU30)qV$f7zpIr3
z-qdc{ysql8x6NqG%b+=ML)GHa7A!z(`9@Y%jdZB8s;CC+se|IM$(9YuO`O=r8CU0P
zv#-sG))qYWTFjj4FY|S^rq-czL#-OTsW$KI*n#y8tZV^q0you`T!rq9h1F#9mTIZy
z1uPdkps?eBsfMZ=ys1_$JgodxwPFvahx7)Wf5V&~J4cZI?v;1^a_23>`Mgiqymt9D
z=qhSsO*w$;@DU3)6e=g&R3*SIoRfvk*hkHjubY4DCnDLE{bcv>C%b9BE2gG(nlif2
z5UMFEn^TOOibTPa+f;SdvDs;I9woqMhAO}#Y}Eci77fqh=-Ef${*A$|fAlaT@y`8&
zY}}2q{27R1KlSd5<m$RQ5#w%>=ji7!QP3^+J_b16YY7`zduI^6$W2L$I?V*A+Hp3O
z>3F2Q^QbGwLafn(E22>^(RP-MM1>~obabwR`>y}7_9mBc+C|AvM2g}rO~#W{CPVFa
z`)L-V;6;t(obEW6Vhg0$Al|3efRc;_6{IekhC#gMZ}zqY6{vRT{t;~=tv4SVIq?LX
z5fS?|&wHAuVv{CU2{Z}NTeJf!gtpR|c2lF*kFpH<lbI&USclWZGtCFu9hfavFV$9W
zpn3E#C^kI)1E|g405HpU8oY*|!`A?P9`Ks&@D_Ln+(yw+EOZsi=a2Xnc-lj-c83H0
z$bO8z<K6$R*S{mm_X$OTb1+QH`3(p(cGg470p@dS_G3!YfpuYvp90&;LNQs2-yo$2
zE^+j=aoKpwq&Fxjb1i;lp<#J9zQXqi^l&7nT_h*9=4h7Yd*u(DPW~=j>qV1Eg2+*1
zimY(XvveZxl>`-cv7??%*%Ci=W##omrb7h(1Ur5Uk-P!G;2RJA2;e>P(e=(lZ+s|r
zyysCzA4S6?40RBO!*M*#h^~a;<uuBQ74bbN7C$HOiU9TP4*->$pgvBip3?<~2-W>h
z%pB<-M(HRF_gcCXhVi%y_3mgooLm)$w#K=(^Q&A?OvE98F5$wYSL;%AmKzU1%Xw-n
zt1fRNd|y31GMB2ZFXvEtS&mr>v!FN&mjKyR+@J0xeOndxtv5a!BwcCD6rW*>c8#77
hj%H4J*G>xGJR@B>)gDq5Y3W<LaQ6<c*p1J9{w;STq}Kod

literal 0
HcmV?d00001

diff --git a/Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..a3ab9d0f7f20e55685a0233fdb12120ba7874b72
GIT binary patch
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

literal 0
HcmV?d00001

diff --git a/Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..aae19a23a65de8f8fbbff6570b1544d1f40a0fd5
GIT binary patch
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-

literal 0
HcmV?d00001

diff --git a/Search-based Planning/3D/env3D.py b/Search-based Planning/Astar_3D/env3D.py
similarity index 100%
rename from Search-based Planning/3D/env3D.py
rename to Search-based Planning/Astar_3D/env3D.py
diff --git a/Search-based Planning/3D/plot_util3D.py b/Search-based Planning/Astar_3D/plot_util3D.py
similarity index 100%
rename from Search-based Planning/3D/plot_util3D.py
rename to Search-based Planning/Astar_3D/plot_util3D.py
diff --git a/Search-based Planning/Astar_3D/utils3D.py b/Search-based Planning/Astar_3D/utils3D.py
new file mode 100644
index 0000000..5db2fd9
--- /dev/null
+++ b/Search-based Planning/Astar_3D/utils3D.py	
@@ -0,0 +1,66 @@
+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