saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-30 09:00:47 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
36249933db5cf3f262ace925e17af37da2dff9df
PathPlanning/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
T

181 lines
11 KiB
Plaintext
Raw Normal View History

yq edited A*3D
2020-06-27 09:58:58 -07:00
B
'update'
2020-07-26 14:44:35 -07:00
ñ_@4ã
@sšddlZddlZddlmZddlZdd„Zdd„Zdd„Zd d
'OBB'
2020-07-17 22:30:56 -07:00
„Z d d „Z
'update'
2020-07-26 14:44:35 -07:00
dCd
'rrt'
2020-07-25 23:58:53 -07:00
d„Z dDdd„Z dEdd„Z
'utils3D.py'
2020-07-09 03:01:40 -07:00
dFdd„Zdd„Zdd„Zdd„Zdd„Zdd„ZdGd d!„Zd"d#„Zd$d%„ZdHd&d'„Zd(d)„ZdId+d,„Zd-d.„Zed/k�r–ddlZdd0lmZmZed1d2d3gd4d5d5geddd6ƒƒZ d7d2d3gd8d2d3gZ!Z"e #d3d5d9gd:d;d<gd=d>d?gd5d5d5gd3d3d3gd9d9d9gg¡Z$e ¡Z%e e e$d@ddAdB�e&e ¡e%ƒdS)JéN)Ú defaultdictcCs<|d|d|d|d|d|dg}t ||g¡S)Nréé)ÚnpÚarray)ÚxÚyÚdirec©r
'OBB'
2020-07-17 22:30:56 -07:00
ú‡c:\Users\39131\Desktop\github\path-planning-algorithms\Search-based Planning\Search_3D/../../Search-based Planning\Search_3D\utils3D.pyÚgetRays.r cCsDt t|d|dd|d|dd|d|ddgƒ¡S)Nrrr)rÚsqrtÚsum)Úpos1Úpos2r
'utils3D.py'
2020-07-09 03:01:40 -07:00
r
r ÚgetDist srcCs>tt|d|dƒt|d|dƒt|d|dƒgƒS)Nrrr)rÚabs)rrr
'OBB'
2020-07-17 22:30:56 -07:00
r
'utils3D.py'
2020-07-09 03:01:40 -07:00
r Ú
getManDistsrcCs6d\}}x(|D] }t||ƒ}||kr||}}qW|S)z!get the nearest point on the grid)ièN)r)ÚSpaceÚptZmindisZminptÚptsÚdisr
'OBB'
2020-07-17 22:30:56 -07:00
r
'changed'
2020-07-09 00:46:50 -07:00
r Ú
'update'
2020-07-10 21:52:57 -07:00
getNearests 
'utils3D.py'
2020-07-09 03:01:40 -07:00

