def test_optimise_point_to_line(self):
""" test optimise_point_to_line """
raw_map = raw_map.RawMap(10, 10, 100 * [0])
area_map = area_map.AreaMap(raw_map, lambda _: True)
p = vector.PointF(3, 4)
a = vector.PointF(2, 0)
b = vector.PointF(4, 0)
p0 = a
# direct path p->(3,0) is valid and optimal
self.assertEqual(area_map.optimise_point_to_line(p, p0, a, b),
vector.PathF([p, vector.PointF(3, 0)]))
# direct path p->a is valid and optimal
p = vector.PointF(1, 4)
self.assertEqual(area_map.optimise_point_to_line(p, p0, a, b),
vector.PathF([p, a]))
# direct path p->b is valid and optimal
p = vector.PointF(5, 4)
self.assertEqual(area_map.optimise_point_to_line(p, p0, a, b),
vector.PathF([p, b]))
print("h")
# direct path p->b is not valid anymore, but
# p->(3,2)->(3,0) is valid and optimal.
# also note that p->a is valid.
area_map[vector.GridTile(3, 1)] = 0 # 3,1 is now unpassable
p = vector.PointF(4, 4)
self.assertEqual(
area_map.optimise_point_to_line(p, p0, a, b),
vector.PathF([p, vector.PointF(3, 2),
vector.PointF(3, 0)]))
def __optimise_path_iteration(self, path):
"""
one iteration step of the optimise path algorithm
"""
path_changed = False
# start new path with the starting point of the old path, truncate old_path
path, old_path = vector.PathF(path.points[0:1]), vector.PathF(path.points[1:])
while not old_path.empty():
# take the last point of the new path
base = path.end()
while old_path.node_count() >= 2:
p0 = old_path.pop_first()
p1 = old_path.start()
# the goal is to optimise the path base->p0->p1,
# we would like to to replace it by base->p1, but that might not the possible,
# so we find the obstruction and improve the path
t, obs = self.find_obstruction_when_transforming_line(base, p0, p1)
obs = obs.toPointF() if obs is not None else None
if obs is None:
# case: we can actually replace the path by base->p1, ignore p0
path_changed = True
continue
elif obs == p0:
# case: p0 is actually part of the obstruction, hence necessary
path.append(p0)
break
elif obs is not None:
# case: we found an obstruction
# we project the obstruction on the p0->p1 line (called pt)
# and replace
# base->p0->p1
# by
# base->obs->pt->p1
# with pt = p0+t*(p1-p0)
# there is further optimisation potential which will be unlocked
# in the next iteration
path.append(obs)
pt = p0 + (p1 - p0).scaled(t)
pt = vector.PointF(pt.x, pt.y)
if obs != pt:
old_path.prepend(pt)
path_changed = True
break
else:
# there is only one element left, the end point which we may not change
path.append(old_path.pop_first())
print("new path: length: %s, nodes: %d" % (path.length(), path.node_count()))
# print("%s" % path)
return path, path_changed
def _build_helper(self, edges):
""" helper method: expand nodes """
# if the nodes list is already too long, ignore it
if len(edges) > self.n:
return
# get the old optimal path
if len(edges) != 1:
old_opt_path = self.optimal_path(edges[:-1])
else:
old_opt_path = vector.PathF([])
# create the key under which we can find the data
key = self.create_key(edges)
# if this key was already visited, do not skip it, just don't compute anything
if key not in self._map_opt_path:
# create an extended path along the new node
not_opt_path = old_opt_path.points + edges[-1].opt_path().points
not_opt_path = vector.PathF(not_opt_path)
# find optimal path
if len(edges) > 1:
opt_path = self.area_map.optimise_path(not_opt_path)
else:
opt_path = not_opt_path
# find optimal gate path
opt_gate_path = self._opt_gate_path(
edges[0].start().position,
edges[0].start_gates(),
edges[-1].end().position,
edges[-1].end_gates(),
opt_path)
# save the optimal path and min/max
self._map_opt_path[key] = opt_path
self._map_length[key] = (opt_gate_path.length(), opt_path.length())
# iterate over all edges
for edge in edges[-1].end().directional_edges():
# do not allow loops
if edge.end() == edges[0].start() or edge.end() in [e.end() for e in edges]:
continue
# now we have an edge with which we can extend our list
new_edges = edges + [edge]
# add it
self._build_helper(new_edges)
def optimise_path_loose_ends(self, path, start_a, start_b, end_a, end_b, max_iterations=1000):
"""
find the shortest path (in the homotopy class) between
a point on start_a->start_b and end_a->end_b taking path
as the starting point.
it is assumed that the lines do not intersect except possibly
at an end point
the high max iteration count is due to convergence issues
in the projection step.
"""
assert (isinstance(start_a, vector.PointF))
assert (isinstance(start_b, vector.PointF))
assert (isinstance(end_a, vector.PointF))
assert (isinstance(end_b, vector.PointF))
print("optimising path of length %s (nodes: %d)..." % (path.length(), path.node_count()))
# copy path
path = vector.PathF(path.points[:])
# to help with convergence, we add additional points to the path:
# without any obstructions the points which are closest together
# are the solution, so we add this expected solution to the path
# this solves a huge class of convergence issues.
pairs = [(s, e) for s in (start_a, start_b) for e in (end_a, end_b)]
s, e = min(pairs, key=lambda s_e: (s_e[1] - s_e[0]).length())
if path.points[0] != s:
path.points.insert(0, s)
if path.points[-1] != e:
path.points.append(e)
for iteration in range(max_iterations):
# optimise the path
path, path_changed = self.__optimise_path_iteration(path)
# compute the path from path.points[1] to the line start_a->start_b
partial_path = self.optimise_point_to_line(path.points[1], path.points[0], start_a, start_b)
# add it needed (which is identified by a change of base points)
if partial_path.points[-1] != path.points[0]:
path.points[:2] = reversed(partial_path.points)
path_changed = True
# compute the path from path.points[-2] to the line start_a->start_b
partial_path = self.optimise_point_to_line(path.points[-2], path.points[-1], end_a, end_b)
# add it needed (which is identified by a change of base points)
if partial_path.points[-1] != path.points[-1]:
path.points[-2:] = partial_path.points
path_changed = True
# do it until the path does not change anymore
if not path_changed:
break
else:
raise RuntimeError("Needed way too many iterations.")
print("done")
return path
def find_path_between_nodes(self, start, end):
""" find the best path between the two points """
assert (isinstance(start, graph.GraphNode))
assert (isinstance(end, graph.GraphNode))
# find the shortest path
path = self.finder.find_path(start, end)
# convert it to PathF and maximise (not very optimised version)
opt_path = sum([edge.opt_path().points for edge in path.edges()], [])
opt_path = vector.PathF(opt_path)
opt_path = self.area_map.optimise_path(opt_path)
# and return it
return opt_path
def optimise_point_to_line(self, point, p0, line_a, line_b):
"""
find the shortest path between point and the line line_a->line_b
when assuming that the line point->p0 is not obstructed, where
p0 is a point on the line line_a->line_b
"""
path = vector.PathF([point, p0])
while True:
# short cuts
line_p = path.points[-1]
base = path.points[-2]
# compute the projection of base to line_a->line_b
p = self.__project_point_on_line(base, line_a, line_b)
# only do something if there is a chance of change
if p == line_p:
break
# we now want to transform the line base->line_p to base->p
t, obs = self.find_obstruction_when_transforming_line(base, line_p, p)
# if there is no obstruction
if not obs:
# just change it, the straight line is valid
path.points[-1] = p
elif obs:
# if there is an obstruction
# compute the point on the line line_p->p
# (in particular this is a valid end point)
pt = p0 + t * (p - p0)
pt = vector.PointF(pt.x, pt.y)
# replace base->p0 by base->obs->pt
path.points[-1] = obs.toPointF()
if obs.toPointF() != pt:
path.points.append(pt)
return path
def exact_eval(path):
path = sum([edge.opt_path().points for edge in path.edges()], [])
path = vector.PathF(path)
opt = self.area_map.optimise_path(path)
return opt.length()
def optimise_path_loose_ends(self):
""" test optimise_path_loose_ends """
raw_map = raw_map.RawMap(10, 10, 100 * [0])
area_map = area_map.AreaMap(raw_map, lambda _: True)
start_a = vector.PointF(2, 0)
start_b = vector.PointF(4, 0)
end_a = vector.PointF(4, 4)
end_b = vector.PointF(6, 4)
# direct start_b->end_a is valid and optimal
path = vector.PathF([start_a, end_b])
self.assertEqual(
area_map.optimise_path_loose_ends(path, start_a, start_b, end_a,
end_b),
vector.PathF([start_b, end_a]))
area_map[vector.GridTile(3, 1)] = 0 # 3,1 is now unpassable
# direct start_b->end_a is not valid anymore. however
# (3,0)->(3,2)->end_a is. note that start_a->end_a is valid
path = vector.PathF([start_a, end_a])
self.assertEqual(
area_map.optimise_path_loose_ends(path, start_a, start_b, end_a,
end_b),
vector.PathF([vector.PointF(3, 0),
vector.PointF(3, 2), end_a]))
area_map[vector.GridTile(3, 1)] = -1 # 3,1 is now passable again
# small number
eps = 0.001
# slow convergence (fixed by preprocess step)
start_a = vector.PointF(2, 0)
start_b = vector.PointF(4, eps)
end_a = vector.PointF(2, 4)
end_b = vector.PointF(4, 4 - eps)
# direct start_b->end_b is valid and optimal
path = vector.PathF([start_a, end_a])
self.assertEqual(
area_map.optimise_path_loose_ends(path, start_a, start_b, end_a,
end_b),
vector.PathF([start_b, end_b]))
area_map[vector.GridTile(2, 1)] = 0 # 2,1 is now unpassable
# prevented convergence catastrophe (fixed by preprocess step)
start_a = vector.PointF(0, 0)
start_b = vector.PointF(10, 1.5)
end_a = vector.PointF(0, 3)
end_b = vector.PointF(10, 1.5)
# optimal is ???->(3,1)->(3,2)->???
path = vector.PathF([start_a, end_a])
self.assertEqual(
area_map.optimise_path_loose_ends(path, start_a, start_b, end_a,
end_b),
vector.PathF([
vector.PointF(860 / 409., 129 / 409.),
vector.PointF(2, 1),
vector.PointF(2, 2),
vector.PointF(860 / 409., 1098 / 409.)
]))
# convergence catastrophe (fixed by preprocess step)
start_a = vector.PointF(2, 0)
start_b = vector.PointF(4, eps)
end_a = vector.PointF(2, 2 * eps)
end_b = vector.PointF(4, eps)
# degerenates to start_b=end_b
path = vector.PathF([start_a, end_a])
self.assertEqual(
area_map.optimise_path_loose_ends(path, start_a, start_b, end_a,
end_b),
vector.PathF([start_b, end_b]))