def shortest_path(polygon: Polygon, s: Point, t: Point) -> Iterable[Point]:
"""Find the shortest path from s to t inside polygon."""
# ==========================================================================
# Reset properties which can be accessed later on.
# ==========================================================================
shortest_path.properties = dict(iterations=0, jarvis_marches=0, predicates=0, ignores=0, ignores_theo=0)
# ==========================================================================
# Type checking.
# ==========================================================================
assert isinstance(polygon, Polygon)
assert isinstance(s, Point)
assert isinstance(t, Point)
# ==========================================================================
# Trivial case: s == t.
# ==========================================================================
# In the very trivial case the start and end point are identical thus we can
# just return without any calculation.
if s == t:
yield s
return
# ==========================================================================
# Locate s and t. Trivial case: both in same triangle.
# ==========================================================================
# Locate start and end point inside our polygon.
s_triangle = polygon.locate_point_in_triangle(s)
t_triangle = polygon.locate_point_in_triangle(t)
# If any point is not inside the polygon return
if s_triangle is None or t_triangle is None:
return
# If both points are located inside the same triangle just return both in order
if s_triangle == t_triangle:
yield s
yield t
return
# ==========================================================================
# Preparation.
# ==========================================================================
# The cusp is the point we are always standing at and from which we can see the triangle boundaries we are visiting.
# It gets updated in case we lose visibility. We obviously start at the starting point.
cusp = s
# The funnel is our visibility angle.
funnel = None
# We also need to save the trapezoid we are currently in.
current_triangle = s_triangle
previous_triangle = current_triangle
boundary = None
# We choose the first neighbour to look at
start_neighbour = polygon.delaunay_first_neighbour(current_triangle)
# ==========================================================================
# Walking the triangles.
# ==========================================================================
while current_triangle != t_triangle:
shortest_path.properties['iterations'] += 1
previous_triangle = current_triangle
current_triangle = parallel_find_feasible_subtree(polygon, previous_triangle, start_neighbour, s_triangle,
t_triangle)
previous_boundary = boundary
# Get the boundary between the old and the new current triangle
boundary = current_triangle.common_edge(previous_triangle)
# Edges need to be oriented in counter-clockwise direction
if Point.turn(cusp, boundary.a, boundary.b) == Point.CW_TURN:
boundary.reverse()
elif Point.turn(cusp, boundary.a, boundary.b) == Point.NO_TURN:
# FIXME: We should do something if edge is aligned with cusp
# Idea: Look at boundary from third point of previous triangle
previous_triangle_points = list(previous_triangle.points)
previous_triangle_points.remove(boundary.a)
previous_triangle_points.remove(boundary.b)
assert len(previous_triangle_points) == 1
previous_triangle_point = previous_triangle_points[0]
if Point.turn(previous_triangle_point, boundary.a, boundary.b) == Point.CW_TURN:
boundary.reverse()
# On encountering the first boundary we do not have a funnel yet. We
# then create it and start looking for the next trapezoid
if funnel is None:
funnel = Funnel(cusp, boundary.a, boundary.b)
# ----------------------------------------------------------------------
# Checking (and possibly updating) the visibility.
# ----------------------------------------------------------------------
# Check where both boundary end points are in respect to the funnel
position_of_a = funnel.position_of(boundary.a)
position_of_b = funnel.position_of(boundary.b)
# Save whether both end points are on the same side of the funnel
both_right_of = position_of_a == position_of_b == Funnel.RIGHT_OF
both_left_of = position_of_a == position_of_b == Funnel.LEFT_OF
# ----------------------------------------------------------------------
# CASE 1: We do not see the boundary any more.
# ----------------------------------------------------------------------
if both_left_of or both_right_of:
# The current view point will definitely change now
yield cusp
# ------------------------------------------------------------------
# Prepare the Jarvis march
# ------------------------------------------------------------------
shortest_path.properties['jarvis_marches'] += 1
params = prepare_jarvis_march(polygon, funnel, current_triangle,
both_right_of, boundary)
# ------------------------------------------------------------------
# Actually perform the Jarvis march
# ------------------------------------------------------------------
if both_right_of:
ignore_func = ignore_function(previous_boundary or boundary, funnel, Funnel.RIGHT_OF)
else:
ignore_func = ignore_function(previous_boundary or boundary, funnel, Funnel.LEFT_OF)
# Since polygon.point_sees_edge2 returns a tuple of the two funnel
# points we directly extract them. Additionally we get the new cusp
# and yield all vertices visited until finding cusp.
(v1, v2), cusp = yield from jarvis_march(
polygon=polygon,
predicate=partial(polygon.point_sees_edge2, edge=boundary),
ignore=ignore_func,
**params
)
# ------------------------------------------------------------------
# Update the cusp and the funnel
# ------------------------------------------------------------------
# In the special case in which the cusp falls together with an end
# point of our edge we advance the funnel point on the next edge
# into the right direction.
# (We only compare cusp with v1 since polygon.point_sees_edge
# guarantees to return the funnel point which falls together first.)
if v1 == cusp:
triangle_points = current_triangle.points
assert v1 in triangle_points
assert v2 in triangle_points
# Going in clockwise direction
if params['direction'] == 1:
v1 = polygon.point(cusp.index + 1)
else:
v1 = polygon.point(cusp.index - 1)
# We need to swap v1 and v2 to have them in the right order
v1, v2 = v2, v1
funnel.cusp = cusp
funnel.first = v1
funnel.second = v2
# ----------------------------------------------------------------------
# CASE 2: We see the boundary but we need to shrink the visibility.
# ----------------------------------------------------------------------
else:
# If needed (i.e. if the edge reduces the funnel) update the
# second and first funnel point
if position_of_a == Funnel.INSIDE:
funnel.first = boundary.a
if position_of_b == Funnel.INSIDE:
funnel.second = boundary.b
start_neighbour = polygon.delaunay_next_neighbour(current_triangle, previous_triangle)
# ==========================================================================
# Do the final Jarvis march
# ==========================================================================
yield cusp
if not polygon.point_sees_other_point(cusp, t):
shortest_path.properties['jarvis_marches'] += 1
params = prepare_jarvis_march(polygon, funnel, current_triangle, funnel.position_of(t) == Funnel.RIGHT_OF,
current_triangle.common_edge(previous_triangle))
# ------------------------------------------------------------------
# Actually perform the Jarvis march
# ------------------------------------------------------------------
# Since we are nearly finished we only care about the list of visited
# nodes and the new cusp (which is not contained in the list)
_, cusp = yield from jarvis_march(
polygon=polygon,
predicate=partial(polygon.point_sees_other_point, other_point=t),
ignore=ignore_function(boundary, funnel, funnel.position_of(t)),
**params
)
yield cusp
yield t
def jarvis_march(polygon: Polygon, start_index: int, end_index: int, direction: int, good_turn: int,
predicate: Callable[[Point], T], ignore: Callable[[Point], bool]=lambda x: False
) -> Iterable[Point]:
"""Do a Jarvis march on the given polygon.
We start at start_index going into direction stopping at end_index. For
every vertex we consider appropriate predicate is applied. If it yields
something which not evaluates to False we immediately stop the march and
return the predicate result together with the vertex and a list of all
points visited beforehand.
Args:
ignore: A function which is called for every vertex and should return True iff this vertex is to be ignored
during the jarvis march.
polygon: A polygon.
start_index: The index of the starting vertex.
end_index: The index of the last vertex to consider.
direction: The direction in which we walk along the polygon edge. Needs
to be either 1 or -1.
good_turn: If the current, the next and a third vertex form a turn that
is the same as good_turn, the third will be chosen over the next.
predicate: A function that takes a vertex as an argument and decides
whether to continue the march or stop.
Returns:
A 3-tuple (result, point, visited) in which result is the result of the
predicate function, point is the point which fulfils the predicate and
visited is a list of vertices visited in between.
Raises:
AssertionError:
a) A type check fails.
b) None of the vertices in the specified range fulfilled
the predicate.
"""
# ==========================================================================
# Type checking.
# ==========================================================================
assert isinstance(polygon, Polygon)
assert isinstance(start_index, int)
assert isinstance(end_index, int)
assert isinstance(direction, int)
assert isinstance(good_turn, int)
first = polygon.point(start_index)
while True:
shortest_path.properties['predicates'] += 1
result = predicate(first)
# If the result does not evaluate to False return
if result:
return result, first
# If this assertion fails none of the vertices fulfilled the predicate
assert first.index != end_index
second = polygon.point(first.index + direction)
if second.index != end_index:
for index in polygon.indices(second.index + direction, end_index,
direction):
point = polygon.point(index)
shortest_path.properties['ignores_theo'] += 1
if Point.turn(first, second, point) == good_turn:
shortest_path.properties['ignores'] += 1
if not ignore(point):
second = polygon.point(index)
yield first
first = second
def ignore(point):
return (Point.turn(boundary.a, boundary.b, point) == Point.CW_TURN or
funnel.position_of(point) != good_position)
