In [1]:

    

import networkx as nx
import random
import math
import matplotlib.pyplot as plt


    

In [2]:

    

workspace_dim = (500,500)
initial_node_pos = (250,250)
node_idx_count = 0
G = nx.Graph()
G.add_node(node_idx_count,pos=initial_node_pos)
node_idx_count +=1
step_size = 10.0
min_iteration = 50
max_iteration = 5000


    

In [3]:

    

def generate_random_sample_pos():
    #randomly draw a position within the workspace
    sample_pos = (random.random()*workspace_dim[0],random.random()*workspace_dim[1])
    return sample_pos

def nodes_distance(pos_1, pos_2):
    #take input as G.node[node_idx]['pos'] or just the position in tuple
    dist = math.sqrt((pos_1[0]-pos_2[0])**2+(pos_1[1]-pos_2[1])**2)
    return dist

def add_new_node(parent_node,sample_pos,step_size):
    global node_idx_count
    if nodes_distance(parent_node['pos'],sample_pos)>step_size:
        theta = math.atan2(sample_pos[1]-parent_node['pos'][1], sample_pos[0]-parent_node['pos'][0])
        new_node_pos = (parent_node['pos'][0]+step_size*math.cos(theta), parent_node['pos'][1]+step_size*math.sin(theta))
        G.add_node(node_idx_count, pos = new_node_pos)
        G.add_edge(node_idx_count-1,node_idx_count,)
        node_idx_count+=1

def add_nearest_negibour(sample_pos, nodes):
    parent_node = nodes[0]
    #The for loop goes trhough node[0],node[1] ... ,node[n]
    for i in range(0,len(nodes)):
        if nodes_distance(nodes[i]['pos'],sample_pos) <= nodes_distance(parent_node['pos'], sample_pos):
            parent_node = nodes[i]
    #To do: add collision check
    add_new_node(G.node[i],sample_pos,step_size)


    

In [32]:

    

G.node


    

    
Out[32]:





