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 costs, number defined by rewire count
: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 = [(x_near, path_cost(self.trees[tree].E, x_init, x_near) +
segment_cost(x_near, x_new)) for x_near in X_near]
# noinspection PyTypeChecker
L_near.sort(key=itemgetter(0))
return L_near
def get_nearby_vertices_better(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.
Effectively makes rewire() redundant. However, it produces much better paths in shorter time.
: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))
def connect_trees(self, a, b, x_new, L_near):
"""
Check nearby vertices for total cost and connect shortest valid edge if possible
This results in both trees being connected
:param a: first tree to connect
:param b: second tree to connect
:param x_new: new vertex to add
:param L_near: nearby vertices
"""
for x_near, c_near in L_near:
c_tent = c_near + path_cost(self.trees[a].E, self.x_init, x_new)
if c_tent < self.c_best and self.X.collision_free(
x_near, x_new, self.r):
self.trees[b].V_count += 1
self.trees[b].E[x_new] = x_near
self.c_best = c_tent
sigma_a = self.reconstruct_path(a, self.x_init, x_new)
sigma_b = self.reconstruct_path(b, self.x_goal, x_new)
del sigma_b[-1]
sigma_b.reverse()
self.sigma_best = sigma_a + sigma_b
break
