def nearest(vertex_rand, current_vertex, vertex_nearest, k): # recursively exhaustively search tree for nearest node current_vertex.k_nearest = k if dist(vertex_rand, current_vertex) < dist( vertex_rand, vertex_nearest ) and current_vertex.at_goal_set is False: # if distance to next vertex in tree is smaller vertex_nearest = current_vertex # update it as the nearest - is this accidentally changing root node value? for i in range(len(current_vertex.children)): # for all children vertices if current_vertex.children[ i].k_nearest != k: # if next vertex not checked yet vertex_nearest = nearest(vertex_rand, current_vertex.children[i], vertex_nearest, k) # call nearest function again return vertex_nearest
def path_generation(vertex): #print 'running path gen 1', 'vertex parents:', vertex.parents paths_parents = [] # list of paths, path = list of vertices costs_parents = [] # list of costs associated with some paths paths = [] costs = [] for i in range(len(vertex.parents)): # for all parents parent = vertex.parents[i] pths, csts = path_generation2( parent) # get all paths and costs for that parent paths_parents.append(pths) # append paths to path list costs_parents.append(csts) # append costs to cost list # separating a list of lists of paths into one list of paths: for i in range(len(paths_parents) ): # for number of lists of paths (number of parents) distance = dist( vertex, vertex.parents[i]) # calculate distance for each new parent for j in range(len(paths_parents[i]) ): # for number of paths in one of those lists paths.append( paths_parents[i][j]) # append that path to new total list costs.append( costs_parents[i][j] + distance ) # append that parent cost + distance to new total cost list for i in range(len(paths)): # for every path in paths paths[i].append(vertex) # add the current vertex to that path if len(paths) == 0: # if this vertex is the root node paths = [[vertex]] # only "path" is itself, and has zero cost costs = [0] return paths, costs
def get_time( path, robo_num, times ): # simple version for holonomic, velocity is controlled / constant-magnitude if len(times) == 0: times.append(0) for i in range(len(path) - 1): add_time = dist(path[i], path[i + 1]) / Settings.robo_velocities[robo_num] times.append(times[i] + add_time) # else times is already calculated return
def near_vertices(vertex_new, current_vertex, k, vertices_near): # return all vertices within near_radius current_vertex.k_near = k # for avoiding checking one vertex multiple times if k == 1: near_radius = Dimensions.eta else: near_radius = min(Dimensions.gamma * sqrt(log(k) / k), Dimensions.eta) # function of numPoints if dist(vertex_new, current_vertex) <= near_radius and ( current_vertex.at_goal_set is False): # if within radius vertices_near.append(current_vertex) # add to list of near vertices for i in range(len( current_vertex.children)): # for all children of this vertex if current_vertex.children[ i].k_near != k: # if that child not checked yet vertices_near = near_vertices(vertex_new, current_vertex.children[i], k, vertices_near) # call function again return vertices_near