def rewire(self, tree, x_new, L_near):
"""
Rewire tree to shorten edges if possible
Only rewires vertices according to rewire count
:param tree: int, tree to rewire
:param x_new: tuple, newly added vertex
:param L_near: list of nearby vertices used to rewire
:return:
"""
for c_near, x_near in L_near:
curr_cost = path_cost(self.trees[tree].E, self.x_init, x_near)
tent_cost = path_cost(self.trees[tree].E, self.x_init, x_new) + segment_cost(x_new, x_near)
if tent_cost < curr_cost and self.X.collision_free(x_near, x_new, self.r):
self.trees[tree].E[x_near] = x_new
def get_nearby_vertices(self, tree, x_init, x_new):
"""
Get nearby vertices to new vertex and their associated path costs from the root of tree
as if new vertex is connected to each one separately.
:param tree: tree in which to search
:param x_init: starting vertex used to calculate path cost
:param x_new: vertex around which to find nearby vertices
:return: list of nearby vertices and their costs, sorted in ascending order by cost
"""
X_near = self.nearby(tree, x_new, self.current_rewire_count(tree))
L_near = [(path_cost(self.trees[tree].E, x_init, x_near) +
segment_cost(x_near, x_new), x_near) for x_near in X_near]
# noinspection PyTypeChecker
L_near.sort(key=itemgetter(0))
return L_near