mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
57 lines
5.2 KiB
Plaintext
57 lines
5.2 KiB
Plaintext
|
B
|
|||
|
|
°ò^Í ã @ sÀ d dl Zd dlmZ d dlZd dlmZ d dlm Z d dl
|
|||
|
|
m Z d dlm Z
|
|||
|
|
dd„ Zdd„ Zd d
|
|||
|
|
„ Zd d „ Zd
|
|||
|
|
d„ Zdd„ Zdd„ Zdd„ Zdd„ Zd dd„Zdd„ Zd d „ Zg d fd d „ZdS )!é N)Úrepmat)ÚAxes3D)ÚPoly3DCollectionc C s< |d | d |d | d |d | d g}t | |g¡S )Nr é é )ÚnpÚarray)ÚxÚyÚdirec© r úN/home/zhou/Program/path-planning-algorithms/Sampling-based Planning/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 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|| dddd�} | | ¡}
|
|||
|
|
|
|
|||
|
|
S d S )zE
|
|||
|
|
Subroutine used by draw_map() to display the environment blocks
|
|||
|
|
r r Úfloat)Údtypeé é r r r é Né Úint64g333333Ã?Úk)ÚalphaZ
|
|||
|
|
linewidthsZ
|
|||
|
|
edgecolors) r r ÚshapeÚzerosÚrangeÚtyper Z set_vertsZadd_collection3d) Úaxr ÚvÚfÚnÚdZvlZflr ZpcÚhr r r
|
|||
|
|
Údraw_block_list s NF
|
|||
|
|
(8,
|
|||
|
|
r, c C sF t j | jjdd… | jjdd… ¡}t| |ƒr8t| ƒS t |¡S d S )Nr r r )r ÚrandomÚuniformÚenvÚboundaryÚisinsideÚ
|
|||
|
|
sampleFreer )Ú
|
|||
|
|
initparamsr r r r
|
|||
|
|
r2 ; s &
|
|||
|
|
r2 c C s† x€| j jD ]t}|d |d kr.|d k r
|
|||
|
|
n q
|
|||
|
|
|d |d krR|d k r
|
|||
|
|
n q
|
|||
|
|
|d |d krv|d k r
|
|||
|
|
n q
|
|||
|
|
dS q
|
|||
|
|
W dS ) zsee if inside obstacler r r r r r TF)r/ r )r3 r r r r r
|
|||
|
|
r1 A s lr1 c C sZ t ||ƒ}t||ƒ}x@t| jjƒD ]0}tj ||¡}|dk r"t||ƒ}||kr"dS q"W dS )z see if line intersects obstacleNTF)r r r r/ r ÚpyrrÚgeometric_testsÚray_intersect_aabb)r3 r r
|
|||
|
|
ÚrayÚdistr ZshotZ dist_wallr r r
|
|||
|
|
Ú isCollideH s
|
|||
|
|
|
|||
|
|
|
|||
|
|
r9 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)r3 r r; ÚxrZdistsr r r
|
|||
|
|
ÚnearestT s
|
|||
|
|
|
|||
|
|
rA c C s* || t j || ¡ }|| j| }|S )N)r r= r> Ústepsize)r3 r r
|
|||
|
|
r Úxnewr r r
|
|||
|
|
Ústeer\ s rD r c C sZ t | j¡}| jdkr | jd S t|t|ƒdƒ}t jj|| dd�|k }|| }t |¡S )Nr r )r: )r r r; r r r< r= r> )r3 r Úrr; r@ ZinsideZ
|
|||
|
|
nearpointsr r r
|
|||
|
|
Úneara s
|
|||
|
|
|
|||
|
|
rF c C sR t || jjkƒrdS | jt|d ƒ t|d ƒ t|d ƒ }t| |ƒt||ƒ S )z-here use the additive recursive cost functionr r r )Úallr/ ÚstartÚParentÚstrÚcostr )r3 r Úxparentr r r
|
|||
|
|
rK k s *rK c C st t | j¡}t | j¡}t | j¡}| jj}| jj}tj ddd�}|j
|
|||
|
|
ddd� | ¡ t || jj
|
|||
|
|
ƒ |g krÞxn|D ]f}|d d |d d f}|d d |d d f} |d d |d d f}
|
|||
|
|
tj || |
|
|||
|
|
¡} | | ¡ qtW |g k�r\xr|D ]j}|d d |d d f}|d d |d d f} |d d |d d f}
|
|||
|
|
tjj|| |
|
|||
|
|
d
|