TVP

# 机器人学：RRT算法

#!/usr/bin/env python

# rrt.py

# This program generates a simple rapidly

# exploring random tree (RRT) in a rectangular region.

# Written by Steve LaValle

# May 2011

importsys,random,pygame,msvcrt

frompygame.localsimport*

frommathimportsqrt,cos,sin,atan2

#constants

XDIM=640

YDIM=480

WINSIZE=[XDIM,YDIM]

EPSILON=7.0

NUMNODES=5000

defdist(p1,p2):

returnsqrt((p1[]-p2[])*(p1[]-p2[])+(p1[1]-p2[1])*(p1[1]-p2[1]))

defstep_from_to(p1,p2):

ifdist(p1,p2)

returnp2

else:

theta=atan2(p2[1]-p1[1],p2[]-p1[])

returnp1[]+EPSILON*cos(theta),p1[1]+EPSILON*sin(theta)

defmain():

#initialize and prepare screen

pygame.init()

screen=pygame.display.set_mode(WINSIZE)

pygame.display.set_caption('RRT S.LaValle May 2011')

white=255,240,200

black=20,20,40

screen.fill(black)

nodes=[]

nodes.append((XDIM/2.0,YDIM/2.0))# Start in the center

# nodes.append((0.0,0.0)) # Start in the corner

foriinrange(NUMNODES):

rand=random.random()*640.0,random.random()*480.0

nn=nodes[]

forpinnodes:

ifdist(p,rand)

nn=p

newnode=step_from_to(nn,rand)

nodes.append(newnode)

pygame.draw.line(screen,white,nn,newnode)

pygame.display.update()

#print i, " ", nodes

foreinpygame.event.get():

ife.type==QUITor(e.type==KEYUPande.key==K_ESCAPE):

sys.exit("Leaving because you requested it.")

# if python says run, then we should run

if__name__=='__main__':

main()

input()

• 发表于:
• 原文链接http://kuaibao.qq.com/s/20180321G1G8M900?refer=cp_1026
• 腾讯「腾讯云开发者社区」是腾讯内容开放平台帐号（企鹅号）传播渠道之一，根据《腾讯内容开放平台服务协议》转载发布内容。
• 如有侵权，请联系 cloudcommunity@tencent.com 删除。

2022-11-27

2022-11-27

2018-07-22

2022-11-27

2022-11-27

2022-11-27

2022-11-27

2022-11-27