saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-30 00:50:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated plotting

Browse Source
This commit is contained in:
391311qy
2020-06-25 18:18:33 -07:00
parent 815ad78bb5
commit c398cefee7
7 changed files with 77 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Sampling-based Planning/rrt_3D/utils3D.py
+8 -2
View File
@@ -53,11 +53,17 @@ def isinside(initparams, x):
return True
return False
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 isCollide(initparams, x, y):
'''see if line intersects obstacle'''
ray = getRay(x, y)
dist = getDist(x, y)
if not isinbound(initparams.env.boundary,y):
return True
for i in getAABB(initparams.env.blocks):
shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
if shot is not None:
@@ -67,8 +73,8 @@ def isCollide(initparams, x, y):
for i in initparams.env.balls:
shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)
if shot != []:
dists_wall = [getDist(x, j) for j in shot]
if all(dists_wall <= dist): # collide
dists_ball = [getDist(x, j) for j in shot]
if all(dists_ball <= dist): # collide
return True
return False