mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
24 lines
2.9 KiB
Plaintext
24 lines
2.9 KiB
Plaintext
|
B
|
|||
|
|
8ô^ï ã @ sv d Z ddlZddlZddlZddlZddlZej ej ej
|
|||
|
|
e ¡¡d ¡ ddl m
|
|||
|
|
Z
|
|||
|
|
ddlmZ G dd„ dƒZdS )z1
|
|||
|
|
utils for collision check
|
|||
|
|
@author: huiming zhou
|
|||
|
|
é Nz /../../Sampling-based Planning/)Úenv)ÚNodec @ sL e Zd Zdd„ Zdd„ Zdd„ Zdd„ Zd d
|
|||
|
|
„ Zed d „ ƒZ ed
|
|||
|
|
d„ ƒZ
|
|||
|
|
dS )ÚUtilsc C s< t ¡ | _ d| _| j j| _| j j| _| j j| _| ¡ | _d S )Ngš™™™™™É?)r ÚEnvÚdeltaÚ
|
|||
|
|
obs_circleÚ
|
|||
|
|
obs_rectangleÚ obs_boundaryÚget_obs_vertexÚ
|
|||
|
|
obs_vertex)Úself© r
|
|||
|
|
úS/home/zhou/Program/path-planning-algorithms/Sampling-based Planning/rrt_2D/utils.pyÚ__init__ s
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
zUtils.__init__c C s| | j }g }xl| jD ]b\}}}}|| || g|| | || g|| | || | g|| || | gg}| |¡ qW |S )N)r r Úappend)r r Zobs_listÚoxÚoyÚwÚhZ vertex_listr
|
|||
|
|
r
|
|||
|
|
r r
|
|||
|
|
s zUtils.get_obs_vertexc C s | ||¡\}}|d |d |d |d g}|d |d |d |d g}|d |d g} t || ¡}
|
|||
|
|
|
|
|||
|
|
dkrzd}
|