From 1b38e203f9967dfca69ecd0f48a9219f81e0079e Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Fri, 10 Jul 2020 21:52:57 -0700
Subject: [PATCH] 'update'

---
 .../Search_2D/__pycache__/env.cpython-37.pyc  | Bin 1270 -> 1308 bytes
 .../__pycache__/plotting.cpython-37.pyc       | Bin 5209 -> 5247 bytes
 .../__pycache__/queue.cpython-37.pyc          | Bin 2673 -> 2711 bytes
 Search-based Planning/Search_3D/Astar3D.py    |  73 ++++++++----------
 Search-based Planning/Search_3D/Dstar3D.py    |  16 ++--
 Search-based Planning/Search_3D/LP_Astar3D.py |  16 ++--
 .../__pycache__/Astar3D.cpython-37.pyc        | Bin 3182 -> 3296 bytes
 .../__pycache__/utils3D.cpython-37.pyc        | Bin 6075 -> 6306 bytes
 .../Search_3D/bidirectional_Astar3D.py        |  53 +++++++------
 Search-based Planning/Search_3D/utils3D.py    |  18 +++--
 10 files changed, 98 insertions(+), 78 deletions(-)

diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc
index 1ae15c3a2c1fb7ad05ec216dc0f65f1d5bfeaddf..de55fabb523261a02a9cfb8c4070bce7b089d00f 100644
GIT binary patch
delta 83
zcmeyyIfskeiI<m)0SF@PnKyE`G3quaTg8MHrxq2*7+V?|8^*Y#7H5~_7sRAzmSmJB
j#S|o#Wat*;B<AI1=B4XS-p8oLuCJ#DLYq%A>M{cW@_iXm

delta 75
zcmbQk^^KF;iI<m)0SNx~Gi~H<W7Lgxwu%WYPAw{q@hHvA&CE+zh|0(>jd4jW&MwI>
bh$%=c$xtZBNzBUwNlo6zsI{4oiH{iodLtNi

diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
index bff683c9238d067d6b2e818155609bba9329eddb..b11ed3be8423a268a86a862c0a425dcc52b5cab9 100644
GIT binary patch
delta 78
zcmcbq@n3`6iI<m)0SF@PnKyF3WYq0Qwu%WYPAw{qF}5@`HjHseEzT~<FNjIcEXgQM
giYZ7e$<QsxNzBX3%uCmutjMG_ai`2?M<xj&0JlpSEdT%j

