Last active
July 25, 2017 15:31
-
-
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.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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