'utils3D.py'

Search-based Planning/Search_3D/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 getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
-    cost
+    cost, children, StateSpace
 from Search_3D.plot_util3D import visualization
 import queue
 import time
@@ -27,6 +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.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)
@@ -44,13 +45,13 @@ class Weighted_A_star(object):
         self.OPEN.put(self.x0, self.g[self.x0] + self.h[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 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
@@ -60,12 +61,12 @@ class Weighted_A_star(object):
             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 = self.children(xi)
+            # 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(xi, xj)
+                    a = gi + cost(self, xi, xj)
                     if a < gj:
                         self.g[xj] = a
                         self.Parent[xj] = xi
@@ -87,9 +88,9 @@ class Weighted_A_star(object):
         if xt in self.CLOSED:
             self.done = True
             self.Path = self.path()
-            if N is None:
-                visualization(self)
-                plt.show()
+            # if N is None:
+            #     visualization(self)
+            #     plt.show()
             return True
 
         return False
@@ -118,6 +119,6 @@ class Weighted_A_star(object):
 
 if __name__ == '__main__':
     sta = time.time()
-    Astar = Weighted_A_star(1)
+    Astar = Weighted_A_star(0.5)
     Astar.run()
     print(time.time() - sta)

Search-based Planning/Search_3D/Dstar3D.py

@@ -3,64 +3,14 @@ import matplotlib.pyplot as plt
 
 import os
 import sys
-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 StateSpace, getDist, getRay, isinbound, isinball
+from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, initcost
 import pyrr
 
 
-
-def isCollide(initparams, x, child):
-    '''see if line intersects obstacle'''
-    ray , dist = getRay(x, child) ,  getDist(x, child)
-    if not isinbound(initparams.env.boundary,child):
-        return True, dist
-    for i in initparams.env.AABB:
-        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, dist
-    for i in initparams.env.balls:
-        if isinball(i, child):
-            return True, dist
-        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, dist
-    return False, dist
-
-def children(initparams, x):
-    # get the neighbor of a specific state
-    allchild = []
-    resolution = initparams.env.resolution
-    for direc in initparams.Alldirec:
-        child = tuple(map(np.add,x,np.multiply(direc,resolution)))
-        if isinbound(initparams.env.boundary,child):
-            allchild.append(child)
-    return allchild
-
-def cost(initparams, x, y):
-    # get the cost between two points, 
-    # do collision check here
-    collide, dist = isCollide(initparams,x,y)
-    if collide: return np.inf
-    else: return dist
-
-def initcost(initparams):
-    # initialize cost dictionary, could be modifed lateron
-    c = defaultdict(lambda: defaultdict(dict)) # two key dicionary
-    for xi in initparams.X:
-        cdren = children(initparams, xi)
-        for child in cdren:
-            c[xi][child] = cost(initparams, xi, child)
-    return c
-    
-
 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],

Search-based Planning/Search_3D/LRT_Astar3D.py

@@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base
 from Search_3D.env3D import env
 from Search_3D import Astar3D
 from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
-    cost, obstacleFree
+    cost, obstacleFree, children
 from Search_3D.plot_util3D import visualization
 import queue
 
@@ -34,8 +34,8 @@ class LRT_A_star2:
             for xi in self.Astar.CLOSED:
                 lasthvals.append(self.Astar.h[xi])
                 # update h values if they are smaller
-                Children = self.Astar.children(xi)
-                minfval = min([cost(xi, xj, settings=0) + self.Astar.h[xj] for xj in Children])
+                Children = children(self.Astar,xi)
+                minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children])
                 # h(s) = h(s') if h(s) > c(s,s') + h(s') 
                 if self.Astar.h[xi] >= minfval:
                     self.Astar.h[xi] = minfval
@@ -47,9 +47,13 @@ class LRT_A_star2:
         ind = 0
         # find the lowest path down hill
         while st in self.Astar.CLOSED:  # when minchild in CLOSED then continue, when minchild in OPEN, stop
-            Children = [i for i in self.Astar.children(st)]
+            Children = children(self.Astar,st)
             minh, minchild = np.inf, None
             for child in Children:
+                # check collision here, not a supper efficient
+                collide, _ = isCollide(self.Astar,st, child)
+                if collide:
+                    continue
                 h = self.Astar.h[child]
                 if h <= minh:
                     minh, minchild = h, child
@@ -76,5 +80,5 @@ class LRT_A_star2:
 
 
 if __name__ == '__main__':
-    T = LRT_A_star2(resolution=0.5, N=150)
+    T = LRT_A_star2(resolution=0.5, N=100)
     T.run()

Search-based Planning/Search_3D/RTA_Astar3D.py

@@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base
 from Search_3D.env3D import env
 from Search_3D import Astar3D
 from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
-    cost, obstacleFree
+    cost, obstacleFree, children
 from Search_3D.plot_util3D import visualization
 import queue
 
@@ -48,7 +48,7 @@ class RTA_A_star:
         while sthval < maxhval:
             parentsvals , parents = [] , []
             # find the max child
