|
| 1 | +# MIT License |
| 2 | + |
| 3 | +# Copyright (c) 2017 Michal Haták |
| 4 | +# Copyright (c) 2025 Bob Myhill |
| 5 | + |
| 6 | +# Permission is hereby granted, free of charge, to any person obtaining a copy |
| 7 | +# of this software and associated documentation files (the "Software"), to deal |
| 8 | +# in the Software without restriction, including without limitation the rights |
| 9 | +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| 10 | +# copies of the Software, and to permit persons to whom the Software is |
| 11 | +# furnished to do so, subject to the following conditions: |
| 12 | + |
| 13 | +# The above copyright notice and this permission notice shall be included in all |
| 14 | +# copies or substantial portions of the Software. |
| 15 | + |
| 16 | +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| 17 | +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| 18 | +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| 19 | +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| 20 | +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| 21 | +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
| 22 | +# SOFTWARE. |
| 23 | + |
| 24 | +import numpy as np |
| 25 | +import heapq |
| 26 | + |
| 27 | + |
| 28 | +def _get_seg_dist_sq(px, py, a, b): |
| 29 | + """ |
| 30 | + Compute the squared distance from point (px, py) to the line segment defined by points a and b. |
| 31 | +
|
| 32 | + :param px: X-coordinate of the point. |
| 33 | + :param py: Y-coordinate of the point. |
| 34 | + :param a: First point of the segment as a tuple (x, y). |
| 35 | + :param b: Second point of the segment as a tuple (x, y). |
| 36 | + :return: Squared distance from point to segment. |
| 37 | + :rtype: float |
| 38 | + """ |
| 39 | + ax, ay = a |
| 40 | + bx, by = b |
| 41 | + dx, dy = bx - ax, by - ay |
| 42 | + |
| 43 | + if dx != 0 or dy != 0: |
| 44 | + t = ((px - ax) * dx + (py - ay) * dy) / (dx * dx + dy * dy) |
| 45 | + if t > 1: |
| 46 | + ax, ay = bx, by |
| 47 | + elif t > 0: |
| 48 | + ax += dx * t |
| 49 | + ay += dy * t |
| 50 | + |
| 51 | + return (px - ax) ** 2 + (py - ay) ** 2 |
| 52 | + |
| 53 | + |
| 54 | +def _point_to_polygon_distance(x, y, polygon): |
| 55 | + """ |
| 56 | + Compute the signed distance from a point to the nearest polygon edge. |
| 57 | +
|
| 58 | + :param x: X-coordinate of the point. |
| 59 | + :param y: Y-coordinate of the point. |
| 60 | + :param polygon: List of rings (each ring is a numpy array of shape (N, 2)). |
| 61 | + :return: Signed distance; positive if inside, negative if outside. |
| 62 | + :rtype: float |
| 63 | + """ |
| 64 | + inside = False |
| 65 | + min_dist_sq = np.inf |
| 66 | + |
| 67 | + for ring in polygon: |
| 68 | + b = ring[-1] |
| 69 | + for a in ring: |
| 70 | + if ((a[1] > y) != (b[1] > y)) and ( |
| 71 | + x < (b[0] - a[0]) * (y - a[1]) / (b[1] - a[1]) + a[0] |
| 72 | + ): |
| 73 | + inside = not inside |
| 74 | + min_dist_sq = min(min_dist_sq, _get_seg_dist_sq(x, y, a, b)) |
| 75 | + b = a |
| 76 | + |
| 77 | + distance = np.sqrt(min_dist_sq) |
| 78 | + return distance if inside else -distance |
| 79 | + |
| 80 | + |
| 81 | +class Cell: |
| 82 | + """ |
| 83 | + Represents a square cell used during the polygon center search. |
| 84 | +
|
| 85 | + :param x: X-coordinate of the cell center. |
| 86 | + :param y: Y-coordinate of the cell center. |
| 87 | + :param h: Half-size of the cell. |
| 88 | + :param polygon: The input polygon as a list of rings. |
| 89 | + """ |
| 90 | + |
| 91 | + def __init__(self, x, y, h, polygon): |
| 92 | + self.x = x |
| 93 | + self.y = y |
| 94 | + self.h = h |
| 95 | + self.d = _point_to_polygon_distance(x, y, polygon) |
| 96 | + self.max = self.d + h * np.sqrt(2) |
| 97 | + |
| 98 | + def __lt__(self, other): |
| 99 | + return self.max > other.max |
| 100 | + |
| 101 | + |
| 102 | +def closest_point_on_segment(p, a, b): |
| 103 | + """ |
| 104 | + Return the closest point on segment ab to point p. |
| 105 | + p, a, b: numpy arrays of shape (2,) |
| 106 | + """ |
| 107 | + ab = b - a |
| 108 | + ap = p - a |
| 109 | + |
| 110 | + ab_len_sq = np.dot(ab, ab) |
| 111 | + if ab_len_sq < np.finfo(float).eps: |
| 112 | + return a.copy() |
| 113 | + else: |
| 114 | + t = np.dot(ap, ab) / ab_len_sq |
| 115 | + t = np.clip(t, 0, 1) # constrain t to [0, 1] |
| 116 | + return a + t * ab |
| 117 | + |
| 118 | + |
| 119 | +def closest_point_on_polygon(p, polygon): |
| 120 | + """ |
| 121 | + Find the closest point on a polygon to point p. |
| 122 | +
|
| 123 | + :param p: np.array of shape (2,) - the target point |
| 124 | + :param polygon: np.array of shape (N, 2) - the polygon vertices |
| 125 | + (assumed closed or will be treated as closed) |
| 126 | + :return: np.array of shape (2,) - closest point on polygon |
| 127 | + """ |
| 128 | + min_dist = np.inf |
| 129 | + closest_point = None |
| 130 | + num_points = polygon.shape[0] |
| 131 | + for i in range(num_points): |
| 132 | + a = polygon[i] |
| 133 | + b = polygon[(i + 1) % num_points] # wrap around |
| 134 | + proj = closest_point_on_segment(p, a, b) |
| 135 | + dist = np.linalg.norm(p - proj) |
| 136 | + if dist < min_dist: |
| 137 | + min_dist = dist |
| 138 | + closest_point = proj |
| 139 | + |
| 140 | + return closest_point |
| 141 | + |
| 142 | + |
| 143 | +def _get_centroid_cell(polygon): |
| 144 | + """ |
| 145 | + Estimate the polygon's centroid as an initial guess. |
| 146 | +
|
| 147 | + :param polygon: List of polygon rings. |
| 148 | + :return: A Cell object located at the estimated centroid. |
| 149 | + :rtype: Cell |
| 150 | + """ |
| 151 | + points = polygon[0] |
| 152 | + area = 0.0 |
| 153 | + cx = 0.0 |
| 154 | + cy = 0.0 |
| 155 | + b = points[-1] |
| 156 | + |
| 157 | + for a in points: |
| 158 | + f = a[0] * b[1] - b[0] * a[1] |
| 159 | + cx += (a[0] + b[0]) * f |
| 160 | + cy += (a[1] + b[1]) * f |
| 161 | + area += f * 3 |
| 162 | + b = a |
| 163 | + |
| 164 | + if area == 0: |
| 165 | + midpoint = (np.min(points, axis=0) + np.max(points, axis=0)) / 2 |
| 166 | + closest = closest_point_on_polygon(midpoint, points) |
| 167 | + return Cell(closest[0], closest[1], 0.0, polygon) |
| 168 | + |
| 169 | + return Cell(cx / area, cy / area, 0.0, polygon) |
| 170 | + |
| 171 | + |
| 172 | +def visual_center_of_polygon(polygon_rings, precision=1.0, with_distance=False): |
| 173 | + """ |
| 174 | + Compute the pole of inaccessibility (visual center) of a polygon with the specified precision. |
| 175 | +
|
| 176 | + :param polygon_rings: A polygon represented as a list of rings. Each ring is a numpy array of shape (N, 2). |
| 177 | + :param precision: Desired precision. Stops when improvement is less than this value. |
| 178 | + :param with_distance: If True, also return the distance to the closest edge. |
| 179 | + :return: The [x, y] coordinates of the center, and optionally the distance. |
| 180 | + :rtype: list or tuple |
| 181 | + """ |
| 182 | + coords = polygon_rings[0] |
| 183 | + if coords.ndim != 2 or coords.shape[1] != 2: |
| 184 | + raise ValueError("Expected polygon ring to be an Nx2 array") |
| 185 | + |
| 186 | + min_x, min_y = np.min(coords, axis=0) |
| 187 | + max_x, max_y = np.max(coords, axis=0) |
| 188 | + |
| 189 | + width = max_x - min_x |
| 190 | + height = max_y - min_y |
| 191 | + cell_size = min(width, height) |
| 192 | + max_dim = max(width, height) |
| 193 | + |
| 194 | + h = cell_size / 2.0 |
| 195 | + |
| 196 | + # If the cell is much longer than it is wide (or vice-versa), |
| 197 | + # just return the mean of x and y. |
| 198 | + if cell_size < max_dim / 100: |
| 199 | + mean_x = (max_x + min_x) / 2.0 |
| 200 | + mean_y = (max_y + min_y) / 2.0 |
| 201 | + return ([mean_x, mean_y], 0.0) if with_distance else [mean_x, mean_y] |
| 202 | + |
| 203 | + # Initialize priority queue |
| 204 | + queue = [] |
| 205 | + heapq.heapify(queue) |
| 206 | + |
| 207 | + x_coords = np.arange(min_x, max_x, cell_size) |
| 208 | + y_coords = np.arange(min_y, max_y, cell_size) |
| 209 | + |
| 210 | + for x in x_coords: |
| 211 | + for y in y_coords: |
| 212 | + heapq.heappush(queue, Cell(x + h, y + h, h, polygon_rings)) |
| 213 | + |
| 214 | + best_cell = _get_centroid_cell(polygon_rings) |
| 215 | + |
| 216 | + bbox_cell = Cell(min_x + width / 2, min_y + height / 2, 0.0, polygon_rings) |
| 217 | + if bbox_cell.d > best_cell.d: |
| 218 | + best_cell = bbox_cell |
| 219 | + |
| 220 | + while queue: |
| 221 | + cell = heapq.heappop(queue) |
| 222 | + |
| 223 | + if cell.d > best_cell.d: |
| 224 | + best_cell = cell |
| 225 | + |
| 226 | + if cell.max - best_cell.d <= precision: |
| 227 | + continue |
| 228 | + |
| 229 | + h = cell.h / 2 |
| 230 | + for dx in [-h, h]: |
| 231 | + for dy in [-h, h]: |
| 232 | + heapq.heappush(queue, Cell(cell.x + dx, cell.y + dy, h, polygon_rings)) |
| 233 | + |
| 234 | + result = [best_cell.x, best_cell.y] |
| 235 | + return (result, best_cell.d) if with_distance else result |
0 commit comments