Skip to content

Instantly share code, notes, and snippets.

@fogonwater
Last active July 25, 2017 15:31
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save fogonwater/2223153b8ba58aa5e8cd4347c10a2c4d to your computer and use it in GitHub Desktop.
Save fogonwater/2223153b8ba58aa5e8cd4347c10a2c4d to your computer and use it in GitHub Desktop.
Simple rapidly-exploring tree for exploring a space. Only tested in Python 2.x, but is likely to work in Python 3.x.
from __future__ import division
import math
from random import randint, choice
def distance(p1, p2):
""" Euclidean distance """
return math.hypot(p2[0] - p1[0], p2[1] - p1[1])
def step_towards(p1, p2, step_dist=5):
""" Find point a specified distance between p1 & p2 """
if distance(p1,p2) < step_dist:
return p2
math.theta = math.atan2(p2[1]-p1[1],p2[0]-p1[0])
return (
int(p1[0] + step_dist * math.cos(math.theta)),
int(p1[1] + step_dist * math.sin(math.theta))
)
class Tree:
""" A minimalist tree. Can extend to store data on nodes. """
def __init__(self):
self.nodes = {}
def add_node(self, coor):
""" Add a node if it's not already part of the tree """
if coor not in self.nodes:
self.nodes[coor] = {'children': []}
def add_edge(self, coor1, coor2):
""" Add a new edge to the tree. """
# Make sure our nodes are in the tree's node dict...
self.add_node(coor1)
self.add_node(coor2)
# ... then update the first node's children array
self.nodes[coor1]['children'].append(coor2)
class RRT:
""" A rapidly-exploring random tree. """
def __init__(self, min_x, min_y, max_x, max_y):
self.min_x = min_x
self.min_y = min_y
self.max_x = max_x
self.max_y = max_y
self.width = max_x - min_x
self.height = max_y - min_y
self.sources = []
self.tree = Tree()
def add_source(self, coor):
""" Add one or more sources prior to growing the tree out """
self.tree.add_node(coor)
self.sources.append(coor)
def grow(self, num_samples):
""" Feed the tree sample points to make it grow """
tree = self.tree
margin = 100
# Create random sample points. I keep mine away from margins
sample_pts = zip(
[randint(self.min_x + margin, self.max_x - margin) for n in range(num_samples)],
[randint(self.min_y + margin, self.max_y - margin) for n in range(num_samples)]
)
# For every sample point, find closest node on network
# there are smarter ways to do this for large networks than brute force comparison
for sample in sample_pts:
tree_nodes = tree.nodes.keys()
if sample in tree_nodes:
# skip any point already part of a tree
continue
# Choose a random node in tree to serve as a nominal "closest node"
closest_node = choice(tree_nodes)
closest_dist = distance(closest_node, sample)
for node in tree_nodes:
this_dist = distance(node, sample)
if this_dist < closest_dist:
closest_node = node
closest_dist = this_dist
# Set the maximum step-size that the rrt can move towards current sample point.
# This can be a constant, but I use a random val within small range
step_dist = randint(3,6)
new_node = step_towards(closest_node, sample, step_dist=step_dist)
tree.add_edge(closest_node, new_node)
def display(self, dst_file='rrt.png'):
""" Render the network. Requires Pillow module. """
from PIL import Image, ImageDraw
im = Image.new(
'RGB',
(self.width, self.height),
(255,255,255)
)
draw = ImageDraw.Draw(im)
for node_coor in self.tree.nodes:
for child_coor in self.tree.nodes[node_coor]['children']:
draw.line(
[node_coor, child_coor],
fill="#242424",
width=1
)
im.save(dst_file, "PNG")
def main():
print('Creating RRT...')
# Establish size of our play area
width = 800
height = 800
rrt = RRT(0, 0, width, height)
# Add 1 - 5 river sources around the margins of the space
for river in range(randint(1,5)):
flip = choice([1, 2])
if flip == 1:
rrt.add_source( (randint(1, width - 1), choice([0, height])) )
else:
rrt.add_source( (choice([0, width]), randint(1, height - 1)) )
rrt.grow(num_samples=1000)
rrt.display()
if __name__ == '__main__':
main()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment