2020-06-27 09:58:58 -07:00
|
|
|
|
B
|
2020-06-29 20:46:59 -07:00
|
|
|
|
`´ú^ë
|
2020-06-27 09:58:58 -07:00
|
|
|
|
ã @ sœ d dl Zd dlZdd„ Zdd„ Zdd„ Zdd „ Zd
|
2020-06-29 20:46:59 -07:00
|
|
|
|
d „ Zd d
|
2020-06-27 17:16:40 -07:00
|
|
|
|
„ Zdd„ Z dd„ Z
|
2020-06-27 09:58:58 -07:00
|
|
|
|
dd„ Z dd„ Z d dd„Z
|
2020-06-29 20:46:59 -07:00
|
|
|
|
dd„ Zdd„ Zd!d d „Zed kr˜d d lmZ dS )"é Nc C s< |d | d |d | d |d | d g}t | |g¡S )Nr é é )ÚnpÚarray)ÚxÚyÚdirec© r úƒ/home/alex/Desktop/github/path-planning-algorithms/Search-based Planning/Search_3D/../../Search-based Planning/Search_3D/utils3D.pyÚgetRay s .r c C sJ g }x@| D ]8}| t t |dd… d¡t |dd… d¡g¡¡ q
|
|
|
|
|
|
W |S )Nr é é )Úappendr r Úadd)ÚblocksÚAABBÚir r r
|
2020-06-27 09:58:58 -07:00
|
|
|
|
ÚgetAABB s
|
2020-06-29 20:46:59 -07:00
|
|
|
|
8r c C sD t t| d |d d | d |d d | d |d d gƒ¡S )Nr r r )r ÚsqrtÚsum)Úpos1Úpos2r r r
|
|
|
|
|
|
ÚgetDist s r c C s> t t| d |d ƒt| d |d ƒt| d |d ƒgƒS )Nr r r )r Úabs)r r r r r
|
|
|
|
|
|
Ú
|
|
|
|
|
|
getManDist s r c C sB d\}}x4| ¡ D ](}t|ƒ}t||ƒ}||k r|| }}qW |S )z!get the nearest point on the grid)iè N)ÚkeysÚdehashr )ÚSpaceÚptZmindisZminptZstrptsÚptsÚdisr r r
|
|
|
|
|
|
Ú
|
|
|
|
|
|
getNearest s
|
2020-06-27 09:58:58 -07:00
|
|
|
|
r! c C s2 i }x(| ¡ D ] }tt|t|ƒ ƒƒ||< qW |S )zMax norm distance)r Úmaxr r )r ÚtÚhÚkr r r
|
2020-06-29 20:46:59 -07:00
|
|
|
|
Ú Heuristic s r& c C s, t | d ƒd t | d ƒ d t | d ƒ S )Nr ú r r )Ústr)r r r r
|
|
|
|
|
|
Úhash3D% s r) c C s t dd„ | d¡D ƒ¡S )Nc S s g | ] }t |ƒ‘qS r )Úfloat)Ú.0r r r r
|
|
|
|
|
|
ú
|
|
|
|
|
|
<listcomp>) s zdehash.<locals>.<listcomp>r' )r r Úsplit)r r r r
|
2020-06-27 09:58:58 -07:00
|
|
|
|
r ( s r c C st | d |d kr | d k rpn nL| d |d krD| d k rpn n(| d |d krh| d k rpn ndS dS ) Nr r r é r é TFr )r r r r r
|
|
|
|
|
|
Ú isinbound+ s lr0 c C s" t | dd… |ƒ| d kr dS dS )Nr r TF)r )r r r r r
|
|
|
|
|
|
Úisinball0 s r1 c C s
|
|
|
|
|
|
| j j}| j j}|d || |d || }}|d || |d || }}|d || |d || }} t |||¡ t¡}
|
|
|
|
|
|
t |||¡ t¡} t || |¡ t¡} t |
|
2020-06-29 20:46:59 -07:00
|
|
|
|
| | ¡}
|
|
|
|
|
|
t |
|
|
|
|
|
|
dt |
|
2020-06-27 09:58:58 -07:00
|
|
|
|
ƒt | ƒ t | ƒ g¡}i }x |j
|
2020-06-29 20:46:59 -07:00
|
|
|
|
D ]}tj |t |ƒ< qðW |S )z|This function is used to get nodes and discretize the space.
|
2020-06-27 22:16:12 -07:00
|
|
|
|
State space is by x*y*z,3 where each 3 is a point in 3D.r r r r. r r/ )
|
|
|
|
|
|
ÚenvÚboundaryÚ
|
|
|
|
|
|
resolutionr ÚarangeÚastyper* ÚmeshgridÚreshapeÚlenÚTÚinfr) )Ú
|
2020-06-29 20:46:59 -07:00
|
|
|
|
initparamsÚfactorr3 r4 ÚxminÚxmaxÚyminÚymaxÚzminÚzmaxÚxarrZyarrZzarrÚVZVVr Úvr r r
|
|
|
|
|
|
Ú
|
2020-06-27 09:58:58 -07:00
|
|
|
|
StateSpace5 s """$ rG c
|
2020-06-29 20:46:59 -07:00
|
|
|
|
sü | j j}t tttjˆ t ||¡ƒƒ¡}tˆ |ƒt ˆ |ƒ }}t
|
2020-06-27 22:16:12 -07:00
|
|
|
|
| j j |ƒsTd|fS x>| j D ]4}t
|
2020-06-29 20:46:59 -07:00
|
|
|
|
j ||¡}|dk r\t ˆ |ƒ} | |kr\d|fS q\W x^| j jD ]R}t||ƒr´d|fS t
|
|
|
|
|
|
j ||¡}|g krž‡ fdd„|D ƒ}
|
|
|
|
|
|
t|
|
|
|
|
|
|
|kƒržd|fS qžW d|fS )z see if line intersects obstacleTNc s g | ]}t ˆ |ƒ‘qS r )r )r+ Új)r r r
|
|
|
|
|
|
r, Y s z isCollide.<locals>.<listcomp>F)r2 r4 r r ÚlistÚmapr Úmultiplyr r r0 r3 r ÚpyrrZgeometric_testsZray_intersect_aabbÚballsr1 Zray_intersect_sphereÚall) r< r r r4 ÚchildZrayÚdistr ZshotZ dist_wallZ
|
|
|
|
|
|
dists_ballr )r r
|
|
|
|
|
|
Ú isCollideG s(
|
|
|
|
|
|
|
|
|
|
|
|
rQ c C sD x | j jD ]}t||ƒr
|
2020-06-27 22:16:12 -07:00
|
|
|
|
dS q
|
2020-06-29 20:46:59 -07:00
|
|
|
|
W x | j jD ]}t||ƒr*dS q*W dS )NFT)r2 r r0 rM r1 )r< r r r r r
|
|
|
|
|
|
Ú obstacleFree^ s
|
