-
Notifications
You must be signed in to change notification settings - Fork 0
/
RRT.py
91 lines (83 loc) · 2.82 KB
/
RRT.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#Assignment 2
#Group Members
#Hemanth Sabbella 2013152
#Aneesh Kumar - 2013126
import pygame, sys, random
import math
from pygame.locals import *
pygame.init()
white = (255,255,255)
red = (255, 0, 0)
blue = (0, 0, 255)
green = (0, 255, 0)
black = (0, 0, 0)
yellow = (255, 255, 0)
start_pointx = 30
start_pointy = 20
end_pointx = 600
end_pointy = 400
delta = 7
small_rect = 5
frame = pygame.display.set_mode((700, 500))
pygame.display.set_caption('RRT algorithm')
frame.fill(black)
rect1 = pygame.Rect(200, 30, 80, 300)
rect2 = pygame.Rect(400, 250, 80, 80)
rect3 = pygame.Rect(400, 100, 140, 80)
rect4 = pygame.Rect(80, 250, 80, 140)
start = pygame.Rect(start_pointx, start_pointy, small_rect, small_rect)
end = pygame.Rect(end_pointx, end_pointy, small_rect, small_rect)
pygame.draw.rect(frame, white, [200, 30, 80, 300])
pygame.draw.rect(frame, white, [400, 250, 80, 80])
pygame.draw.rect(frame, white, [400, 100, 140, 80])
pygame.draw.rect(frame, white, [80, 250, 80, 140])
pygame.draw.rect(frame, red, [start_pointx, start_pointy, 10, 10])
pygame.draw.rect(frame, blue, [end_pointx, end_pointy, 10, 10])
#Function colliderect() is used to check whether two rectangles overlapping or not.
old_point = []
new_point = []
points = []
points.append((start_pointx, start_pointy))
#points.append((end_pointx, end_pointy))
def distance(randx, randy):
count = 0
for i in points:
count = count + 1
if(count == 1):
smallest = (randx - i[0])**2 + (randy - i[1])**2
a = i
elif(count > 1):
if(smallest > (randx - i[0])**2 + (randy - i[1])**2):
smallest = (randx - i[0])**2 + (randy - i[1])**2
a = i
return a
def newnode(randx, randy, nearest_point):
angle = math.atan((randy - nearest_point[1])/(randx - nearest_point[0]))
new_point.append((nearest_point[0] + delta*math.cos(angle), nearest_point[1] + delta*math.sin(angle)))
return new_point
x_old = start_pointx
y_old = start_pointy
while True:
randx = random.random()*700
randy = random.random()*500
nearest_point = distance(randx, randy)
new_point = []
new_point = newnode(randx, randy, nearest_point)
x = int(new_point[0][0])
y = int(new_point[0][1])
objects = pygame.Rect(x, y, small_rect, small_rect)
if not objects.colliderect(rect1) and not objects.colliderect(rect2) and not objects.colliderect(rect3) and not objects.colliderect(rect4):
pygame.draw.rect(frame, green, [int(new_point[0][0]), int(new_point[0][1]), small_rect, small_rect])
points.append((new_point[0]))
#pygame.draw.line(frame, yellow,(x_old, y_old),(x,y),2)
if objects.colliderect(end):
pygame.display.update()
break
#pygame.draw.line(frame, green, (int(old_point[0][0]), int(old_point[0][1])), (int(new_point[0][0]), int(new_point[0][1])), 1)
for event in pygame.event.get():
if event.type == QUIT:
pygame.quit()
sys.exit()
pygame.display.update()
x_old = new_point[0][0]
y_old = new_point[0][1]