2020-06-24 22:52:40 -07:00
|
|
|
|
B
|
2020-06-26 23:05:10 -07:00
|
|
|
|
˜ö^í ã @ s| d dl mZ d dlmZ d dlmZ d dlmZ d dlm Z d dl
|
2020-06-25 18:18:33 -07:00
|
|
|
|
Z dd„ Z dd„ Z
|
2020-06-26 23:05:10 -07:00
|
|
|
|
dd
|
|
|
|
|
|
d „Zdd
|
2020-06-24 22:52:40 -07:00
|
|
|
|
d„Zdd„ Zdd„ ZdS )é N)ÚAxes3D)ÚPoly3DCollection)Úproj3dc C s° t ddt j d¡}t dt jd¡}t t |¡t |¡¡}t t |¡t |¡¡}t t t |¡¡t |¡¡}|| | d || | d || | d }}}|||fS )Nr é é é )ÚnpÚlinspaceÚpiÚouterÚcosÚsinÚonesÚsize)ÚcenterÚrÚuÚvÚxÚyÚz© r ú…/home/alex/Desktop/github/path-planning-algorithms/Sampling-based Planning/rrt_3D/../../Sampling-based Planning/rrt_3D/plot_util3D.pyÚ CreateSphere s 4r c C sB x<|D ]4}t |dd… |d ƒ\}}}| j|||ddd� qW d S )Nr é éÿÿÿÿg333333Ã?Úb)ÚalphaÚcolor)r Zplot_wireframe)ÚaxÚballsÚiÚxsÚysÚzsr r r Ú draw_Spheres s
|
|
|
|
|
|
r% ç333333Ã?c
|
2020-06-25 18:18:33 -07:00
|
|
|
|
C s® t jdddgdddgdddgdddgdddgdddgdddgdddggdd�}t ddddgddddgdd d
|
|
|
|
|
|
dgd ddd
|
|
|
|
|
|
gdddd gdddd
|
|
|
|
|
|
gg¡}|jd }|d d …d d…f |d d …d d …f }t d | d f¡}t jd| dfd
|
|
|
|
|
|
d�} xpt|ƒD ]d}
|
|
|
|
|
|
|||
|
|
|
|
|
|
||
|
|
|
|
|
|
d d …f ||
|
|
|
|
|
|
d |
|
|
|
|
|
|
d d …d d …f< ||
|
|
|
|
|
|
d | |
|
|
|
|
|
|
d |
|
|
|
|
|
|
d d …d d …f< qøW t| ƒtk�r~| || ¡ n,t|| |ddd�} | |¡ | | ¡} | S d S )z)
|
2020-06-26 23:05:10 -07:00
|
|
|
|
drawing the blocks on the graph
|
2020-06-25 18:18:33 -07:00
|
|
|
|
r r Úfloat)Údtypeé é r é r é Né Úint64Úk)r Ú
|
2020-06-24 22:52:40 -07:00
|
|
|
|
linewidthsÚ
|
2020-06-26 23:05:10 -07:00
|
|
|
|
edgecolors)
|
|
|
|
|
|
r ÚarrayÚshapeÚzerosÚrangeÚtyper Ú set_vertsÚ
|
|
|
|
|
|
set_facecolorZadd_collection3d)
|
2020-06-25 18:18:33 -07:00
|
|
|
|
r Úblocksr r r ÚfÚnÚdÚvlZflr/ ÚpcÚhr r r Údraw_block_list s FF
|
|
|
|
|
|
(8,
|
|
|
|
|
|
|
|
|
|
|
|
r@ r c C s‚ |g kr~xt|D ]l}|d d |d d f}|d d |d d f}|d d |d d f}t jj|||||d�}| |¡ qW d S )Nr r r )r r )Úplt3dZart3dZLine3DÚadd_line) r ZSETÚ
|
|
|
|
|
|
visibilityr r! r" r# r$ Úliner r r Ú draw_line- s
|
2020-06-26 23:05:10 -07:00
|
|
|
|
rE c C sþ | j d dks| j�rút | j¡}| j}t | j¡}| jj}| jj }|
|
|
|
|
|
|
¡ }t j ddd�}|j
|
|
|
|
|
|
ddd� | ¡ t|| jjƒ t|| jjƒ t|t | jjg¡dd � t||d
|
2020-06-24 22:52:40 -07:00
|
|
|
|
d � t||d d
|
2020-06-26 23:05:10 -07:00
|
|
|
|
� |j|d d …df |d d …df |d d …df ddd� |j|dd… |dd… |dd … dddd� |j|dd… |dd… |dd … dddd� | jjd | jjd }} | jjd | jjd }
|
2020-06-24 22:52:40 -07:00
|
|
|
|
} | jjd | jjd } }
|
|
|
|
|
|
| | | |
|
2020-06-25 18:18:33 -07:00
|
|
|
|
|
|
|
|
|
|
|
| }}}t|d| d| d| ƒ|_t d¡ t d¡ t d ¡ d S ) Né
|
2020-06-24 22:52:40 -07:00
|
|
|
|
r éo Ú3d)Ú
|
2020-06-25 18:18:33 -07:00
|
|
|
|
projectiong éZ )ÚelevÚazim)r g Ð?)rC r )r r r Úg)Úsr Zgor, r/ )Ú
|
|
|
|
|
|
markersizeÚmarkeredgecolorÚror r* r) r r gü©ñÒMbP?) ÚindÚdoner r2 ÚVÚEÚPathÚenvÚstartÚgoalÚget_edgeÚpltÚsubplotZ view_initÚclearr% r r@ r9 ÚboundaryrE Z scatter3DÚplotÚ
|
|
|
|
|
|
make_get_projÚget_projÚxlabelÚylabelÚpause)Ú
|
|
|
|
|
|
initparamsrT rU rV rX rY Úedgesr ÚxminÚxmaxÚyminÚymaxÚzminÚzmaxÚdxÚdyÚdzr r r Ú
|
2020-06-26 23:05:10 -07:00
|
|
|
|
visualization6 s4 8..
|
|
|
|
|
|
|
|
|
|
|
|
rp c s: t |||ƒ}|| ‰ || ‰|| ‰‡ ‡‡‡fdd„}|S )z¨
|
|
|
|
|
|
Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that
|
2020-06-25 18:18:33 -07:00
|
|
|
|
makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*.
|
|
|
|
|
|
c s² t jˆj d t jˆj d } }ˆ ¡ \}}ˆ ¡ \}}ˆ ¡ \}}t ||||||¡}d} t ˆ d ˆ ˆ d ˆˆ d ˆ¡}
|
2020-06-26 23:05:10 -07:00
|
|
|
|
t dddg¡} | d t
|
2020-06-25 18:18:33 -07:00
|
|
|
|
|¡t
|
|
|
|
|
|
| ¡ ˆj | } | d t |¡t
|
