2020-06-27 09:58:58 -07:00
|
|
|
|
B
|
2020-07-09 16:05:54 -07:00
|
|
|
|
œˆ_å ã @ sv d Z ddlZdd„ ZG dd„ deƒZdd„ Zd d
|
2020-07-09 13:38:46 -07:00
|
|
|
|
„ Zd d d
|
|
|
|
|
|
dddgfdd„ZG dd„ dƒZ e
|
2020-06-27 09:58:58 -07:00
|
|
|
|
dkrre ƒ Z e e j
|
|
|
|
|
|
ƒ dS )z
|
2020-07-09 16:05:54 -07:00
|
|
|
|
@author: yue qi
|
2020-06-30 15:10:38 -07:00
|
|
|
|
é Nc C sv ddddddgddddddgd ddd
|
2020-06-27 09:58:58 -07:00
|
|
|
|
ddgd ddd ddgg} g }x*| D ]"}t |¡}| d
|
|
|
|
|
|
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 újC:\Users\Huiming Zhou\Desktop\Github-ZHM\path planning algorithms\Search-based Planning\Search_3D\env3D.pyú
|
2020-07-09 13:38:46 -07:00
|
|
|
|
<listcomp> s z getblocks.<locals>.<listcomp>)ÚnpÚarrayÚappend)ÚblockÚ ObstaclesÚir r r Ú getblocks
|
|
|
|
|
|
s
|
|
|
|
|
|
|
|
|
|
|
|
r
|
|
|
|
|
|
c @ s e Zd Zdd„ ZdS )Úaabbc C s| |d |d d |d |d d |d |d d g| _ |d |d d |d |d d |d |d d g| _d S )Né r é é é é )ÚPÚE)ÚselfÚAABBr r r Ú__init__! s <z
|
2020-06-27 09:58:58 -07:00
|
|
|
|
aabb.__init__N)Ú__name__Ú
|
2020-07-09 13:38:46 -07:00
|
|
|
|
__module__Ú __qualname__r r r r r r s r c C s$ g }x| D ]}| t|ƒ¡ q
|
|
|
|
|
|
W |S )N)r r )Úblocksr r r r r ÚgetAABB2% s
|
|
|
|
|
|
r c C sH ddddgddddgg} g }x | D ]}| dd„ |D ƒ¡ q"W t |¡S ) Né g @r r é
|
|
|
|
|
|
r c S s g | ]}|‘qS r r )r r r r r r 0 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_block3 s r! c @ sH e Zd Zddd„Zdd „ Zd
|
2020-07-09 16:05:54 -07:00
|
|
|
|
d „ Zdddgdd ddgd
|
2020-07-09 13:38:46 -07:00
|
|
|
|
ddfdd„Zd
|
|
|
|
|
|
S )Úenvr é r é r c C sh || _ t ||||||g¡| _tƒ | _t| jƒ| _tƒ | _ t dddg¡| _
|
|
|
|
|
|
t dddg¡| _ d| _ d S )Ng à?g @g @g 3@r )
|
|
|
|
|
|
Ú
|
2020-07-09 16:05:54 -07:00
|
|
|
|
resolutionr r Úboundaryr
|
2020-07-09 13:38:46 -07:00
|
|
|
|
r r r r ÚballsÚstartÚgoalÚt)r ÚxminÚyminÚzminÚxmaxÚymaxÚzmaxr% r r r r 7 s z env.__init__c C s( t ƒ }t | j|g¡| _t| jƒ| _d S )N)r! r Úvstackr r r )r Znewblockr r r Ú New_blockA s z
|
|
|
|
|
|
env.New_blockc C s
|
