2020-06-27 09:58:58 -07:00
|
|
|
|
B
|
2020-06-27 17:16:40 -07:00
|
|
|
|
-Ý÷^£ ã @ s‚ d dl Zd dlZdd„ Zdd„ Zdd„ Zdd „ Zd
|
2020-06-27 09:58:58 -07:00
|
|
|
|
d „ Zd d
|
2020-06-27 17:16:40 -07:00
|
|
|
|
„ Zdd„ Z dd„ Z
|
|
|
|
|
|
ddd„Z dd„ Z dd„ Z
|
2020-06-27 09:58:58 -07:00
|
|
|
|
edkr~d dlmZ 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
|
2020-06-27 17:16:40 -07:00
|
|
|
|
W |S )Nr é é )Úappendr r Úadd)ÚblocksÚAABBÚir r r
|
|
|
|
|
|
ÚgetAABB s
|
2020-06-27 09:58:58 -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)Zpos1Zpos2r r r
|
|
|
|
|
|
ÚgetDist 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
|
2020-06-27 17:16:40 -07:00
|
|
|
|
Ú
|
|
|
|
|
|
getNearest s
|
|
|
|
|
|
r c C s2 i }x(| ¡ D ] }tt|t|ƒ ƒƒ||< qW |S )zMax norm distance)r ÚmaxÚabsr )r ÚtÚhÚkr r r
|
2020-06-27 09:58:58 -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
|
2020-06-27 17:16:40 -07:00
|
|
|
|
Ú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 lr- c C s
|
|
|
|
|
|
| j j}| j j}|d || |d || }}|d || |d || }}|d || |d || }} t |||¡ t¡}
|
|
|
|
|
|
t |||¡ t¡} t || |¡ t¡} t |
|
|
|
|
|
|
| | ¡}
|
2020-06-27 17:16:40 -07:00
|
|
|
|
t |
|
|
|
|
|
|
dt |
|
|
|
|
|
|
ƒt | ƒ t | ƒ g¡}i }x |j
|
2020-06-27 09:58:58 -07:00
|
|
|
|
D ]}tj |t |ƒ< qðW |S )z|This function is used to get nodes and discretize the space.
|
2020-06-27 17:16:40 -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& )Ú
|
|
|
|
|
|
initparamsÚfactorr/ r0 ÚxminÚxmaxÚyminÚymaxÚzminÚzmaxÚxarrZyarrZzarrÚVZVVr Úvr r r
|
|
|
|
|
|
Ú
|
|
|
|
|
|
StateSpace- s """$ rC c
|
2020-06-27 09:58:58 -07:00
|
|
|
|
sê | j j}t tttjˆ t ||¡ƒƒ¡}tˆ |ƒt ˆ |ƒ }}t
|
2020-06-27 17:16:40 -07:00
|
|
|
|
| j j |ƒsTd|fS x>| j D ]4}t
|
|
|
|
|
|
j ||¡}|dk r\t ˆ |ƒ} | |kr\d|fS q\W xL| j jD ]@}t
|
|
|
|
|
|
j ||¡}|g krž‡ fdd„|D ƒ}
|
|
|
|
|
|
t|
|
2020-06-27 09:58:58 -07:00
|
|
|
|
|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) O s z isCollide.<locals>.<listcomp>F)r. r0 r r ÚlistÚmapr Úmultiplyr r r- r/ r ÚpyrrZgeometric_testsZray_intersect_aabbÚballsZray_intersect_sphereÚall) r8 r r r0 ÚchildZrayÚdistr ZshotZ dist_wallZ
|
2020-06-27 17:16:40 -07:00
|
|
|
|
dists_ballr )r r
|