{0: {'pos': (250, 250)},
 1: {'pos': (240.496350548791, 246.88862617023682)},
 2: {'pos': (248.3429136940692, 240.68931598222704)},
 3: {'pos': (252.11372682026644, 231.42751440978452)},
 4: {'pos': (246.34225597248007, 223.26111224890903)},
 5: {'pos': (253.54739277015892, 230.19551948577754)},
 6: {'pos': (244.61842702160672, 225.6929012850879)},
 7: {'pos': (252.83711401487236, 231.38967090538378)},
 8: {'pos': (260.9539875267111, 237.23059244164932)},
 9: {'pos': (253.2277372564243, 230.88196607361562)},
 10: {'pos': (253.10866503678992, 220.88267500841948)},
 11: {'pos': (263.1086601483844, 220.89256278304586)},
 12: {'pos': (268.13993518449684, 229.53468471640178)},
 13: {'pos': (259.9741099463625, 223.7623976323901)},
 14: {'pos': (257.98388150421164, 213.9624491249716)},
 15: {'pos': (253.66621335830322, 204.94259647751622)},
 16: {'pos': (255.55380511000953, 214.76283055715456)},
 17: {'pos': (248.02726840985608, 208.17866430004628)},
 18: {'pos': (257.87243792531524, 209.9315582534485)},
 19: {'pos': (254.40310414579048, 219.3104560099148)},
 20: {'pos': (263.68190691700926, 215.58167348302715)},
 21: {'pos': (257.71164621378676, 223.60389182557785)},
 22: {'pos': (265.2864003080762, 217.07525487663932)},
 23: {'pos': (255.29599147351394, 217.51312622566766)},
 24: {'pos': (249.37334568671358, 225.57056172861127)},
 25: {'pos': (259.18884632251354, 223.65850857333277)},
 26: {'pos': (268.8055082055548, 220.91628060555536)},
 27: {'pos': (273.4703674595728, 229.7615668157468)},
 28: {'pos': (265.58584983252683, 223.6106008925115)},
 29: {'pos': (271.34254028569904, 215.43377296070594)},
 30: {'pos': (281.3309209324771, 214.95184759815598)},
 31: {'pos': (289.8508589140476, 220.18736636807677)},
 32: {'pos': (281.706276340741, 214.3851448360607)},
 33: {'pos': (282.7848515799959, 224.32680845203896)},
 34: {'pos': (276.98625565305423, 216.17964420451975)},
 35: {'pos': (284.65313826093364, 209.75944864075413)},
 36: {'pos': (293.11604746435125, 204.43223850648036)},
 37: {'pos': (293.1264890720871, 194.43224395784046)},
 38: {'pos': (284.37685245249634, 199.27412985334814)},
 39: {'pos': (276.674989055343, 192.89594043850715)},
 40: {'pos': (282.64356412183594, 200.9194129832124)},
 41: {'pos': (280.0389277187028, 210.57424955394075)},
 42: {'pos': (277.2458926205277, 220.17627824273157)},
 43: {'pos': (268.18086478111314, 215.95427986866275)},
 44: {'pos': (259.26753435078643, 211.42078886946445)},
 45: {'pos': (267.8889371838167, 216.48748534652097)},
 46: {'pos': (276.97908331592606, 220.65512776123194)},
 47: {'pos': (268.43953478172944, 225.85859879815527)},
 48: {'pos': (278.3944204006677, 226.80741506796542)},
 49: {'pos': (281.6791454008031, 236.25255041233638)},
 50: {'pos': (284.19689947426576, 245.9304073251845)},
 51: {'pos': (280.3106785226431, 255.14437968867512)},
 52: {'pos': (270.4536844375915, 253.45924835632823)},
 53: {'pos': (260.47519938544013, 252.80362951584837)},
 54: {'pos': (262.79585555806125, 262.5306308486578)},
 55: {'pos': (260.4843794145978, 252.8014439330826)},
 56: {'pos': (251.2162526189195, 249.04620416720488)},
 57: {'pos': (246.98890390380265, 239.983670148466)},
 58: {'pos': (245.92011945153024, 249.92639109368923)},
 59: {'pos': (237.99956480336107, 256.0308823964405)},
 60: {'pos': (244.75426410634182, 263.40475772263846)},
 61: {'pos': (241.59296793176006, 272.8919178085954)},
 62: {'pos': (251.5279305796086, 274.0305648943309)},
 63: {'pos': (258.89689083143656, 280.79062590048903)},
 64: {'pos': (266.3861343430623, 274.16407086611616)},
 65: {'pos': (259.70510319265486, 266.7233838839024)},
 66: {'pos': (249.78889895112383, 268.01524047046365)},
 67: {'pos': (257.44287800514996, 274.4508138435617)},
 68: {'pos': (254.10364971049532, 283.876818002068)},
 69: {'pos': (248.66866060820612, 275.4827192739139)},
 70: {'pos': (241.5865990626923, 268.4226623148268)},
 71: {'pos': (251.48633058341633, 267.01010583185587)},
 72: {'pos': (257.97875786507973, 259.4042930552808)},
 73: {'pos': (258.7075517518365, 269.3777006710672)},
 74: {'pos': (262.71519610840426, 260.2158893484482)},
 75: {'pos': (252.8476620722826, 258.593615621243)},
 76: {'pos': (247.4694571839747, 267.02420944252157)},
 77: {'pos': (255.91220964206667, 261.6651114837806)},
 78: {'pos': (247.91972867133634, 267.6751237909655)},
 79: {'pos': (243.3255197655768, 258.7929368682398)},
 80: {'pos': (252.13850377690963, 263.5185387151998)},
 81: {'pos': (258.29328339763157, 271.4000796698828)},
 82: {'pos': (250.74071530734312, 277.9543695049112)},
 83: {'pos': (255.252201646998, 286.87885778195584)},
 84: {'pos': (264.8493183034309, 284.0689909020452)},
 85: {'pos': (274.21889144436295, 280.5745525842715)},
 86: {'pos': (282.2933095188361, 286.47402482834735)},
 87: {'pos': (289.44365920885485, 279.4831379525319)},
 88: {'pos': (281.58056969249145, 273.3048029618038)},
 89: {'pos': (291.57473455057874, 273.64637109464644)},
 90: {'pos': (294.96898750225216, 264.2400409441544)},
 91: {'pos': (292.97019482273896, 254.44183560724065)},
 92: {'pos': (283.90508323151005, 250.22001706085953)},
 93: {'pos': (274.3737636607732, 253.24556614168577)},
 94: {'pos': (266.7356625436232, 246.79115591888524)},
 95: {'pos': (257.5091696320923, 242.93475493435707)},
 96: {'pos': (251.56763387542483, 250.9782711338958)},
 97: {'pos': (261.5614562629775, 250.62682542535708)},
 98: {'pos': (266.491647217807, 259.3270103170084)},
 99: {'pos': (260.33093417786904, 251.45010643035735)}}

In [14]:

    

while node_idx_count < 100:
    sample_pos = generate_random_sample_pos()
    add_nearest_negibour(sample_pos, G.node)


    

In [37]:

    

pos = nx.get_node_attributes(G,'pos')
nx.draw_networkx_nodes(G,pos,node_size=100)
plt.show()


    

In [ ]: