这是RRT算法的代码
导入系统 导入pygame 导入随机、数学 从数学导入sqrt、atan2、cos、sin 从pygame.locals导入*
类别RRT(对象):
x = 0
y = 0
X_dimension = 0
Y_dimension = 0
Window_size = 0
EPS = 0
Max_nodes = 0
nodes = list()
K_ESCAPE = True
KEYUP = True
QUIT = True
def __init__(self,x,y):
self.x = x
self.y = y
#parameters
self.X_dimension = 1280 #length of the window
self.Y_dimension = 1280 #breadth of the window
self.Window_size = [self.X_dimension, self.Y_dimension] #Window size
self.EPS = 7000 #EPSILON or Incremental Distance
self.Max_nodes = 100 #maximum number of nodes
self.nodes = list()
self.QUIT = QUIT
self.KEYUP = KEYUP
self.K_ESCAPE = K_ESCAPE
#function for calculating euclidean distance
def Calculate_Distance(self,x,y):
x = [10,20]
y = [15,30]
return sqrt((x[0]-y[0])*(x[0]-y[0])+(x[1]-y[1])*(x[1]-y[1]))
pass
def Initiate_Sampling(self,x,y):
self.EPS = 7000
if Calculate_Distance(x,y) < 7000:
return y
else:
theta = atan2(y[1]-x[1], y[0]-x[0])
return x[0] + self.EPS*cos(theta), x[1] + self.EPS*sin(theta)
#Function for displaying the output
def Start_The_Game(self):
pygame.init()
screen = pygame.display.set_node(Window_size)
caption = pygame.display.set_caption("performing RRT")
white = 255, 240, 200
black = 20, 20, 40
screen.fill(black)
#Main Function
def Node_Generation(self, nodes):
self.nodes = []
self.QUIT = QUIT
self.KEYUP = KEYUP
self.K_ESCAPE = K_ESCAPE
#nodes.append(X_dimension/2.0, Y_dimension/2.0)
nodes.append(0.0, 0.0)
pygame.init()
for i in range(Max_nodes):
rand = random.random()*640.0, random.random()*480.0
nn = nodes[0]
for p in nodes:
if dist(p,rand) < dist(nn,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)
for j in pygame.event.get():
if j.type == QUIT or (j.type == KEYUP and j.key == K_ESCAPE):
pygame.quit()
sys.exit("GAME OVER")
路径=RRT(10,15)
路径。计算_距离(10,15)
路径。启动_采样(路径。计算_距离(10,15))
开始游戏()
path.Node_生成()
#我的查询-我想将Calculate_Distance函数作为参数传递给Initiate_Sampling函数,以将其与EPS进行比较
您可以通过
self
访问成员函数,访问方式与访问类中的其他类成员相同。i、 e相关问题 更多 >
编程相关推荐