delta 75
zcmeybaZ`iaiI<m)0SKG~88>pjWYkS^wu%WYPAw{q@hHvA&CE+zh|0(>jd4jW&MwI>
bh$%=c$xtZBNzBUwNljK{(%M|e#3uv*Xo?s8

diff --git a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc
index a4592425fee42f1cdfe952de2e8095143300c66d..183bce05a9e98a2e48ee4eb82470915ac791899c 100644
GIT binary patch
delta 84
zcmew;GF_C%iI<m)0SF@Pnd3L|6f^3!CR@dX7N-^!#~5208XLyAq!wqF<QK%GXO?7?
kCdCvamSpG_<Rs?hW#*;pPF};P#jdZX2SS^-Gw$L501i4D8vp<R

delta 76
zcmbO(`cZ_(iI<m)0SGut|HW<ODQ47-ceaWNElw>ej`1kX%+1V8SBT2UFO6|YEzT~<
cFNi5fEXhzP$Vtr014&I@!>G0S594eO0Fmh$EC2ui

diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py
index 0331cd2..8a5b430 100644
--- a/Search-based Planning/Search_3D/Astar3D.py	
+++ b/Search-based Planning/Search_3D/Astar3D.py	
@@ -13,27 +13,26 @@ 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, g_Space, Heuristic, getNearest, isCollide, \
-    cost, children, StateSpace
+    cost, children, StateSpace, heuristic_fun
 from Search_3D.plot_util3D import visualization
 import queue
 import time
 
 class Weighted_A_star(object):
     def __init__(self, resolution=0.5):
-        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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
 
         self.env = env(resolution=resolution)
-        self.X = StateSpace(self.env)
-        self.g = g_Space(self)  # key is the point, store g value
-        self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
-        # self.AABB = getAABB(self.env.blocks)
-        self.g[getNearest(self.g, self.start)] = 0  # set g(x0) = 0
-
-        self.h = Heuristic(self.g, self.goal)
+        self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
+        self.g = {self.start:0,self.goal:np.inf}
         self.Parent = {}
         self.CLOSED = set()
         self.V = []
@@ -42,40 +41,36 @@ class Weighted_A_star(object):
         self.ind = 0
         self.x0, self.xt = self.start, self.goal
         self.OPEN = queue.QueuePrior()  # store [point,priority]
-        self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0])  # item, priority = g + h
+        self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0))  # item, priority = g + h
         self.lastpoint = self.x0
 
-    # 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, N=None):
         xt = self.xt
         xi = self.x0
-        while xt not in self.CLOSED and self.OPEN:  # while xt not reached and open is not empty
+        while self.OPEN:  # while xt not reached and open is not empty
             xi = self.OPEN.get()
             if xi not in self.CLOSED:
                 self.V.append(np.array(xi))
             self.CLOSED.add(xi)  # add the point in CLOSED set
-            visualization(self)
-            allchild = children(self,xi)
-            for xj in allchild:
-                if xj not in self.CLOSED:
-                    gi, gj = self.g[xi], self.g[xj]
-                    a = gi + cost(self, xi, xj)
-                    if a < gj:
-                        self.g[xj] = a
-                        self.Parent[xj] = xi
-                        if (a, xj) in self.OPEN.enumerate():
-                            # update priority of xj
-                            self.OPEN.put(xj, a + 1 * self.h[xj])
-                        else:
-                            # add xj in to OPEN set
-                            self.OPEN.put(xj, a + 1 * self.h[xj])
+            if xi == xt:
+                break
+            # visualization(self)
+            for xj in children(self,xi):
+                # if xj not in self.CLOSED:
+                if xj not in self.g:
+                    self.g[xj] = np.inf
+                else:
+                    pass
+                a = self.g[xi] + cost(self, xi, xj)
+                if a < self.g[xj]:
+                    self.g[xj] = a
+                    self.Parent[xj] = xi
+                    # if (a, xj) in self.OPEN.enumerate():
+                        # update priority of xj
+                    self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj))
+                    # else:
+                        # add xj in to OPEN set
+                    # self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj))
             # For specified expanded nodes, used primarily in LRTA*
             if N:
                 if len(self.CLOSED) % N == 0:
@@ -111,7 +106,7 @@ class Weighted_A_star(object):
         self.start = xj
         self.g[getNearest(self.g, self.start)] = 0  # set g(x0) = 0
         self.x0 = xj
-        self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0])  # item, priority = g + h
+        self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0))  # item, priority = g + h
         self.CLOSED = set()
 
         # self.h = h(self.Space, self.goal)
diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py
index e618237..a501429 100644
--- a/Search-based Planning/Search_3D/Dstar3D.py	
+++ b/Search-based Planning/Search_3D/Dstar3D.py	
@@ -15,11 +15,15 @@ from Search_3D.plot_util3D import visualization
 
 class D_star(object):
     def __init__(self, resolution=1):
-        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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         self.env = env(resolution=resolution)
         self.X = StateSpace(self.env)
         self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
@@ -133,7 +137,6 @@ class D_star(object):
             self.insert(x, self.h[xparent] + cost(self, x, xparent))
     def modify(self, x):
         self.modify_cost(x)
-        self.V = set()
         while True:
             kmin = self.process_state()
             # visualization(self)
@@ -175,6 +178,7 @@ class D_star(object):
             self.env.move_block(a=[0, 0, -0.25], s=0.5, block_to_move=0, mode='translation')
             # travel from end to start
             s = tuple(self.env.start)
+            self.V = set()
             while s != self.xt:
                 if s == tuple(self.env.start):
                     sparent = self.b[self.x0]
diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py
index de4632e..33ed5c0 100644
--- a/Search-based Planning/Search_3D/LP_Astar3D.py	
+++ b/Search-based Planning/Search_3D/LP_Astar3D.py	
@@ -17,11 +17,15 @@ import time
 
 class Lifelong_Astar(object):
     def __init__(self,resolution = 1):
-        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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         self.env = env(resolution=resolution)
         self.g = g_Space(self)
         self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
@@ -177,7 +181,7 @@ class Lifelong_Astar(object):
 
 if __name__ == '__main__':
     sta = time.time()
-    Astar = Lifelong_Astar(0.5)
+    Astar = Lifelong_Astar(1)
     Astar.ComputePath()
     Astar.change_env()
     Astar.ComputePath()
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 a14034838a09e1b4bd06f6b9625683610ddea84d..3cd3c71112e5835c1d819a72d1e7c6f44713b8d8 100644
GIT binary patch
literal 3296
zcmcIm-ESMm5#PNpeu$DNiu$rGn|7QwbYsPViW+s3*p8JC6osNzZ37%s98TObb=2`l
z?JW~Ypn#$@(7qJ#AJ7NtvH#LO8EAn*|3Y8d-|UH$qXd2Hkvls#GdnvwJ0JJy>};9g
zIeuIU+I7bMK`)c10OT1)_D=x8d?vWC67E}Ow!4n+@|@=Ro}m@|LR9D!$EC;vEE_lz
zRnGZ27xoG}=SJR1X1jC#oWb2>zFYOH-35Q4Tk~t(MSrnd_v_syf2q6dFLzh`mF}v)
z+FkS4jJ%iJ=-%{iUhaF#zh(Aa_t(E*q9BT|m?%cKN49?_YKYRICErBr<Mngq-;M5w
z@`(HQqI=_a&KYOX{hSlsU1cM(%i3klnbU>xvv8medh$u*WDqru;_?L!&$dt3KiJry
z|85k@*1^O5FpETECkfLuPTL<exwfP9n`T)T+L79dGo?KszX?x32zL8nE7GMeqk#k`
zZs{_3+fWoit`ui4dPx$CNIR__2*uVxoCq1Ey1c7G73HgE4<@UFmxC1h&3+SSgD{Ct
zLlyVZ7FjuYY#`4tvPS?B^Eoch5;MXQ_ABn&q9R=30dsPf`K~C5@+*c*bl`+pJ^ytS
zw+~b#g6Bb|LisPK!Sm*6%gQlEGs1<vz{u`%ga^O+DK<u+ob&(JEbPD4hAgm@J><gq
zD+g{XXUIBs#~txY%o}lwf#YI$;JE@=$5X6Rh%L~HF>y<(JfeA~O|T*$8O$1dj^u-R
zT1o4St|}-Csy1FcC+Rxq_3;uQ=$#_XYfLQ<!Ru5<7Q)@)28Zm5VDM~V`vR_F7v#NW
zt!c|D5J@BhKrr}}Iw4G{P5a_Mc)q`+c3p>ALe$AFZ!4YR#(>E#hDrbgqwj5{Q{1p7
zOQvFzU8Z6a9n-BQ5^ozemHxgR#QM%I#xSnS$eCU;ppZ9S_`JEQt+cP5>_{SYo+pWj
zWz>@7V(qFyKZ&$UF|4%H?uCiA<MgHGZSCzK{ZggvEJCin_|;dtpKWRWd+mr`icH)I
z)d9&0Z5=<tP})5jM1x3|e@)YljC)c$U+sLht?m9m$qMaPO2SO_dvU6o7A%UAmvRyG
zzccw0O#l9K>&f2lvPfoon?L_>^TWNZC_7ZW{$4v)2ZQ~+KDK<=zpN+^!=&AlV05#+
zA1pbS)L?V#e-)-HslDgqh0U#v{)sLIL7c`a2$o@1_9FnsJ^WV9ui;jl+jiadc+pUa
zQ{!cRpP#N?D-0VGMcdQLP=vA)hN%pH#BC!R*cmde!_Vy3{P_JL;nrc{HJ49-ftJr>
z=bW9{Lp$fnUt)I0o+^xsLz^0i$t7!CqBqWtU#l`Q%^q1>><@oBbB2`vGlvEF7ggv~
z2p>8h(cYNX*qNI`dV(9w2GtvSYL>JNoe|p8Sz%Zhx`#~eW1eUrd*&{6+2eWaQxIGs
z<FUr7vpma>_P=IBj*~cQ0rJ*}5BV}$%ArNH9kQb)>?!4eS`?0|8(CDW5nnMaXviFr
z<Bw5ZgW6FQI<b4Obm=h<x`w7Ip~j3?ObuE^1!RXsaE{g5&}dON#y6?K33rzX&s3eO
zI&&d#HK!}-pt~Q*M%oimBRcMfsX#OPxbfT1o44eBNXT~x+$Qj20BxgR$vcF3VZR@x
zLQ?V9ZYU)>oE-`w=`%v!ByfWOg@C+8;5`B+J?tb(<vqM=w=a<-$UP-xllTpyI+OwO
zF1^pvyWLNawb?=M+h$QdAPV0$<{=+rjE--$X~dd`Cc|>@zF8{==_f$4PXQ*;T}Bkw
zc*Uw(WnRHd?^Q%OX1n2fyv{9NWy`!~c~%8cPy8Z&j&-_tJ@TQyMYW6`2gAiMG5-um
zo1LL6UB&(J1Kgo~Xd}}3*fRHN53M!UMwc{kZQjnTqZ-8#?u>6Yo!m3#CLTej@g}I`
z2>CMr&5z|Oy-;zvI=bN{AZ2p^j5{WY%Q)Peby~lE408E2cd2O7N1r)|4@4ny5z8z<
zJOkpx52xbjyMP$h$U=@0hq4dZ3P>37kaY@KMA)Pks-o$&CDRM7ZBxISWl5gPBgdSE
z49Me;LhmYk<T|}lOg63D3BMr1cMq>!^oK}&3r<Ec$8dvmuO3yerR#?`vP~}MAxJJS
zP-RTRZWIK%90c8-7$k&Og5U_BS~*8PgtYva0NsLoO5n;0gHZ@da{m%WMkNtJ_Fj3T
zIAf@^O*f3wAZUYy=grgRySfkrqSwN9F4D7qlBd{0H&p$kr;>Odm76f_<+FT@h1%+6
zNTZW1w}s+F7tjI8R9y)7GrC{e?qzIFGN1Pnbd3#su5NDSeRAQ7iq4v3pxe+SDX#PD
z@dz_FLx;(;+hqI<y^^Q((v=c2GayY+M42q#B>_jpFv{!gccPXuevr?Ijw%pp*R-eH
ptv{tDKj2Ya{3ySYPsp6C0pK7zD;9FpY9ON@A!`Y*b}4J=_g_qm<q`k@

delta 1874
zcmZ`(OK&4Z5bo}IcszCz$8q9Eym@WdT?ev)4Ud3~&_VzSp=_2t5LrT#S<kFvdt!Ur
zZIOtvEaa6qA#F4_Ua}EFf?t664@ewg#34uavUe^(Rgbd|By4qc)mK$rkFK7x{KMSz
zn^RLJf!`OO#H_z(Pp5PA?DX=d`;<7;(Y^dpp;C}M^A;<MfHALFDZWdbm}A@{j^SPy
zDwQR7*@-t9|HCbgiepk)ahDu(NGq%E>S%3DC~+@F8h0g7>H9Pu+@$-#PxRhk2_nA*
ztNSx0M35212o9?$K!KAxaIi~_+#?zF=Q!@@5!nMb@lzxo$*4yesNB&*82~`hRT@SC
zF@ebOaiDX@DB$@4;5i9!o(Rf#UX+kbuvQpTpIA&NK6TX3kynK3lZG;+{(xAT(0u|d
zv0}miIEJc5e8`@V%pqe0EaI?ChoRLYEm)R`NK8Dz5^R!nw4M^vL)>R-NK=jmbXM|d
zJj0sv$)Ev8HcWYV2B3MdFv^b6JO_Aglm`m9Oe&KHypcpCPqJjpD9Y$naL6rE(sv)g
z9TJY@!9VaT(a_{>x9#;rz1@1Jv>38hC)8}t?UT^B?s-oA8h5Llp;rP(NcXuW_ZpAk
zu62dYMX1)=w#Rj;ISl?%x3?<h&7RwH_jtX{L-oeq+q<FC>4`Az*<I0T*IS~*LbdC9
zcla#0{XqDYpssyc+yH^lGJT#Ju(nx}CaFm;u?$Pmps2t6QXKRKEIE_kLn%-q>0`%f
zU%5|@Kk$)f&Ded)4}b%V9|WK2*9OAGYozijd}YY`>hX8Jx<fwts;~LbdUWC~*pt38
z>7<q?jP&(>%-6+~PkhDKhDw(74d3u%O#;?95J_JvqmDGDC1?2B9ZW(Q&5TJ?=MN=6
zNI?+8fjMf-`fzY&az%GBq%!(n?T5stP{W2ugB^o8qzlAhO@`PWRzK$`kB~i)aa1uc
zZM7jC(yT1mCn_m}A$<kvvF7fk;84idT9`CThr_;=<CpcxC@1GD53hkV4bXfe%;1+;
zfzAYf#-1IlLG$@4k|L6IAfZxo1;2otZg)Cv%i(F%%}Jf@I6R3We#Q6#61+rS020RC
zR`1B=ws83~2<8z~Jh#PHkkdLGMkQ3bg7ZtLUm75+c5HFTmyuOE9?W0&u>El<&MzZE
zcRA)5vg3NiusyGOSoa)|8WmQn!)k=o=2yTXA|-hW@3ppp>Fxj_REK{K=FX&Pm_I{u
zCS9N@Y9u0<Eh#H3NXAdgGA-GUO>mV}l~kDt-vS_{$2^OQrz1+;?cn$Lt5gr>%=ch6
z<r`M>B&eBdH)NZnAHK7w-`MKMUE+%<;vJM&6b=jcPnb}H_w9<K;5YLsoepA&%{sm?
z{u&ZYkC&fzl38L)_!N+NSY3Q=(1JB{!$?RpG%V}LuD2|!1}gZSy-`{ZW0vK#tCkgN
zaHE|Qo({fFEN5N>b;#OXxWbce6mc{7HPJf==Wn?-uO3=kJ6AhiTUhW7^0s!ONx6t#
zs;D2iq29jTaH~QF=C30T!(dmm>-gn~>UaUrC+dz$2!2eir=NrGhd~XNssfe&e*q96
BrAz<-

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 69a2b147b6570e34ad999dd66d5eec4dc9af2cf2..cf4416e5392b32aa2f14647a93a0d6b25fa0b7cf 100644
GIT binary patch
delta 1599
zcmZWpO>7%Q6yBLxZ`K=o6WhrqaVUxNV`oE49Z*!osVb6GQBYA3#HFg$sBt`-#?HnG
zvkpomM?ECsgixbF2!w=6B`(}hPN+yo91sF7h!ZP%;lznU4;(o0-YiX!*wuXV=G!+j
zZ{GW6A76enWzQ#*mIlv*h4|f$?1|k^zB?%_O%-^O+sCA0@h;wdtW`RBil?DZ@C@&P
zKFNEz1HH{BcpvXSCMTZ`UK!%UKxMegb9@9Q_wZ3Z228K#@bN7wzUBE!pV#jVczs9Y
zh!BmF70sRe@{bSLZ4+WxfEL^VsB4FG|IN1cQ2R`~uB|Q}vKDF4HsNGIYcs@t?I>cX
zVgSz=rWinE?lJj`K5es5-|`NEkZo7@MOqHWuHIfG;(6Gr#sKiaz5|*H;x#bZlxth2
zu-jS#!chTG6@Uuk-|Z;J00`nUMiy8h4;$Xwsrn)d3yCy<%R;N(thaWnqPiW33&;=3
zxm0E-x#{hRdeExZ?r!Wg$KXR?0BFP{1`#>A7JHI%&sm@7MC9eNvE+;ZTV8aDNP*!}
zF%F&Rz(wUZMm%--d{P!C6;6LO-lb$juEj@5MIOf&$h7=Eeu)%M|B4^bB5ook0YaTa
zJfU8#uCGJN>O3T`hIG>xQ|R#$dXTcXh#Og^UvGMIbMx~{^6SJjnK^x$=qJ4xnZS&S
zQ3M@fOb*#^7$r1k<vaEUsmdqzIJqHN*ALknNY5d}076!8Zn(M_23!8wHCh&61WKn&
zCl<Um5w8PR!<F&DS!dOI9|kBBkX*V2)oIfsiV}iK(gwp(Fu|!@?Vjo^aoT1ZbVYl+
zBtPyR>}#Bj8@DvhqHnG)f9%dV%aChJ<yz!nhbY{VBq?Wd2F77*z3TgcD1z<ARO|vn
z3S+g+y3fVSIA<J^b|KdWGjISjW~#%O#9<Ce5St|c#Z7sb`u$P?zPmJ}FBVm0q0VJs
z1g1fB0{m@V^`TC+o$cLgAIkUAYb6W|bnq5>pcwu)Mce}RBWM8yFf`YWPBAO<nW6iy
z02i9udw#3F>mP)y+B^^|$QkSYPHiiQj*S%zjq2{M*W~{_XT^l$M@O9n7ZFFmV4p>S
zF^6X4;WZz?D9QoVLFx&TO_D63s@cS*^5@J}UMXS~7g8C=V1!BwJI}AmY36=nXFX_D
zYreNAJntbm1~z~OO0i)12*nyOHPi||1~jzQjb8!-w4&6~W=HxRdKf!oZC$Boq8$@s
zEz$s;v>7y=Q_$9RU`r9(V)ALvbA_9*h!U{CABe~RxJDEymLnt$r2<*(?a8S!p4Bqc
zYdb;f3vdeXZ=sPim5tt?$YXiMnRSMN5ibCQbiZDS)wuAQqO30ZCr+GvB|mrE^fKg`
zfR9U;qdqHtbFPi$aIVT8CQhkSpj?Hza(IbAZZm!5;zck`tOPS-rVg)Vc33gffHz|f
SSSc9ynBAsjb%6!{lYamObT=CS

delta 1340
zcmYjQ-)|d55Z<{vpRX7D9Q*t$ZB;vtlgI^<rlM^EsHkZawM_+-7$RAX!1kVt6X#;~
z4k<ylDwL=XJdrFfNT^E7BM+$Z3linELa6@$Cm{rX0ut}c+*O4=?YG~|&fe_I&OBcD
zY2I2+CQXLpU=;u8+v1T`gr`TdH|CSvI%Ex#r}#LoV?50>xF)#Gv$!VtG|%z;Av7#r
z;Kf6BbaG~XhL?C5RW_gG6*S5^IX<<mh#z^ik#`DC(a9gc0RZE$&T6wy{{C9sAlKJ%
zdJTjw8!CJ64cG~L#8%km%24&eSA6wE8vut1Hc$zR3?(I$5JO1~CB!1Tr#?a-@(=A&
zF&1ds&b}9@JIy_j!T*aGfi5etx3m=y=j3Jm_w+P**AW=>Km#!)tMMD@63X?}8i+KK
znj$K47@thPg2D^i(_~v6{SbekKuKOUry!6G^ExcZhvsWAH+pXFEBXxDi&?prJZsGp
zy?_uvU7VM9irG`WuIpU8w!FL|kCT_+%IKG55pv{Ms03vKjbKv#W8Kjgi7v_8sSa$)
zQED1)%HLAIPTnN-+XNI{pmyC(O$*Gf+x7cR(cJOm&*M|z%0I{L#dpx6O+TkW=5d^1
z`X3<iF|h;XK%uk`*p{-TlH`>+$k)>gV;4CL)Q+;wKD{XW=`;6RFXh$-<4UwwQ$H_x
zj#KV-$`oF?>-vuHoVM?kcUzv{Z1)^Ys~M(MOrg{0>J(cL6HWJ?6R17M4KzxOxDu*c
z&0fzFbI5Bku|{X;+Ex&2f70!7u|S#%X4ICiX3Wwaie46hDJEE8Kwiy50xUIwDC%+}
z^Os%4-I@|8a~DK~D2R1=+n#{0WXJya4Z0RS`3JbX+$K)7D#i#tL^#IjDF`~NSy2>A
z$Jx^TtEdW$ozHrHx8K_j)TX;H-Y1>j>g~3-y(sE1V7=MzJ1+l!RDprPMsY3SA#sX;
z9Mc_%lmEboEFAYsl)O9wQ#1@U0jK4|Yyv=jpWU7bi{dWsMK0+QB8*)9FN-e{_iio;
z-^k6}*)=LS8q^WQ!|GT?g4Ge#hk92TYQq>cBR2QUKob=owy-V+F`Oz_P`ixUMfogO
zt<>>o)OWXt*$6c~@=vJ`U`wX+S!^48zl_%2_5ANp?NPfj7zeqUe-0yQ7v9W=FJGfG
z%3ik-Yjfea_-@omRP1#0iTt8~j{#_e`3jc_`LuAg6jmC=9+pv9S0Zd2>If`~bH)1H
hd87s<Q8i+QhM#E|hK`?YOqgkuGDgZUO$!<J{=Z0W6hZ(1

diff --git a/Search-based Planning/Search_3D/bidirectional_Astar3D.py b/Search-based Planning/Search_3D/bidirectional_Astar3D.py
index e93ee6a..439c28d 100644
--- a/Search-based Planning/Search_3D/bidirectional_Astar3D.py	
+++ b/Search-based Planning/Search_3D/bidirectional_Astar3D.py	
@@ -13,26 +13,27 @@ 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, g_Space, Heuristic, getNearest, isCollide, cost, children
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children, heuristic_fun
 from Search_3D.plot_util3D import visualization
 import queue
 
 
 class Weighted_A_star(object):
     def __init__(self,resolution=0.5):
-        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.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         self.env = env(resolution = resolution)
-        self.g = g_Space(self) # key is the point, store g value
-        self.start, self.goal = getNearest(self.g,self.env.start), getNearest(self.g,self.env.goal)
-        self.g[self.start] = 0 # set g(x0) = 0
-        self.g[self.goal] = 0 # set g(x0) = 0
+        self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
+        self.g = {self.start:0,self.goal:0}
         self.OPEN1 = queue.QueuePrior() # store [point,priority]
         self.OPEN2 = queue.QueuePrior()
-        self.h1 = Heuristic(self.g,self.goal) # tree NO.1
-        self.h2 = Heuristic(self.g,self.start) # tree NO.2
         self.Parent1, self.Parent2 = {}, {}
         self.CLOSED1, self.CLOSED2 = set(), set()
         self.V = []
@@ -41,8 +42,8 @@ class Weighted_A_star(object):
 
     def run(self):
         x0, xt = self.start, self.goal
-        self.OPEN1.put(x0, self.g[x0] + self.h1[x0]) # item, priority = g + h
-        self.OPEN2.put(xt, self.g[xt] + self.h2[xt]) # item, priority = g + h
+        self.OPEN1.put(x0, self.g[x0] + heuristic_fun(self,x0,xt)) # item, priority = g + h
+        self.OPEN2.put(xt, self.g[xt] + heuristic_fun(self,xt,x0)) # item, priority = g + h
         self.ind = 0
         while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty
             xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() 
@@ -50,7 +51,7 @@ class Weighted_A_star(object):
             self.CLOSED2.add(xi2)
             self.V.append(xi1)
             self.V.append(xi2)
-            visualization(self)
+            # visualization(self)
             allchild1,  allchild2 = children(self,xi1), children(self,xi2)
             self.evaluation(allchild1,xi1,conf=1)
             self.evaluation(allchild2,xi2,conf=2)
@@ -66,26 +67,34 @@ class Weighted_A_star(object):
         for xj in allchild:
             if conf == 1:
                 if xj not in self.CLOSED1:
-                    gi, gj = self.g[xi], self.g[xj]
+                    if xj not in self.g:
+                        self.g[xj] = np.inf
+                    else:
+                        pass
+                    gi = self.g[xi]
                     a = gi + cost(self,xi,xj)
-                    if a < gj:
+                    if a < self.g[xj]:
                         self.g[xj] = a
                         self.Parent1[xj] = xi
                         if (a, xj) in self.OPEN1.enumerate():
-                            self.OPEN1.put(xj, a+1*self.h1[xj])
+                            self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))
                         else:
-                            self.OPEN1.put(xj, a+1*self.h1[xj])
+                            self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal))
             if conf == 2:
                 if xj not in self.CLOSED2:
-                    gi, gj = self.g[xi], self.g[xj]
+                    if xj not in self.g:
+                        self.g[xj] = np.inf
+                    else:
+                        pass
+                    gi = self.g[xi]
                     a = gi + cost(self,xi,xj)
-                    if a < gj:
+                    if a < self.g[xj]:
                         self.g[xj] = a
                         self.Parent2[xj] = xi
                         if (a, xj) in self.OPEN2.enumerate():
-                            self.OPEN2.put(xj, a+1*self.h2[xj])
+                            self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
                         else:
-                            self.OPEN2.put(xj, a+1*self.h2[xj])
+                            self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
             
     def path(self):
         # TODO: fix path
diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py
index c6f7c44..636d756 100644
--- a/Search-based Planning/Search_3D/utils3D.py	
+++ b/Search-based Planning/Search_3D/utils3D.py	
@@ -30,9 +30,13 @@ def Heuristic(Space, t):
     '''Max norm distance'''
     h = {}
     for k in Space.keys():
-        h[k] = max(abs(np.array([t[0] - k[0], t[1] - k[1], t[2] - k[2]])))
+        h[k] = max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
     return h
 
+def heuristic_fun(initparams, k, t=None):
+    if t is None:
+        t = initparams.goal
+    return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
 
 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]:
@@ -117,9 +121,11 @@ def g_Space(initparams):
     return g
 
 
-def isCollide(initparams, x, child):
+def isCollide(initparams, x, child, dist):
     '''see if line intersects obstacle'''
-    dist = getDist(x, child)
+    '''specified for expansion in A* 3D lookup table'''
+    if dist==None:
+        dist = getDist(x, child)
     if not isinbound(initparams.env.boundary, child): 
         return True, dist
     for i in range(len(initparams.env.AABB)):
@@ -147,6 +153,7 @@ def children(initparams, x):
             continue
         if isinbound(initparams.env.boundary, child):
             allchild.append(child)
+            # initparams.Alldirec[direc]*resolution
     return allchild
 
 
@@ -160,8 +167,9 @@ def obstacleFree(initparams, x):
     return True
 
 
-def cost(initparams, i, j, settings=0):
-    collide, dist = isCollide(initparams, i, j)
+def cost(initparams, i, j, dist=None, settings=0):
+    collide, dist = isCollide(initparams, i, j, dist)
+    # collide, dist= False, getDist(i, j)
     if settings == 0:
         if collide:
             return np.inf