-            for xi in self.Astar.children(st):
+            for xi in children(self.Astar,st):
                 if xi in self.Astar.CLOSED:
                     parents.append(xi)
                     parentsvals.append(self.Astar.h[xi])

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 getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost, children
 from Search_3D.plot_util3D import visualization
 import queue
 
@@ -39,14 +39,6 @@ class Weighted_A_star(object):
         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, xt = self.start, self.goal
         self.OPEN1.put(x0, self.g[x0] + self.h1[x0]) # item, priority = g + h
@@ -59,7 +51,7 @@ class Weighted_A_star(object):
             self.V.append(xi1)
             self.V.append(xi2)
             visualization(self)
-            allchild1,  allchild2 = self.children(xi1), self.children(xi2)
+            allchild1,  allchild2 = children(self,xi1), children(self,xi2)
             self.evaluation(allchild1,xi1,conf=1)
             self.evaluation(allchild2,xi2,conf=2)
             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
@@ -75,7 +67,7 @@ class Weighted_A_star(object):
             if conf == 1:
                 if xj not in self.CLOSED1:
                     gi, gj = self.g[xi], self.g[xj]
-                    a = gi + cost(xi,xj)
+                    a = gi + cost(self,xi,xj)
                     if a < gj:
                         self.g[xj] = a
                         self.Parent1[xj] = xi
@@ -86,7 +78,7 @@ class Weighted_A_star(object):
             if conf == 2:
                 if xj not in self.CLOSED2:
                     gi, gj = self.g[xi], self.g[xj]
-                    a = gi + cost(xi,xj)
+                    a = gi + cost(self,xi,xj)
                     if a < gj:
                         self.g[xj] = a
                         self.Parent2[xj] = xi

Search-based Planning/Search_3D/utils3D.py

@@ -1,5 +1,6 @@
 import numpy as np
 import pyrr
+from collections import defaultdict
 
 def getRay(x, y):
     direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
@@ -43,6 +44,31 @@ def isinball(i, x):
         return True
     return False
 
+def lineSphere(p0,p1,ball):
+    # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf
+    c, r= ball[0:3],ball[-1]
+    line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]]
+    d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]]
+    t = (1 / (line[0]*line[0] + line[1]*line[1] + line[2]*line[2])) * (line[0]*d1[0] + line[1]*d1[1] + line[2]*d1[2])
+    if t <= 0: 
+        if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True
+    elif t >= 1: 
+        d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]]
+        if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True
+    elif 0 < t < 1: 
+        x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]]
+        k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]]
+        if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r**2: return True
+    return False
+    
+def lineAABB(p0,p1,dist,AABB):
+    #https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
+    P = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point
+    D = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction
+    t = dist / 2 # radius
+    # TODO: implement this
+
+
 def StateSpace(env, factor = 0):
     boundary = env.boundary
     resolution = env.resolution
@@ -68,28 +94,37 @@ def g_Space(initparams):
         g[v] = np.inf # this hashmap initialize all g values at inf
     return g
 
-def isCollide(initparams, x, direc):
+def isCollide(initparams, x, child):
     '''see if line intersects obstacle'''
-    resolution = initparams.env.resolution
-    child = tuple(map(np.add,x,np.multiply(direc,resolution)))
     ray , dist = getRay(x, child) ,  getDist(x, child)
     if not isinbound(initparams.env.boundary,child):
-        return True, child
+        return True, dist
     for i in initparams.env.AABB:
         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
+                return True, dist
     for i in initparams.env.balls:
         if isinball(i, child):
-            return True, child
-        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
+            return True, dist
+        # 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, dist
+        if lineSphere(x, child, i): return True, dist
+    return False, dist
+
+def children(initparams, x):
+    # get the neighbor of a specific state
+    allchild = []
+    resolution = initparams.env.resolution
+    for direc in initparams.Alldirec:
+        child = tuple(map(np.add,x,np.multiply(direc,resolution)))
+        if isinbound(initparams.env.boundary,child):
+            allchild.append(child)
+    return allchild
 
 def obstacleFree(initparams,x):
     for i in initparams.env.blocks:
@@ -100,11 +135,24 @@ def obstacleFree(initparams,x):
             return False
     return True
 
-def cost(i,j,settings=0):
+def cost(initparams, i,j,settings=0):
+    collide, dist = isCollide(initparams,i,j)
     if settings == 0:
-        return getDist(i,j)
+        if collide: return np.inf
+        else: return dist
     if settings == 1:
-        return getManDist(i,j)
+        if collide: return np.inf
+        else: return getManDist(i,j)
+
+def initcost(initparams):
+    # initialize cost dictionary, could be modifed lateron
+    c = defaultdict(lambda: defaultdict(dict)) # two key dicionary
+    for xi in initparams.X:
+        cdren = children(initparams, xi)
+        for child in cdren:
+            c[xi][child] = cost(initparams, xi, child)
+    return c
     
 if __name__ == "__main__":
-    from env3D import env
+    a = [10,2.5,1,1]
+    lineAABB([0,0,0],[1,1,1],)