2020-06-23 19:37:10 -07:00
|
|
|
|
B
|
2020-07-02 14:41:09 -07:00
|
|
|
|
yOþ^³ ã @ sâ d dl Zd dlmZ d dlZd dlZd dlZej ej ej
|
2020-06-24 22:52:40 -07:00
|
|
|
|
e ¡¡d ¡ d dl m
|
2020-06-26 23:05:10 -07:00
|
|
|
|
Z
|
2020-06-24 22:52:40 -07:00
|
|
|
|
dd„ Zdd„ Zd d
|
|
|
|
|
|
„ Zd d „ Zd
|
2020-07-02 14:41:09 -07:00
|
|
|
|
d„ Zdd„ Zdd„ Zdd„ Zdd„ Zdd„ Zdd„ Zg d fdd „Zd d „ Zd d „ ZG d!d"„ d"eƒZ dS )#é N)Úrepmatz /../../Sampling-based Planning/)Ú
|
|
|
|
|
|
visualizationc 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/Sampling-based Planning/rrt_3D/../../Sampling-based Planning/rrt_3D/utils3D.pyÚgetRay
|
|
|
|
|
|
s .r
|
2020-06-24 13:58:07 -07:00
|
|
|
|
c C sJ g }x@| D ]8}| t t |dd… d¡t |dd… d¡g¡¡ q
|
2020-07-02 14:41:09 -07:00
|
|
|
|
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 sn t j | jjdd… | jjdd… ¡}t j ¡ }t| |ƒrBt| ƒS |dk rV| jjd S t |¡S t |¡S dS )zbiased samplingr r r gš™™™™™©?g{®Gáz„?N) r ÚrandomÚuniformÚenvÚboundaryÚisinsideÚ
|
2020-06-23 19:37:10 -07:00
|
|
|
|
sampleFreeÚgoalr )Ú
|
|
|
|
|
|
initparamsr r r r r r $ s &
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
r c C s† x€| j jD ]t}|d |d kr.|d k r
|
|
|
|
|
|
n q
|
2020-07-02 14:41:09 -07:00
|
|
|
|
|d |d krR|d k r
|
2020-06-25 18:18:33 -07:00
|
|
|
|
n q
|
2020-07-02 14:41:09 -07:00
|
|
|
|
|d |d krv|d k r
|
2020-06-25 18:18:33 -07:00
|
|
|
|
n q
|
2020-06-23 19:37:10 -07:00
|
|
|
|
dS q
|
2020-06-25 18:18:33 -07:00
|
|
|
|
W dS ) zsee if inside obstacler r r é r é TF)r r )r r r r r r r 1 s lr 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 r r! TFr )r r r r r Ú isinbound8 s lr" c s¶ t ˆ |ƒ}tˆ |ƒ}t| jj|ƒs&dS x@t| jjƒD ]0}tj ||¡}|dk r4tˆ |ƒ}||kr4dS q4W xH| jj
|
2020-07-02 14:41:09 -07:00
|
|
|
|
D ]<}tj ||¡}|g krr‡ fdd„|D ƒ}t ||kƒrrdS qrW dS )z see if line intersects obstacleTNc s g | ]}t ˆ |ƒ‘qS r )r )Ú.0Új)r r r ú
|
2020-06-23 19:37:10 -07:00
|
|
|
|
<listcomp>L s z isCollide.<locals>.<listcomp>F)
|
2020-07-02 14:41:09 -07:00
|
|
|
|
r
|
|
|
|
|
|
r r" r r r r ÚpyrrZgeometric_testsZray_intersect_aabbÚballsZray_intersect_sphereÚall) r r r ZrayÚdistr ZshotZ dist_wallZ
|
2020-06-25 18:55:55 -07:00
|
|
|
|
dists_ballr )r r Ú isCollide= s"
|
2020-07-02 14:41:09 -07:00
|
|
|
|
|
|
|
|
|
|
|
2020-06-25 18:18:33 -07:00
|
|
|
|
r* c C sT t | j¡}| jdkr | jd S t|t|ƒdƒ}t jj|| dd�}| jt |¡ S )Nr r )Úaxis) r r ÚVr r ÚlenÚlinalgÚnormÚargmin)r r r, ÚxrZdistsr r r ÚnearestR s
|
|
|
|
|
|
|
2020-07-02 14:41:09 -07:00
|
|
|
|
r2 c C s* || t j || ¡ }|| j| }|S )N)r r. r/ Ústepsize)r r r r
|
2020-06-25 18:18:33 -07:00
|
|
|
|
Úxnewr r r Ústeer[ s r5 c
|
2020-07-02 14:41:09 -07:00
|
|
|
|
C s” t | jƒ}| j}| j}t|t |¡| |ƒ}| jr8d}t | j¡}| j dkrZ| jd gS t
|
2020-06-25 18:18:33 -07:00
|
|
|
|
|t |ƒdƒ}tj j || dd�|k }|| } t | ¡S )Nr r )r+ )
|
2020-07-02 14:41:09 -07:00
|
|
|
|
r- r, ÚetaÚgammaÚminr ÚlogÚdoner r r r. r/ )
|
|
|
|
|
|
r r ZcardVr6 r7 Úrr, r1 ÚinsideZ
|
2020-06-25 18:18:33 -07:00
|
|
|
|
nearpointsr r r Úneara s
|
2020-07-02 14:41:09 -07:00
|
|
|
|
|
