From 6fdc623a087e2c6f575eacf7fdbb4b9e7274ee36 Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Sun, 5 Jul 2020 21:01:14 -0700
Subject: [PATCH] init Dstar

---
 Search-based Planning/Search_3D/Dstar3D.py    |  59 ++++++++++++++++++
 .../__pycache__/Astar3D.cpython-37.pyc        | Bin 3456 -> 3456 bytes
 Search-based Planning/Search_3D/env3D.py      |   1 -
 3 files changed, 59 insertions(+), 1 deletion(-)
 create mode 100644 Search-based Planning/Search_3D/Dstar3D.py

diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py
new file mode 100644
index 0000000..a1a31e9
--- /dev/null
+++ b/Search-based Planning/Search_3D/Dstar3D.py	
@@ -0,0 +1,59 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+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 import Astar3D
+from Search_3D.utils3D import getDist, getRay
+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 = {}
+    for x in xarr:
+        for y in yarr:
+            for z in zarr:
+                g[(x,y,z)] = np.inf
+    return g
+
+def Heuristic(initparams,x):
+    h = {}
+    x = np.array(x)
+    for xi in initparams.g.keys():
+        h[xi] = max(abs(x-np.array(xi)))
+    return h
+
+def getNearest(Space,pt):
+    '''get the nearest point on the grid'''
+    mindis,minpt = 1000,None
+    for pts in Space.keys(): 
+        dis = getDist(pts,pt)
+        if dis < mindis:
+            mindis,minpt = dis,pts
+    return minpt
+
+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.env = env(resolution = resolution)
+        self.g = StateSpace(self.env)
+        self.x0, self.xt = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
+        self.h = Heuristic(self,self.x0) # getting heuristic for x0
+        
+if __name__ == '__main__':
+    D = D_star(1)
+    print(D.h[D.x0])
\ No newline at end of file
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 fa681606d959ce113c94e6cf45c70f72c73600a9..0ef65629ce7fc550d8bf0a63cf6c4a0df88a26bf 100644
GIT binary patch
delta 86
zcmZpWZjk16;^pOH0D`$8|Kb`p@^W)BMs1elbYo()nVi7gz-<SVDl!KV4wGMV?`Mpf
hyqd=zNWSJV5po0y-(oH<NsMB31d>HXc9RWx{Q;b37Y6_U

delta 86
zcmZpWZjk16;^pOH0D_{1zj4hQdAT_mZ*7+3bYo()ot(hkz-<qbG6xZklV5Z1XS_9e
hHIF@ze9dDb<OCGH#avvHc#G8$NEQ{@Pd4QB2LR%^84mye

diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py
index 754e208..7ddd2e6 100644
--- a/Search-based Planning/Search_3D/env3D.py	
+++ b/Search-based Planning/Search_3D/env3D.py	
@@ -54,7 +54,6 @@ class env():
         self.blocks = np.vstack([self.blocks,newblock])
         self.AABB = getAABB(self.blocks)
 
-
 if __name__ == '__main__':
     newenv = env()
     print(newenv.balls)