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)