def find_supporting_line(point: TPoint, polygon: TPolygon, polygon_number: int) -> Optional[List[PointData]]:
"""
Find pair of supporting points from point (not a part of polygon) to polygon in squared time.
:param point: visibility point (x, y)
:param polygon: convex, given counter-clockwise, first and last points must be equal
:param polygon_number: sequence number of polygon for PointData
:return: list of PointData of points connecting supporting points or None if unable to find
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 2
assert compare_points(polygon[0], polygon[-1])
supporting_pair = find_supporting_pair_brute(point, polygon, polygon_size, None)
if supporting_pair is None:
return None
point1, point2 = supporting_pair
line = list()
if point2 - point1 > (polygon_size - 1) / 2:
for i in range(point2, polygon_size):
line.append((polygon[i], polygon_number, i, True, 0))
for i in range(0, point1 + 1):
line.append((polygon[i], polygon_number, i, True, 0))
else:
for i in range(point1, point2 + 1):
line.append((polygon[i], polygon_number, i, True, 0))
return line
def find_supporting_pair(
point: TPoint, polygon: TPolygon, polygon_number: int,
angles: Optional[TAngles]) -> Optional[Tuple[PointData, PointData]]:
"""
Find pair of supporting points from point to polygon in log time (binary search).
:param point: visibility point (x, y)
:param polygon: convex, given counter-clockwise, first and last points must be equal
:param polygon_number: sequence number of polygon for PointData
:param angles: tuple of polar angles from #0 point of polygon to all others except itself
:return: pair of PointData of supporting points or None if unable to find
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 2
assert compare_points(polygon[0], polygon[-1])
if polygon_size <= 3:
return find_supporting_pair_semiplanes(point, polygon, polygon_number)
assert turn(polygon[0], polygon[1], polygon[2]) >= 0
assert len(angles) == polygon_size - 1
start_to_point = localize_convex(point, polygon, angles, False)
# ray polygon[0], point intersects polygon
if start_to_point[1] is not None:
index1 = find_supporting_point(point, polygon, 0, start_to_point[1],
True)
if index1 is None:
return None
index2 = find_supporting_point(point, polygon, start_to_point[1],
polygon_size - 1, False)
if index2 is None:
return None
else:
point_to_start = localize_convex(point, polygon, angles, True)
# ray polygon[0], point does not intersect polygon
if point_to_start[1] is not None:
index1 = find_supporting_point(point, polygon, 0,
point_to_start[1], False)
if index1 is None:
return None
index2 = find_supporting_point(point, polygon, point_to_start[1],
polygon_size - 1, True)
if index2 is None:
return None
# polygon[0] is supporting point
else:
index1 = 0
index2 = find_supporting_point(
point, polygon, 1, polygon_size - 1,
turn(polygon[0], polygon[1], point) >= 0)
if index2 is None:
return None
return (polygon[index1], polygon_number, index1, True,
0), (polygon[index2], polygon_number, index2, True, 0)
def __retrace(self) -> None:
current = self.__goal
while not compare_points(current, self.__start):
self.__path.append(current)
try:
current = self.__came_from[current]
except KeyError:
raise Exception("Could not retrace path!")
self.__path.append(self.__start)
self.__path.reverse()
def find_inner_edges(point: TPoint, point_number: Optional[int],
polygon: Sequence[TPolygon], polygon_number: int,
inside_percent: float, weight: int) -> List[PointData]:
"""
Finds segments from point to polygon vertices which are strictly inside polygon.
If point is not a polygon vertex, finds all segments.
If point is a polygon vertex, finds diagonals to further vertices.
Currently only outer polygon is processed => everywhere polygon[0] is used.
:param point: point strictly inside outer polygon (x, y)
:param point_number: None if point is not a polygon vertex else number or vertex
:param polygon: polygons (polygon[0] is outer, rest are inner), for each polygon first and last points must be equal
:param polygon_number: sequence number of polygon for PointData
:param inside_percent: (from 0 to 1) - controls the number of inner polygon edges
:param weight: surface weight for PointData
:return: list of PointData tuples of each point forming an inner edge with point
"""
assert 0 <= inside_percent <= 1
polygon_size = len(polygon[0]) - 1
assert polygon_size >= 2
assert compare_points(polygon[0][0], polygon[0][-1])
edges_inside = list()
# point is strictly in polygon
if point_number is None:
for i in range(polygon_size):
if Polygon(polygon[0]).contains(LineString([point,
polygon[0][i]])):
if inside_percent == 1 or choice(
arange(0, 2), p=[1 - inside_percent, inside_percent
]) == 1:
edges_inside.append(
(polygon[0][i], polygon_number, i, True, weight))
return edges_inside
for i in range(polygon_size):
if i == point_number:
continue
if fabs(i - point_number) in [1, polygon_size - 1]:
edges_inside.append(
(polygon[0][i], polygon_number, i, True, weight))
continue
if Polygon(polygon[0]).contains(LineString([point, polygon[0][i]])):
if inside_percent == 1 or choice(
arange(0, 2), p=[1 - inside_percent, inside_percent]) == 1:
edges_inside.append(
(polygon[0][i], polygon_number, i, True, weight))
return edges_inside
def localize_convex_linear(point: TPoint, polygon: TPolygon) -> bool:
"""
Localize point inside convex polygon in linear time.
:param point: point to be localized (x, y)
:param polygon: convex, given counter-clockwise, first and last points must be equal
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 2
assert compare_points(polygon[0], polygon[-1])
if polygon_size <= 2:
return False
polygon_cross = turn(polygon[0], polygon[1], polygon[2])
for i in range(polygon_size):
if turn(polygon[i], polygon[i + 1], point) * polygon_cross <= 0:
return False
return True
def find_restriction_pair(point: TPoint, polygon: TPolygon, point_number: int) -> Optional[Tuple[TPoint, TPoint]]:
"""
Find pair of supporting points from point (part of polygon) to polygon in squared time.
:param point: visibility point (x, y)
:param polygon: convex, given counter-clockwise, first and last points must be equal
:param point_number: sequence number of point in polygon
:return: pair of supporting points or None if unable to find
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 2
assert compare_points(polygon[0], polygon[-1])
supporting_pair = find_supporting_pair_brute(point, polygon, polygon_size, point_number)
if supporting_pair is None:
return None
point1, point2 = supporting_pair
return polygon[point1], polygon[point2]
def __remove_inner_polygons(self):
polygon_number = self.polygons.shape[0]
for i in range(polygon_number):
if self.polygons.loc[i, "geometry"] is None:
continue
polygon = self.polygons.loc[i, "geometry"]
for j in range(1, len(polygon)):
point = polygon[j][0]
for k in range(i + 1, polygon_number):
if self.polygons.loc[k, "geometry"] is None:
if k == polygon_number - 1:
self.polygons.loc[i, "tag"].append(None)
continue
new_point = self.polygons.loc[k, "geometry"][0][0]
if compare_points(point, new_point):
self.polygons.loc[i, "tag"].append(
self.polygons.loc[k, "tag"])
self.polygons.loc[k, "geometry"] = None
elif k == polygon_number - 1:
self.polygons.loc[i, "tag"].append(None)
self.polygons = self.polygons[self.polygons['geometry'].notna()]
self.polygons = self.polygons.reset_index().drop(columns='index')
def find_supporting_point(point: TPoint, polygon: TPolygon, low: int,
high: int, low_contains: bool) -> Optional[int]:
"""
Find supporting point from point to polygon (index between low and high) with binary search.
:param point: visibility point (x, y)
:param polygon: convex, given counter-clockwise, first and last points must be equal
:param low: binary search algorithm parameter (polygon min index)
:param high: binary search algorithm parameter (polygon max index)
:param low_contains: angle formed by polygon[low] contains point (True) or not (False)
:return: index of supporting point from point to polygon or None if unable to find
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 3
assert compare_points(polygon[0], polygon[-1])
assert turn(polygon[0], polygon[1], polygon[2]) >= 0
mid = (high + low) // 2
while low <= high and mid < polygon_size:
# supporting point separating 2 subsets is found
if turn(polygon[mid], polygon[(mid + 1) % polygon_size], point) <= 0 and \
turn(polygon[(mid - 1) % polygon_size], polygon[mid], point) >= 0 or \
turn(polygon[mid], polygon[(mid + 1) % polygon_size], point) >= 0 and \
turn(polygon[(mid - 1) % polygon_size], polygon[mid], point) <= 0:
return mid
# update mid
if low_contains and turn(polygon[mid], polygon[(mid + 1) % polygon_size], point) >= 0 or \
not low_contains and turn(polygon[mid], polygon[(mid + 1) % polygon_size], point) <= 0:
low = mid + 1
else:
high = mid - 1
mid = (high + low) // 2
return None
def find_supporting_pair_semiplanes(point: TPoint, polygon: TPolygon,
polygon_number: int) -> Optional[tuple]:
"""
Find pair of supporting points from point to polygon in linear line.
:param point: visibility point (x, y)
:param polygon: convex, first and last points must be equal
:param polygon_number: sequence number of polygon for PointData
:return: pair of PointData of supporting points or None if unable to find
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 2
assert compare_points(polygon[0], polygon[-1])
if polygon_size == 2:
return (polygon[0], polygon_number, 0, True,
0), (polygon[1], polygon_number, 1, True, 0)
semiplanes = [1] * polygon_size
count = 0
polygon_turn = turn(polygon[0], polygon[1], polygon[2])
# fill an array of angles containing (1) or not containing (0) point (2 subsets)
for i in range(polygon_size):
if turn(polygon[i % polygon_size], polygon[
(i + 1) % polygon_size], point) * polygon_turn < 0:
semiplanes[i] = 0
count += 1
if count in (0, polygon_size):
return None
start = semiplanes.index(1) if semiplanes[0] == 0 else semiplanes.index(0)
end = (len(semiplanes) - semiplanes[::-1].index(1)) % polygon_size if semiplanes[0] == 0 else \
(len(semiplanes) - semiplanes[::-1].index(0)) % polygon_size
return (polygon[start], polygon_number, start, True,
0), (polygon[end], polygon_number, end, True, 0)
def build_convex_hull(
polygon: TPolygon
) -> Tuple[TPolygon, Tuple[int, ...], Optional[TAngles]]:
"""
:param polygon: first and last points must be equal
:return: tuple of 3 elements:
1. tuple of convex hull points
2. tuple of their indexes in initial polygon
3. tuple of polar angles from first point to others except itself or None if polygon is a segment or a point
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 2
assert compare_points(polygon[0], polygon[-1])
if polygon_size == 2:
return polygon, tuple([i for i in range(len(polygon) - 1)]), None
if polygon_size == 3:
polygon = check_polygon_direction(polygon)
starting_point = polygon[0]
angles = [polar_angle(starting_point, vertex) for vertex in polygon]
angles.pop(0)
angles.pop()
return polygon, tuple([i for i in range(len(polygon) - 1)
]), tuple(angles)
ch = ConvexHull(polygon)
vertices = list(ch.vertices)
vertices.append(vertices[0])
points = ch.points[vertices]
points = check_polygon_direction(points)
points = tuple([tuple(point) for point in points])
vertices.pop()
return points, tuple(vertices), calculate_angles(points)
def localize_convex(point: TPoint,
polygon: TPolygon,
angles: Optional[TAngles],
reverse_angle: bool = False) -> Tuple[bool, Optional[int]]:
"""
Localize point inside convex polygon in log time (Preparata-Shamos algorithm).
:param point: point to be localized (x, y)
:param polygon: convex, given counter-clockwise, first and last points must be equal
:param angles: tuple of polar angles from #0 point of polygon to all others except itself
:param reverse_angle: point angles should be turned on pi (True) or not (False)
:return: tuple of 2 elements:
1. bool - point is inside polygon
2. None if point is inside polygon else int - number of polygon vertex where point_angle is located.
"""
polygon_size = len(polygon) - 1
assert polygon_size >= 2
assert compare_points(polygon[0], polygon[-1])
if polygon_size <= 2:
return False, None
assert turn(polygon[0], polygon[1], polygon[2]) >= 0
assert len(angles) == polygon_size - 1
if compare_points(point, polygon[0]):
return True, None
point_angle = polar_angle(point,
polygon[0]) if reverse_angle else polar_angle(
polygon[0], point)
# point angle not between angles
if angles[-1] < pi < angles[0]:
if angles[-1] <= point_angle <= angles[0]:
return False, None
elif point_angle <= angles[0] or point_angle >= angles[-1]:
return False, None
angles_len = len(angles)
mid = (angles_len - 1) // 2
low = 0
high = angles_len - 1
while low <= high:
if mid + 1 == angles_len:
return False, None
angle1 = angles[mid]
angle2 = angles[mid + 1]
# 2 angles contain zero-angle
if angle1 > pi > angle2:
if point_angle >= angle1 or point_angle <= angle2:
return turn(polygon[mid + 1], polygon[mid + 2],
point) > 0, mid + 2
if point_angle > pi:
high = mid - 1
if point_angle < pi:
low = mid + 1
else:
if angle1 <= point_angle <= angle2:
return turn(polygon[mid + 1], polygon[mid + 2],
point) > 0, mid + 2
if point_angle - pi > angle2:
high = mid - 1
elif point_angle + pi < angle1:
low = mid + 1
elif point_angle < angle1:
high = mid - 1
else:
low = mid + 1
mid = (high + low) // 2
return False, None
def find(self, start, goal, default_weight=10, heuristic_multiplier=10):
"""
Find route from point start to point goal.
:param TPoint start: start point
:param TPoint goal: goal point
:param int default_weight: default weight for unfilled areas
:param int heuristic_multiplier: multiplier to weight heuristic (http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#scale)
:rtype: offroad_routing.pathfinding.path.Path
"""
# PointData format
start_data = (start, None, None, None, None)
goal_data = (goal, None, None, None, None)
# vgraph nodes incident to goal point (defined by object and position in the object)
goal_neighbours = self.__vgraph.incident_vertices(goal_data)
if len(goal_neighbours) == 0:
raise Exception("Goal point has no neighbours on the graph")
goal_data = (goal, None, None, None, goal_neighbours[0][4])
goal_neighbours = [(i[1], i[2], i[3]) for i in goal_neighbours]
frontier = PriorityQueue()
frontier.put(start_data, 0)
came_from = dict()
came_from[start_data[0]] = None
cost_so_far = dict()
cost_so_far[start_data[0]] = 0
while not frontier.empty():
current = frontier.get()
current_point = current[0]
if compare_points(current_point, goal):
break
neighbours = self.__vgraph.incident_vertices(current)
# if current is goal neighbour add it to neighbour list
goal_neighbour = (current[1], current[2],
current[3]) in goal_neighbours
if goal_neighbour:
neighbours.insert(0, goal_data)
for neighbour in neighbours:
neighbour_point = neighbour[0]
neighbour_weight = neighbour[
4] if neighbour[4] > 0 else default_weight
new_cost = cost_so_far[current_point] + point_distance(
current_point, neighbour_point) * neighbour_weight
# neighbour not visited or shorter path found
if neighbour_point not in cost_so_far or new_cost < cost_so_far[
neighbour_point]:
cost_so_far[neighbour_point] = new_cost
priority = new_cost + self.__heuristic(
goal, neighbour_point, heuristic_multiplier)
frontier.put(neighbour, priority)
came_from[neighbour_point] = current_point
return Path(came_from, start, goal)
