2020-06-27 09:58:58 -07:00
|
|
|
|
B
|
2020-07-02 14:41:09 -07:00
|
|
|
|
%Lþ^þ ã @ sf d Z ddlZdd„ Zdd„ Zdd„ Zd d
|
|
|
|
|
|
d d d
|
|
|
|
|
|
dgfdd„ZG dd„ dƒZedkrbeƒ Z e
|
2020-06-27 09:58:58 -07:00
|
|
|
|
e j ƒ dS )z
|
|
|
|
|
|
@author: yue qi
|
2020-06-30 15:49:14 -07:00
|
|
|
|
é Nc C sv ddddddgddddddgd ddd
|
2020-06-30 15:10:38 -07:00
|
|
|
|
ddgd ddd ddgg} g }x*| D ]"}t |¡}| d
|
2020-06-27 09:58:58 -07:00
|
|
|
|
d„ |D ƒ¡ qFW t |¡S )NgÍÌÌÌÌÌ@g gÍÌÌÌÌÌ @g333333@g @g @g333333"@gÍÌÌÌÌÌ#@g333333(@gÍÌÌÌÌÌ)@gš™™™™2@gfffffæ2@c S s g | ]}|‘qS © r )Ú.0Újr r ú�/home/alex/Desktop/github/path-planning-algorithms/Search-based Planning/Search_3D/../../Search-based Planning/Search_3D/env3D.pyú
|
|
|
|
|
|
<listcomp> s z getblocks.<locals>.<listcomp>)ÚnpÚarrayÚappend)ÚblockÚ ObstaclesÚir r r Ú getblocks
|
2020-07-02 14:41:09 -07:00
|
|
|
|
s
|
|
|
|
|
|
|
|
|
|
|
|
r
|
|
|
|
|
|
c C sJ g }x@| D ]8}| t t |dd… d¡t |dd… d¡g¡¡ q
|
2020-06-27 09:58:58 -07:00
|
|
|
|
W |S )Nr é é )r r r Úadd)ÚblocksÚAABBr r r r ÚgetAABB s
|
2020-07-02 14:41:09 -07:00
|
|
|
|
8r c C sH ddddgddddgg} g }x | D ]}| dd„ |D ƒ¡ q"W t |¡S ) Né g @é é é
|
|
|
|
|
|
é c S s g | ]}|‘qS r r )r r r r r r $ s z getballs.<locals>.<listcomp>)r r r )Zspheresr r r r r Úgetballs s
|
|
|
|
|
|
|
|
|
|
|
|
r g333333.@g gÍÌÌÌÌÌ @gÍÌÌÌÌÌ/@g @g @c C s | S )Nr )r
|
|
|
|
|
|
r r r Ú add_block' s r c @ s e Zd Zd dd„Zdd „ Zd
|
|
|
|
|
|
S ) Úenvr é é r r c C sb || _ t ||||||g¡| _tƒ | _t| jƒ| _tƒ | _ t dddg¡| _
|
|
|
|
|
|
t dddg¡| _ d S )Ng à?g @g @g 3@) Ú
|
|
|
|
|
|
resolutionr r Úboundaryr
|
