mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
42 lines
3.6 KiB
Plaintext
42 lines
3.6 KiB
Plaintext
|
B
|
|||
|
|
÷^´ ã @ s‚ d dl Zd dlZdd„ Zdd„ Zdd„ Zdd „ Zd
|
|||
|
|
d „ Zd d
|
|||
|
|
„ Zdd„ Z dd„ Z
|
|||
|
|
ddd„Z dd„ Z dd„ Z
|
|||
|
|
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/Astar_3D/../../Search-based Planning/Astar_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)ÚblocksZAABBÚir r r
|
|||
|
|
ÚgetAABB s
|
|||
|
|
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 )ÚSpaceZptZmindisZminptZstrptsZptsÚdisr r r
|
|||
|
|
Ú
|
|||
|
|
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
|
|||
|
|
Ú 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
|
|||
|
|
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 |
|
|||
|
|
| | ¡}
|
|||
|
|
t |
|
|||
|
|
dt |
|
|||
|
|
ƒt | ƒ t | ƒ g¡}i }x |j
|
|||
|
|
D ]}tj |t |ƒ< qðW |S )z|This function is used to get nodes and discretize the space.
|
|||
|
|
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# )Ú
|
|||
|
|
initparamsZfactorr, r- ÚxminÚxmaxÚyminÚymaxÚzminÚzmaxZxarrZyarrZzarrÚVZVVr Úvr r r
|
|||
|
|
Ú
|
|||
|
|
StateSpace- s """$ r> c
|
|||
|
|
sî | j j}t tttjˆ t ||¡ƒƒ¡}tˆ |ƒ}t ˆ |ƒ}t
|
|||
|
|
| j j |ƒsRd|fS xDt | j j
|
|||
|
|
ƒD ]4}tj ||¡}|dk r`t ˆ |ƒ} | |kr`d|fS q`W xL| j jD ]@}tj ||¡}|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& P s z isCollide.<locals>.<listcomp>F)r+ r- r r ÚlistÚmapr Úmultiplyr r r* r, r r ÚpyrrZgeometric_testsZray_intersect_aabbÚballsZray_intersect_sphereÚall) r5 r r r- ÚchildZrayÚdistr ZshotZ dist_wallZ
|
|||
|
|
dists_ballr )r r
|
|||
|
|
Ú isCollide? s&
|
|||
|
|
|
|||
|
|
|