'OBB'
2020-07-17 22:30:56 -07:00
rcCs\i}xR| ¡D]F}tt|d|dƒt|d|dƒt|d|dƒgƒ||<qW|S)zMax norm distancerrr)ÚkeysÚmaxr)rÚtÚhÚkr
'update'
2020-07-10 21:52:57 -07:00
r
r Ú HeuristicsFrcCsL|dkr|j}tt|d|dƒt|d|dƒt|d|dƒgƒS)Nrrr)Úgoalrr)Ú
'update'
2020-07-26 14:44:35 -07:00
initparamsrrr
'utils3D.py'
2020-07-09 03:01:40 -07:00
r
'update'
2020-07-26 14:44:35 -07:00
r Ú
'utils3D.py'
2020-07-09 03:01:40 -07:00
heuristic_fun%sr!FcCs\|dkrt|||ƒS|rÌ|d||dd…dfk|dd…df|d|k@}|d||dd…dfk|dd…df|d|k@}|d||dd…dfk|dd…df|d|k@}||@|@S|d||dkoò|d|kn�oV|d||dk�o"|d|kn�oV|d||dk�oR|d|kSSdS)NÚobbréréré)Úisinobb)ÚirÚmodeÚfactorÚisarrayZcompxZcompyZcompzr
'update'
2020-07-26 14:44:35 -07:00
r
r Ú isinbound*s 888 r+cCs&t|dd…|ƒ|d|kr"dSdS)Nrr#TF)r)r'rr)r
r
r Úisinball5sr,cCsä|r„|jt |t t|ƒ¡f¡jjdd…dd…f}|jd |jd |jd |jd
|jd
|jd
g}t|||d�S|jt |d¡}|jd |jd |jd |jd
'OBB'
2020-07-17 22:30:56 -07:00
|jd
'update'
2020-07-26 14:44:35 -07:00
|jd
'OBB'
2020-07-17 22:30:56 -07:00
g}t||ƒSdS)Nrr#rr)r*)ÚTrÚ column_stackÚonesÚlenÚEr+Úappend)r'rr*rÚblockrr
'update'
2020-07-26 14:44:35 -07:00
r
r r&:s2@@r&c Cs8t |¡}|j}|j}|j}|dt|ddƒ|dt|ddƒ|dt|ddƒ}|dt|ddƒ|dt|ddƒ|dt|ddƒ}|dt|ddƒ|dt|ddƒ|dt|ddƒ}t |||g¡}||_||_t dddgdddgdddgg¡|_|S)Nrrr)ÚcopyÚdeepcopyÚPr1ÚOrrr) r"Úaabbr6ÚaÚAZExZEyZEzr1r
'utils3D.py'
2020-07-09 03:01:40 -07:00
r
r ÚOBB2AABBEs
HHH$r;c Cs`|dd…|d}}|d|d|d|d|d|dg}|d|d|d|d|d|dg}d|d|d|d|d|d|d|d|d|d|d|d|d}|dk�r"|d|d|d|d|d|d|dk�r\dS�n:|dk�rš|d|d|d|d|d|dg}|d|d|d|d|d|d|dk�r\dSnÂd|k�r²dk�r\nn¦|d||d|d||d|d||dg} |d| d|d| d|d| dg}
|
d|
d|
'OBB'
2020-07-17 22:30:56 -07:00
d|
'update'
2020-07-10 21:52:57 -07:00
d|
'utils3D.py'
2020-07-09 03:01:40 -07:00
d|
d|dk�r\dSdS)Nrr#éÿÿÿÿrrTFr
'update'
2020-07-26 14:44:35 -07:00
) Úp0Úp1ZballÚcÚrÚlineÚd1rÚd2rrr
'utils3D.py'
2020-07-09 03:01:40 -07:00
r
'update'
2020-07-26 14:44:35 -07:00
r Ú
'OBB'
2020-07-17 22:30:56 -07:00
lineSphereUs$..22
'update'
2020-07-26 14:44:35 -07:00
:
'OBB'
2020-07-17 22:30:56 -07:00
.::.:rDc CsD|d|dd|d|dd|d|ddg}|d|d||d|d||d|d|g}|d}|jd|d|jd|d|jd|dg}t|dƒ|jd|t|dƒkrÚdSt|dƒ|jd|t|dƒk�rdSt|dƒ|jd|t|dƒk�r2dS|jdt|dƒ|jdt|dƒ}t|d|d|d|dƒ|k�rŒdS|jdt|dƒ|jdt|dƒ}t|d|d|d|dƒ|k�rædS|jdt|dƒ|jdt|dƒ}t|d|d|d|dƒ|k�r@dSdS)NrrrFT)r6rr1) r=r>Údistr8ÚmidÚIÚhlr-r@r
'update'
2020-07-26 14:44:35 -07:00
r
r ÚlineAABBgs(::4&((,*,*,*rIcCsh|jt t ||g¡ddgg¡j}|jdddg}|_t|dd…df|dd…df||ƒ}||_|S)Nrrr#)r-rr.rr6rI)r=r>rEr"ÚresZoldPZansr
'OBB'
2020-07-17 22:30:56 -07:00
r
r ÚlineOBB~s
$&rKcCsšt |j¡t |j¡}}t |j¡t |j¡}}t |j¡t |j¡}}||}||d||d||dg} t ddg¡}
�x tddƒD�]ü} �xôtddƒD�]ä} || || |
| | <x~tddƒD]p} || }
|dt|
| dƒ|dt|
| dƒ|dt|
| dƒ}t| | ƒ}||
|krÒdSqÒWx¶tddƒD]¨} |dt|
d| ƒ|dt|
d| ƒ|dt|
d| ƒ}
|| }t| d|
d| | d|
d| | d|
d| ƒ}||
|k�rRdS�qRW|dt|
ddƒ|dt|
ddƒ}
|dt|
ddƒ|dt|
ddƒ}t| d|
dd| d|
ddƒ}||
|k�rœdS|dt|
ddƒ|dt|
ddƒ}
|dt|
ddƒ|dt|
ddƒ}t| d|
dd| d|
ddƒ}||
|k�r:dS|dt|
ddƒ|dt|
ddƒ}
|dt|
ddƒ|dt|
ddƒ}t| d|
dd| d|
ddƒ}||
|k�rØdS|dt|
ddƒ|dt|
ddƒ}
|dt|
ddƒ|dt|
ddƒ}t| d|
dd| d|
ddƒ}||
|k�rvdS|dt|
ddƒ|dt|
ddƒ}
|dt|
ddƒ|dt|
ddƒ}t| d|
dd| d|
ddƒ}||
|k�rdS|dt|
ddƒ|dt|
ddƒ}
|dt|
ddƒ|dt|
ddƒ}t| d|
dd| d|
'update'
2020-07-26 14:44:35 -07:00
ddƒ}||
'OBB'
2020-07-17 22:30:56 -07:00
|k�r²dS|dt|
'update'
2020-07-26 14:44:35 -07:00
ddƒ|dt|
ddƒ}
'statespace'
2020-07-08 23:53:27 -07:00
|dt|
'OBB'
2020-07-17 22:30:56 -07:00
ddƒ|dt|
'update'
2020-07-26 14:44:35 -07:00
ddƒ}t| d|
'utils3D.py'
2020-07-09 03:01:40 -07:00
dd| d|
ddƒ}||
'update'
2020-07-26 14:44:35 -07:00
|k�rPdS|dt|
'statespace'
2020-07-08 23:53:27 -07:00
ddƒ|dt|
ddƒ}
'update'
2020-07-26 14:44:35 -07:00
|dt|
ddƒ|dt|
'utils3D.py'
2020-07-09 03:01:40 -07:00
ddƒ}t| d|
'update'
2020-07-26 14:44:35 -07:00
dd| d|
'statespace'
2020-07-08 23:53:27 -07:00
ddƒ}||
|k�rîdS|dt|
'update'
2020-07-26 14:44:35 -07:00
ddƒ|dt|
ddƒ}
'utils3D.py'
2020-07-09 03:01:40 -07:00
|dt|
'update'
2020-07-26 14:44:35 -07:00
ddƒ|dt|
ddƒ}t| d|
'OBB'
2020-07-17 22:30:56 -07:00
dd| d|
'update'
2020-07-26 14:44:35 -07:00
ddƒ}||
'update'
2020-07-09 17:30:32 -07:00
|k�rŒdSdSWq”WdS)Nrrrr#FT)rrr1r6r7ÚzerosÚranger)Úobb1Zobb2r9ÚbZPaZPbr:ÚBÚvr-ÚRr'rZraÚrbrr
'update'
2020-07-26 14:44:35 -07:00
r
r ÚOBBOBB‰s†"H  H@
00,00,00,00,00,00,00,00,00,rTcCsò|j}|j}|d|||d||}}|d|||d||}}|d|||d||}} t |||¡ t¡}
t |||¡ t¡} t || |¡ t¡} tƒ}
x8|
D]0}x*| D]"}x| D]}|
 |||f¡qÎWqÄWqºW|
LRTA*
2020-06-29 20:46:59 -07:00
S)Nrr#rr$rr%)ÚboundaryÚ
'update'
2020-07-26 14:44:35 -07:00
resolutionrÚarangeÚastypeÚfloatÚsetÚadd)Úenvr)rUrVÚxminÚxmaxÚyminÚymaxÚzminÚzmaxÚxarrZyarrZzarrrrrÚzr
'utils3D.py'
2020-07-09 03:01:40 -07:00
r
'update'
2020-07-26 14:44:35 -07:00
r Ú
LRTA*
2020-06-29 20:46:59 -07:00
StateSpaceðs"""
'update'
2020-07-26 14:44:35 -07:00

'utils3D.py'
2020-07-09 03:01:40 -07:00

'update'
2020-07-26 14:44:35 -07:00
recCs*i}t|jƒ}x|D]}tj||<qW|S)z|This function is used to get nodes and discretize the space.
'AnyDstar'
2020-07-19 17:17:06 -07:00
State space is by x*y*z,3 where each 3 is a point in 3D.)rer\rÚinf)r ÚgrrQr
'update'
2020-07-26 14:44:35 -07:00
r
'utils3D.py'
2020-07-09 03:01:40 -07:00
r Úg_Spaces

'update'
2020-07-26 14:44:35 -07:00

'utils3D.py'
2020-07-09 03:01:40 -07:00
rhcCs¶|dkrt||ƒ}t|jj|ƒs(d|fSx6tt|jjƒƒD]"}t||||jj|ƒr:d|fSq:Wx$|jjD]}t |||ƒrjd|fSqjWx&|jj
'update'
2020-07-26 14:44:35 -07:00
D]}t ||||ƒr�d|fSq�Wd|fS)zsee if line intersects obstacleNTF) rr+r\rUrMr0ÚAABBrIÚballsrDÚOBBrK)r rÚchildrEr'r
'utils3D.py'
2020-07-09 03:01:40 -07:00
r
'update'
2020-07-26 14:44:35 -07:00
r Ú isCollide s
'utils3D.py'
2020-07-09 03:01:40 -07:00
    rmc sÚg}g}|jj}x¬|jD]¢}tttj|t ||¡ƒƒ‰t‡fdd„|jj DƒƒrRqt‡fdd„|jj
Dƒƒrnqt‡fdd„|jj DƒƒrŠqt |jj
'update'
2020-07-26 14:44:35 -07:00
ˆƒr| ˆ¡| ˆ|j||f¡qW|dkrÊ|S|dkrÖ|SdS)Ncsg|]}t|ˆƒ‘qSr
'update'
2020-07-10 21:52:57 -07:00
)r&)Ú.0r')rlr
'OBB'
2020-07-17 22:30:56 -07:00
r ú
'update'
2020-07-26 14:44:35 -07:00
<listcomp>&szchildren.<locals>.<listcomp>csg|]}t|ˆƒ‘qSr
)r,)rnr')rlr
r ro(scsg|]}t|ˆƒ‘qSr
)r+)rnr')rlr
r ro*srr)r\rVÚAlldirecÚtupleÚmaprr[ÚmultiplyÚanyrkrjÚblocksr+rUr2)r rÚsettingsÚallchildZallcostrVr r
'update'
2020-07-09 17:30:32 -07:00
)rlr Úchildrens$ 
'OBB_line'
2020-07-18 23:37:52 -07:00
rxcCsDx|jjD]}t||ƒr
'update'
2020-07-26 14:44:35 -07:00
dSq
Reference in New Issue Copy Permalink