From a7dda7594494252d31d4475ede783a14c968b07f Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Thu, 9 Jul 2020 11:32:11 -0700
Subject: [PATCH] 'collision_detection'

---
 .../__pycache__/env3D.cpython-37.pyc          | Bin 2361 -> 2917 bytes
 .../__pycache__/utils3D.cpython-37.pyc        | Bin 5537 -> 6184 bytes
 Search-based Planning/Search_3D/env3D.py      |  14 ++++-
 Search-based Planning/Search_3D/utils3D.py    |  50 ++++++++++++------
 4 files changed, 46 insertions(+), 18 deletions(-)

diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
index 018f3217116e9323a5e725b0fd03c86872a81f07..c6679f6afab2c14438e66e36e045a554f207dd7b 100644
GIT binary patch
delta 984
zcmZWnOKVd>6rOXRliNJnnC8*6Cbp^N)(2w2iWF(9q6-NH1>r)uq?y~8+NO4HTD7E&
zkp6)(e?Yf_3zr4AZrr*HxDi}r;V*FGIk#9U4xDe!`R05x-<`Q9>7Uu;aw6d{96xHd
ze;7MX=Hc*orqq4Nge~|H6Fe|`x@U=)aE_Q~iy;xmnTv!-ij)`@BO=|f)<IklnIrIG
zs2deo)Ma~`=LAC{_d=8VfzxANJV*rbjbsmi3GE4U^ZO&tVr{qmd|Tcrzu67SJ56Um
zR;qRO>EnXlrCh^U#0WD80rNmGPZJ<CG@%O}w=w!gt+uvSr%oC)n;7>n!VW?Q1QZCh
z0?RR>7b!O)#yrPZ5u^WQ8hW?G{C~vx|HNm^nfR1x=%_`jXwR<7B$|+bFd%OOgCHu&
zrmOW~r4J8qHo{<YU8a$g)TkcWnbIXpLQ*hTfc*r0>2=?4wwi6<ueyeM%BNvmb$Px^
z7gWXwNy|Y}U8iGc=on|0Qo*9Z4&e1zhuvqZ=>x3;?^qi;S`Xgi>+Cqs@Vn7FR9#KR
zushn!)#V7~k-<1+bkZr9Os&{Pu@SVFmX?=qPUAwz5twFx18I<D^<Ce~$Sf&F5eD?r
z$sB_E0JG}Yn1vhav(Xs6ic}c=k&p)x=Rr=X+veNE9Ieoi3}i)pF(1ce9t)Ww$f*hI
z+fbR($lsLuZ52#eL_&?((}fvYIqw(CapdOJs=Wyn_0^t)qWWc5U{R&{wJVpY|J?Ri
zx~7~U_98)vfHshGbo-O^#kVk`x8mT$Br$#N`&+eU%lF-sI^m`HzO}u!5!Bn!g~~Fv
z$SDGPl1LF9;aRQVRnXe&n=fUv)s`+Ek#qZeVQX9LZU%QP><;NF4d6OYaeLV0Hn;P7
IGR47yKj#OnA^-pY

delta 427
zcmYL^Jx{_=6oz~HdG9Tywg_pf0#T5luozdPaWOH(#evwR(cD6c$WZ(NCNQ{?8~=ox
zqrbwzjoqPxqfQR?;NTseoco@S_neQ)t6My^EGFge>R5?>j78Cd!J<{`af6e&6l$Du
z#!X(}7B3F<0dSkI%^}p!B<^r`E^&7zhek|z=~hl}Vq+$SG-fd!nKJ;1%a>An{pGo@
zh(I4XnVK7AG6Fp_5Knp^8sbwQmg~7NU2Os$sK9cjzDSG*tA!bCk{}Jlx6vzPTK-9G
zgeNY@yII1}MfjpaeSN7;<MBY8(`L1bDt7+?JC`Jil1XwOMf>8Bjv)|VbQ^Yr#~N@b
zj##(6iS=8EKa)IaYvMp_p6sF6LHy5LMXQxI@&r~)@lXPcrl_Fb-E}gFqIN;Nv6_?m
ixtazD%%@%9m?fx-rny_eD^wr~v57$pPbu02bod3oUrrMM

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 698ea5248d79ebb85e8bcd59e481ac04a8e6aa21..7f432dffadc2adc6b921888ee680d948a6977ff9 100644
GIT binary patch
delta 1734
zcmZ`(O>Y}T7@pakU9X)u&4*2#q>$32N!CRzR1m5P(j+vMxU~5aNh~4D@wm+<8^@j9
zNDW%c2M4tmM610aaX_eu6B0t)_yGuU=CB7Y{0$B~?`#~x0k7tnnP;B&W8RtZH`l*^
zdv-aW&oTUVb~DvKN{szQlitt7U=c0&{pD<HXH=F>S>=*+<(M2l<&{A>Auod&lCR4*
z&=1Q=c?JE5d{bUUUyx-vC8wb*%4>24%xjHNIeTEJU*%k-)EH}wH%eXJ<v;%YEXR%b
zv~Pxs_=QvG+%_1CnB-$D;)J728WY5rngO0OhGu{%^Vn>Q)F!nrv4||Ww$%3L3|YeO
z7)u#gT9H9<+8sMG_gOmw<ZUO)=tKk|q#5l>5b7DZ5vd!v$c9FgC1saKj<&PGSSeFL
zk_STlDa8iUI{45!+E%L5OeYn-;L16dB8-+&xKyf{bLA~kA|xE>fM=^<Fw%^6P5O4O
z7aWOUwZM=vdd#3Huu_LY<qiZAcz)&<JNh5DdaoDUrjEdBx9EF8O8<J*6-euIF|}U$
zor?uK%QkhS^Fy}iQw_51xi!?gl&rKim}@^!9QRfU0=mWbH(fK~s|jCG!<f2#>IN;{
z1SCRwL70eIZFe^@4?UR_`NxT|=c{>=yiXFososfKi-o(s*K8~;EiW&E4c-DU;|{;W
z9d6^hVN5f@XVPZO@iG(tLACJHcYP7J@4`$i#&h{cd?)@ge~mwkf6o7AKO}l3{(NAe
z<6#)+cWDZq!#BXyCb&B7g?<*AjrI{*@Ej20l1E0@XqzFA*uJrE(k!?ld1O8^HrUe{
z&5C>M)A6<lEmB8jB)USCdzvTZE%tOCx(|(g8^SV#nMjZ$wbv7cjy4rtQ?2*7C+k~y
zNogS0j9*>4x_aNusv+d1t`n>f(6$pn)l&;Jw03L0A1JD#dJo`QYK_Jjy;$G#e5q~|
zF%5_-gX2>Qg8+Y0s5(l*96n$VuznOL#~iyKBeflWJ$P*EaEoziXma@;#EElw?1x^<
zKS{IEiP*JPtI?FMeIclWgwnW+FjHBAEjsu3iy>$18K%JifEfZO-p|Jo83$91e;D#6
zDQ05t`bYJHAPw^g7`;2!RBQ44aGAf1Yr~_w7M~0UtJ}0n$Ea2bMgYBBXuk>HuQ)*?
z487*#ppvN{`MxI`X+7v-C=Gq26!oJZ{3`xnWP;b@?U7&jtGHOWnXi(n4xqZmYlQ;W
zTUqy-UPu<-VkzhcFry!vp<8yDE4q1!Q>`A>yk@ofnK9?OT)(AWH<4J)<HOd8&b=Zq
zP;pXNS)wCV`U=xUReF0mg&IvMk8|J6+F_$wO?WM_TFPtU?fH0t@q^;jEqav>71wZt
cW8%v?8Qois<=_l*gJ6nI&dKJo7;(Y>0kH2<TL1t6

delta 1142
zcmYjQO>7)B6t?GYz0=Khv%ASAX-OcYO*f6yAi?H9g^~cF60}fpXf-Mt&5ozrN%pU{
zBQ|JOIcy~^s1mY-1Sfhx;)H}aaDf9?&ON}~fD7C>bK*TOi)zc?v!DIm`=0&$_0E&4
zn+J1qK7sbm?_RiFC*%)Y%$f_6Js9$rPdA52lQ;O%BO09N%Y5Y#2`c;ozX)=kKgU-=
zR{0u#9%PMg@fY|yoUHQ~`37hfl0|;$*b-0qWxmO;@GBiBXe4D$uy<_KfX+@D6Z_l0
zKlUlrqjQ(P`=UIeUJJsDUA<JfPxtj%<q|#AUsZng4pDtwpUl5@b{h$r9BhaB(92Z3
z4Z7F{z{X%;Ca=Sg-vB6*(af4yicY0P#`f4zRz{BOBL{cnHm8|$-+G^n_KenjgM9Gv
z*j3(9;Y*g;nft^Vdzr_r<G3hV#*sI%#)YhqImcAAX66rDcgUz^Tol2@7P#;;7ek20
zvoq=((#YJ2Ek-ll!_ghW?J0$(qX*UXvs;JFk|+UvF^_N+;W`3dQSR*T9~|V)@KlK4
zd?y+75+%|&REbnFsEX(`O#3PkGKp0fMeTO(wxe!Wif#1p3P6*I8#phZH#dHec6nff
zUGDHyDzO2zK>|lU7^t9#n(#xg*S}R)ngQ&{RRCfw!sRm<h(3%G%s_f2QljFvUag&Y
zhVridyta1mDwyP~chXhquzNbq$Ry)M!(r0rP45{>48j>!Vi(Lr5#e3^Q@y6=>c!<D
zEM*yhSdNLiVpxc~`dU3*!<f0(?hfK(ISq3QG$!|^EpF)V>s!kofoAGq%K1MOf&Qm1
zcd^I>7Lj-hp$0Hxw*m8<9tBJ$N~Qgd3<~j}+f8{Q-a~$#6Az@C=pPp@(0lsVg&*i+
z{n_HR^}Fck9y)TTY3`sSYm^3V%tg|lW*4!vpDOh532aMze`1y3VzJUMQqe{u7KXhj
z?T6uC_zyaGh7e~@;16h0L7x#zx!XVKL7A9jZo-bf*?99M^Plk72$QMdhVN<!{vV_t
u^^!2mX(W7e^M5wVJ6J#ruiRn|v!NGR!AO_6tm;=_y~uo4^ouY9-v0vBz4rJ3

diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py
index 7ddd2e6..12b18ab 100644
--- a/Search-based Planning/Search_3D/env3D.py	
+++ b/Search-based Planning/Search_3D/env3D.py	
@@ -29,6 +29,18 @@ def getAABB(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
 
+class aabb(object):
+    def __init__(self,AABB):
+        self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point
+        self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents
+
+def getAABB2(blocks):
+    # used in lineAABB
+    AABB = []
+    for i in blocks:
+        AABB.append(aabb(i))
+    return AABB
+
 def getballs():
     spheres = [[16,2.5,4,2],[10,2.5,1,1]]
     Obstacles = []
@@ -44,7 +56,7 @@ class env():
         self.resolution = resolution
         self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
         self.blocks = getblocks()
-        self.AABB = getAABB(self.blocks)
+        self.AABB = getAABB2(self.blocks)
         self.balls = getballs()
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py
index f91fe6c..c4c1820 100644
--- a/Search-based Planning/Search_3D/utils3D.py	
+++ b/Search-based Planning/Search_3D/utils3D.py	
@@ -61,13 +61,30 @@ def lineSphere(p0,p1,ball):
         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):
+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
+    mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point
+    I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction
+    hl = dist / 2 # radius
+    P = aabb.P#center of the AABB
+    E = aabb.E# extents of AABB
+    T = [P[0] - mid[0], P[1] - mid[1], P[2] - mid[2]]
+    # do any of the principal axis form a separting axis?
+    if abs(T[0]) > (E[0] + hl * abs(I[0])): return False
+    if abs(T[1]) > (E[1] + hl * abs(I[1])): return False
+    if abs(T[2]) > (E[2] + hl * abs(I[2])): return False
+    # I.cross(x axis) ?
+    r = E[1] * abs(I[2]) + E[2] * abs(I[1])
+    if abs(T[1] * I[2] - T[2] * I[1]) > r: return False
+    # I.cross(y axis) ?
+    r = E[0] * abs(I[2]) + E[2] * abs(I[0])
+    if abs(T[2] * I[0] - T[0] * I[2]) > r: return False
+    # I.cross(z axis) ?
+    r = E[0] * abs(I[1]) + E[1] * abs(I[0])
+    if abs(T[0] * I[1] - T[1] * I[0]) > r: return False
 
+    return True
+    
 
 def StateSpace(env, factor = 0):
     boundary = env.boundary
@@ -96,18 +113,17 @@ def g_Space(initparams):
 
 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
+    dist = 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
+        # 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
+        if lineAABB(x, child, dist, i):return True, dist
     for i in initparams.env.balls:
-        if isinball(i, child):
-            return True, dist
+        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]
@@ -154,5 +170,5 @@ def initcost(initparams):
     return c
     
 if __name__ == "__main__":
-    a = [10,2.5,1,1]
-    lineAABB([0,0,0],[1,1,1],)
\ No newline at end of file
+    a = '()'
+    print(list(a))
\ No newline at end of file