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 test_find_obstruction_when_transforming_line(self):
""" test find_obstruction_when_transforming_line """
raw_map = raw_map.RawMap(10, 10, 100 * [0])
area_map = area_map.AreaMap(raw_map, lambda _: True)
base = vector.PointF(2, 2)
start = vector.PointF(8, 2)
end = vector.PointF(2, 8)
# print(area_map.find_tiles_in_triangle(base, start, end))
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(1.0, None))
area_map[vector.GridTile(4, 4)] = 0 # 4,4 is now unpassable
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(0.4, vector.GridPoint(5, 4)))
area_map[vector.GridTile(6, 3)] = 0 # 6,3 is now unpassable
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(1. / 6., vector.GridTile(7, 3)))
area_map[vector.GridTile(4, 2)] = 0 # 4,2 is now unpassable
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(0., vector.GridPoint(5, 2)))
area_map[vector.GridTile(2, 2)] = 0 # 2,2 is now unpassable
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(0., vector.GridPoint(5, 2)))
# test angle > 90
base = vector.PointF(6, 4)
start = vector.PointF(9, 5)
end = vector.PointF(3, 5)
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(2. / 3., vector.GridPoint(5, 5)))
base = vector.PointF(3, 3)
start = vector.PointF(0, 5)
end = vector.PointF(9, 7)
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(0.5, vector.GridPoint(4, 5)))
area_map[vector.GridTile(6, 5)] = 0 # 6,5 is now unpassable
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(0.5, vector.GridPoint(4, 5)))
area_map[vector.GridTile(4, 4)] = -1 # 4,4 is now passable again
self.assertEqual(
area_map.find_obstruction_when_transforming_line(base, start, end),
(5. / 7., vector.GridPoint(6, 6)))
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 __project_point_on_line(p, a, b):
""" project the point p on the line a->b """
# care about the degenerate case:
if a == b:
return a
# a->b is given by a + t(b-a)
# we want to project it on the line, i.e. project
# p - a on t(b - a). we can do this by multiplying
# the equation with (b - a) and solve for t:
t = (p - a) * (b - a) / (b - a).length() ** 2
# clamp t to [0,1]
if t > 1.: t = 1.
if t < 0.: t = 0.
# compute the point p
p = a + t * (b - a)
return vector.PointF(p.x, p.y)
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 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]))
def test_find_gridpoints_in_triangle_iterator(self):
""" test find_gridpoints_in_triangle_iterator """
# shortcut
GridPoint = vector.GridPoint
# create map
base_map = map_base.MapBase(10, 10, [])
# triangle (draw it!)
a = vector.PointF(1, 1)
b = vector.PointF(3, 1)
c = vector.PointF(2, 3)
result = list(base_map.find_gridpoints_in_triangle_iterator(a, b, c))
expected = [
GridPoint(1, 1),
GridPoint(2, 1),
GridPoint(3, 1),
GridPoint(2, 2),
GridPoint(2, 3)
]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(1, 1)
b = vector.PointF(3, 1)
c = vector.PointF(1, 3)
result = list(base_map.find_gridpoints_in_triangle_iterator(a, b, c))
expected = [
GridPoint(1, 1),
GridPoint(2, 1),
GridPoint(3, 1),
GridPoint(1, 2),
GridPoint(2, 2),
GridPoint(1, 3)
]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(6, 4)
b = vector.PointF(3, 1)
c = vector.PointF(1, 3)
result = list(base_map.find_gridpoints_in_triangle_iterator(a, b, c))
expected = [
GridPoint(3, 1),
GridPoint(2, 2),
GridPoint(3, 2),
GridPoint(4, 2),
GridPoint(1, 3),
GridPoint(2, 3),
GridPoint(3, 3),
GridPoint(4, 3),
GridPoint(5, 3),
GridPoint(6, 4)
]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(5, 5)
b = vector.PointF(3, 1)
c = vector.PointF(1, 3)
result = list(base_map.find_gridpoints_in_triangle_iterator(a, b, c))
expected = [
GridPoint(3, 1),
GridPoint(2, 2),
GridPoint(3, 2),
GridPoint(1, 3),
GridPoint(2, 3),
GridPoint(3, 3),
GridPoint(4, 3),
GridPoint(3, 4),
GridPoint(4, 4),
GridPoint(5, 5)
]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(1, 1)
b = vector.PointF(2, 1)
c = vector.PointF(3, 1)
result = list(base_map.find_gridpoints_in_triangle_iterator(a, b, c))
expected = []
self.assertEqual(result, expected)
def test_find_tiles_in_triangle_iterator(self):
""" test find_tiles_in_triangle_iterator """
# shortcut
GridTile = vector.GridTile
# create map
base_map = map_base.MapBase(10, 10, [])
# shortcut
f = base_map.find_tiles_in_triangle_iterator
# f = lambda a,b,c: sorted(base_map.find_tiles_in_triangle_iterator_slow(a,b,c), key=lambda t: (t.y,t.x))
# triangle (draw it!)
a = vector.PointF(1, 1)
b = vector.PointF(3, 1)
c = vector.PointF(2, 3)
result = list(f(a, b, c))
expected = [
GridTile(1, 1),
GridTile(2, 1),
GridTile(1, 2),
GridTile(2, 2)
]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(1, 1)
b = vector.PointF(2, 2)
c = vector.PointF(1, 3)
result = list(f(a, b, c))
expected = [GridTile(1, 1), GridTile(1, 2)]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(1, 1)
b = vector.PointF(2, 1.8)
c = vector.PointF(1, 3)
result = list(f(a, b, c))
expected = [GridTile(1, 1), GridTile(1, 2)]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(1, 1)
b = vector.PointF(2, 2.2)
c = vector.PointF(1, 3)
result = list(f(a, b, c))
expected = [GridTile(1, 1), GridTile(1, 2)]
self.assertEqual(result, expected)
# triangle (draw it!)
a = vector.PointF(1.8, 1.2)
b = vector.PointF(2.1, 1.2)
c = vector.PointF(2.1, 2.1)
result = list(f(a, b, c))
expected = [GridTile(1, 1), GridTile(2, 1), GridTile(2, 2)]
self.assertEqual(result, expected)
# triangle (draw it!)
# special case 2. i_y_max == i_y_mid
a = vector.PointF(1.2, 1.8)
b = vector.PointF(1.2, 2.1)
c = vector.PointF(2.1, 2.2)
result = list(f(a, b, c))
expected = [GridTile(1, 1), GridTile(1, 2), GridTile(2, 2)]
self.assertEqual(result, expected)
# triangle (draw it!)
# special case 2. i_y_max == i_y_mid
a = vector.PointF(1.2, 1.8)
b = vector.PointF(2.1, 2.1)
c = vector.PointF(1.2, 2.2)
result = list(f(a, b, c))
expected = [GridTile(1, 1), GridTile(1, 2), GridTile(2, 2)]
self.assertEqual(result, expected)
# triangle (draw it!)
# special case 1. i_y_min == i_y_mid
a = vector.PointF(1, 1)
b = vector.PointF(2.1, 1)
c = vector.PointF(1, 2)
result = list(f(a, b, c))
expected = [GridTile(1, 1), GridTile(2, 1)]
self.assertEqual(result, expected)
