sumolib.geomhelper
1# Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo 2# Copyright (C) 2013-2026 German Aerospace Center (DLR) and others. 3# This program and the accompanying materials are made available under the 4# terms of the Eclipse Public License 2.0 which is available at 5# https://www.eclipse.org/legal/epl-2.0/ 6# This Source Code may also be made available under the following Secondary 7# Licenses when the conditions for such availability set forth in the Eclipse 8# Public License 2.0 are satisfied: GNU General Public License, version 2 9# or later which is available at 10# https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html 11# SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later 12 13# @file geomhelper.py 14# @author Daniel Krajzewicz 15# @author Jakob Erdmann 16# @author Michael Behrisch 17# @author Matthias Schwamborn 18# @date 2013-02-25 19 20from __future__ import absolute_import 21import math 22import sys 23 24INVALID_DISTANCE = -1 25 26# back-ported from python 3 for backward compatibility 27# https://www.python.org/dev/peps/pep-0485/#proposed-implementation 28 29 30def isclose(a, b, rel_tol=1e-09, abs_tol=0.0): 31 return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol) 32 33 34def distance(p1, p2): 35 dx = p1[0] - p2[0] 36 dy = p1[1] - p2[1] 37 return math.sqrt(dx * dx + dy * dy) 38 39 40def polyLength(polygon): 41 return sum([distance(a, b) for a, b in zip(polygon[:-1], polygon[1:])]) 42 43 44def addToBoundingBox(coordList, bbox=None): 45 if bbox is None: 46 minX = 1e400 47 minY = 1e400 48 maxX = -1e400 49 maxY = -1e400 50 else: 51 minX, minY, maxX, maxY = bbox 52 for x, y in coordList: 53 minX = min(x, minX) 54 minY = min(y, minY) 55 maxX = max(x, maxX) 56 maxY = max(y, maxY) 57 return minX, minY, maxX, maxY 58 59 60def isLeft(point, line_start, line_end): 61 return ((line_end[0] - line_start[0]) * (point[1] - line_start[1]) 62 < (point[0] - line_start[0]) * (line_end[1] - line_start[1])) 63 64 65def lineOffsetWithMinimumDistanceToPoint(point, line_start, line_end, perpendicular=False): 66 """Return the offset from line (line_start, line_end) where the distance to 67 point is minimal""" 68 d = distance(line_start, line_end) 69 u = ((point[0] - line_start[0]) * (line_end[0] - line_start[0]) 70 + (point[1] - line_start[1]) * (line_end[1] - line_start[1])) 71 if d == 0. or u < 0. or u > d * d: 72 if perpendicular: 73 return INVALID_DISTANCE 74 if u < 0.: 75 return 0. 76 return d 77 return u / d 78 79 80def polygonOffsetAndDistanceToPoint(point, polygon, perpendicular=False): 81 """Return the offset and the distance from the polygon start where the distance to the point is minimal""" 82 p = point 83 s = polygon 84 seen = 0 85 minDist = 1e400 86 minOffset = INVALID_DISTANCE 87 for i in range(len(s) - 1): 88 pos = lineOffsetWithMinimumDistanceToPoint( 89 p, s[i], s[i + 1], perpendicular) 90 dist = minDist if pos == INVALID_DISTANCE else distance( 91 p, positionAtOffset(s[i], s[i + 1], pos)) 92 if dist < minDist: 93 minDist = dist 94 minOffset = pos + seen 95 if perpendicular and i != 0 and pos == INVALID_DISTANCE: 96 # even if perpendicular is set we still need to check the distance 97 # to the inner points 98 cornerDist = distance(p, s[i]) 99 if cornerDist < minDist: 100 pos1 = lineOffsetWithMinimumDistanceToPoint( 101 p, s[i - 1], s[i], False) 102 pos2 = lineOffsetWithMinimumDistanceToPoint( 103 p, s[i], s[i + 1], False) 104 if pos1 == distance(s[i - 1], s[i]) and pos2 == 0.: 105 minOffset = seen 106 minDist = cornerDist 107 seen += distance(s[i], s[i + 1]) 108 return minOffset, minDist 109 110 111def polygonOffsetWithMinimumDistanceToPoint(point, polygon, perpendicular=False): 112 """Return the offset from the polygon start where the distance to the point is minimal""" 113 return polygonOffsetAndDistanceToPoint(point, polygon, perpendicular)[0] 114 115 116def distancePointToLine(point, line_start, line_end, perpendicular=False): 117 """Return the minimum distance between point and the line (line_start, line_end)""" 118 p1 = line_start 119 p2 = line_end 120 offset = lineOffsetWithMinimumDistanceToPoint( 121 point, line_start, line_end, perpendicular) 122 if offset == INVALID_DISTANCE: 123 return INVALID_DISTANCE 124 if offset == 0: 125 return distance(point, p1) 126 u = offset / distance(line_start, line_end) 127 intersection = (p1[0] + u * (p2[0] - p1[0]), p1[1] + u * (p2[1] - p1[1])) 128 return distance(point, intersection) 129 130 131def distancePointToPolygon(point, polygon, perpendicular=False): 132 """Return the minimum distance between point and polygon""" 133 p = point 134 s = polygon 135 minDist = None 136 for i in range(0, len(s) - 1): 137 dist = distancePointToLine(p, s[i], s[i + 1], perpendicular) 138 if dist == INVALID_DISTANCE and perpendicular and i != 0: 139 # distance to inner corner 140 dist = distance(point, s[i]) 141 if dist != INVALID_DISTANCE: 142 if minDist is None or dist < minDist: 143 minDist = dist 144 if minDist is not None: 145 return minDist 146 else: 147 return INVALID_DISTANCE 148 149 150def positionAtOffset(p1, p2, offset): 151 if isclose(offset, 0.): # for pathological cases with dist == 0 and offset == 0 152 return p1 153 154 dist = distance(p1, p2) 155 156 if isclose(dist, offset): 157 return p2 158 159 if offset > dist: 160 return None 161 162 return (p1[0] + (p2[0] - p1[0]) * (offset / dist), p1[1] + (p2[1] - p1[1]) * (offset / dist)) 163 164 165def indexAtShapeOffset(shape, offset): 166 """Returns the index of the shape segment which contains the offset and 167 the cumulated length of the shape up to the start point of the segment. 168 If the offset is less or equal to 0, it returns (0, 0.) If the offset is 169 larger than the shape length it returns (None, length of the shape)""" 170 seenLength = 0. 171 curr = shape[0] 172 for idx, p in enumerate(shape[1:]): 173 nextLength = distance(curr, p) 174 if seenLength + nextLength > offset: 175 return idx, seenLength 176 seenLength += nextLength 177 curr = p 178 return None, seenLength 179 180 181def positionAtShapeOffset(shape, offset): 182 idx, seen = indexAtShapeOffset(shape, offset) 183 if idx is None: 184 return shape[-1] 185 return positionAtOffset(shape[idx], shape[idx + 1], offset - seen) 186 187 188def rotationAtShapeOffset(shape, offset): 189 idx, _ = indexAtShapeOffset(shape, offset) 190 if idx is None: 191 return None 192 return math.atan2(shape[idx + 1][1] - shape[idx][1], shape[idx + 1][0] - shape[idx][0]) 193 194 195def angle2D(p1, p2): 196 theta1 = math.atan2(p1[1], p1[0]) 197 theta2 = math.atan2(p2[1], p2[0]) 198 dtheta = theta2 - theta1 199 while dtheta > math.pi: 200 dtheta -= 2.0 * math.pi 201 while dtheta < -math.pi: 202 dtheta += 2.0 * math.pi 203 return dtheta 204 205 206def angleTo2D(p1, p2): 207 return math.atan2(p2[1] - p1[1], p2[0] - p1[0]) 208 209 210def naviDegree(rad): 211 return normalizeAngle(math.degrees(math.pi / 2. - rad), 0, 360, 360) 212 213 214def fromNaviDegree(degrees): 215 return math.pi / 2. - math.radians(degrees) 216 217 218def normalizeAngle(a, lower, upper, circle): 219 while a < lower: 220 a = a + circle 221 while a > upper: 222 a = a - circle 223 return a 224 225 226def minAngleDegreeDiff(d1, d2): 227 return min(normalizeAngle(d1 - d2, 0, 360, 360), 228 normalizeAngle(d2 - d1, 0, 360, 360)) 229 230 231def isWithin(pos, shape): 232 """Returns whether the given pos coordinate is inside the polygon shape with ray-casting.""" 233 x, y = pos 234 inside = False 235 n = len(shape) 236 for i in range(n): 237 x1, y1 = shape[i] 238 x2, y2 = shape[(i + 1) % n] 239 240 if min(x1, x2) <= x <= max(x1, x2) and min(y1, y2) <= y <= max(y1, y2): 241 if abs((x2 - x1) * (y - y1) - (y2 - y1) * (x - x1)) < 1e-12: 242 return True 243 244 if (y1 > y) != (y2 > y) and x < (x2 - x1) * (y - y1) / (y2 - y1) + x1: 245 inside = not inside 246 247 return inside 248 249 250def sideOffset(fromPos, toPos, amount): 251 scale = amount / distance(fromPos, toPos) 252 return (scale * (fromPos[1] - toPos[1]), 253 scale * (toPos[0] - fromPos[0])) 254 255 256def sub(a, b): 257 return (a[0] - b[0], a[1] - b[1]) 258 259 260def add(a, b): 261 return (a[0] + b[0], a[1] + b[1]) 262 263 264def mul(a, x): 265 return (a[0] * x, a[1] * x) 266 267 268def dotProduct(a, b): 269 return a[0] * b[0] + a[1] * b[1] 270 271 272def crossProduct2D(a, b): 273 return a[0] * b[1] - a[1] * b[0] 274 275 276def orthoIntersection(a, b): 277 c = add(a, b) 278 quot = dotProduct(c, a) 279 if abs(quot) > 0.001: 280 return mul(mul(c, dotProduct(a, a)), 1 / quot) 281 else: 282 return None 283 284 285def rotateAround2D(p, rad, origin): 286 s = math.sin(rad) 287 c = math.cos(rad) 288 tmp = sub(p, origin) 289 tmp2 = [tmp[0] * c - tmp[1] * s, 290 tmp[0] * s + tmp[1] * c] 291 return add(tmp2, origin) 292 293 294def length(a): 295 return math.sqrt(dotProduct(a, a)) 296 297 298def norm(a): 299 return mul(a, 1 / length(a)) 300 301 302def narrow(fromPos, pos, toPos, amount): 303 """detect narrow turns which cannot be shifted regularly""" 304 a = sub(toPos, pos) 305 b = sub(pos, fromPos) 306 c = add(a, b) 307 dPac = dotProduct(a, c) 308 if dPac == 0: 309 return True 310 x = dotProduct(a, a) * length(c) / dPac 311 return x < amount 312 313 314def line2boundary(shape, width): 315 """expand center line by width/2 on both sides to obtain a (closed) boundary shape 316 (i.e. for an edge or lane) 317 """ 318 left = move2side(shape, width / 2) 319 right = move2side(shape, -width / 2) 320 321 return left + list(reversed(right)) + [left[0]] 322 323 324def move2side(shape, amount): 325 shape = [s for i, s in enumerate(shape) if i == 0 or shape[i-1] != s] 326 if len(shape) < 2: 327 return shape 328 if polyLength(shape) == 0: 329 return shape 330 result = [] 331 for i, pos in enumerate(shape): 332 if i == 0: 333 fromPos = pos 334 toPos = shape[i + 1] 335 if fromPos != toPos: 336 result.append(sub(fromPos, sideOffset(fromPos, toPos, amount))) 337 elif i == len(shape) - 1: 338 fromPos = shape[i - 1] 339 toPos = pos 340 if fromPos != toPos: 341 result.append(sub(toPos, sideOffset(fromPos, toPos, amount))) 342 else: 343 fromPos = shape[i - 1] 344 toPos = shape[i + 1] 345 # check for narrow turns 346 if narrow(fromPos, pos, toPos, amount): 347 # print("narrow at i=%s pos=%s" % (i, pos)) 348 pass 349 else: 350 a = sideOffset(fromPos, pos, -amount) 351 b = sideOffset(pos, toPos, -amount) 352 c = orthoIntersection(a, b) 353 if c is not None: 354 pos2 = add(pos, c) 355 else: 356 extend = norm(sub(pos, fromPos)) 357 pos2 = add(pos, mul(extend, amount)) 358 result.append(pos2) 359 # print("move2side", amount) 360 # print(shape) 361 # print(result) 362 # print() 363 return result 364 365 366def isClosedPolygon(polygon): 367 return (len(polygon) >= 2) and (polygon[0] == polygon[-1]) 368 369 370def splitPolygonAtLengths2D(polygon, lengths): 371 """ 372 Returns the polygon segments split at the given 2D-lengths. 373 """ 374 if (len(polygon) <= 1 or len(lengths) == 0): 375 return [polygon] 376 offsets = [offset for offset in sorted(lengths) if offset > 0.0 and offset < polyLength(polygon)] 377 ret = [] 378 seenLength = 0 379 curr = polygon[0] 380 polygonIndex = 0 381 for offset in offsets: 382 currSlice = [curr] 383 while polygonIndex < len(polygon) - 1: 384 p = polygon[polygonIndex + 1] 385 if offset < seenLength + distance(curr, p): 386 splitPos = positionAtOffset(curr, p, offset - seenLength) 387 if not isclose(distance(currSlice[-1], splitPos), 0): 388 currSlice.append(splitPos) 389 seenLength += distance(curr, splitPos) 390 curr = splitPos 391 break 392 else: 393 if not isclose(distance(currSlice[-1], p), 0): 394 currSlice.append(p) 395 seenLength += distance(curr, p) 396 curr = p 397 polygonIndex += 1 398 ret.append(currSlice) 399 if polygonIndex < len(polygon) - 1: 400 finalSlice = [curr] + polygon[polygonIndex + 1:] 401 ret.append(finalSlice) 402 return ret 403 404 405def intersectsAtLengths2D(polygon1, polygon2): 406 """ 407 Returns the 2D-length from polygon1's start to all intersections between polygon1 and polygon2. 408 """ 409 ret = [] 410 if (len(polygon1) == 0 or len(polygon2) == 0): 411 return ret 412 polygon1Length = polyLength(polygon1) 413 for j in range(len(polygon2) - 1): 414 p21 = polygon2[j] 415 p22 = polygon2[j + 1] 416 pos = 0.0 417 for i in range(len(polygon1) - 1): 418 p11 = polygon1[i] 419 p12 = polygon1[i + 1] 420 pIntersection = [0.0, 0.0] 421 if intersectsLineSegment(p11, p12, p21, p22, 0.0, pIntersection, True): 422 for k in range(0, len(pIntersection), 2): 423 length = pos + distance(p11, (pIntersection[k], pIntersection[k + 1])) 424 # special case for closed polygons 425 if isClosedPolygon(polygon1) and isclose(length, polygon1Length): 426 length = 0.0 427 # check for duplicate 428 isDuplicate = False 429 for result in ret: 430 if isclose(length, result): 431 isDuplicate = True 432 break 433 if not isDuplicate: 434 ret.append(length) 435 pos += distance(p11, p12) 436 return ret 437 438 439def intersectsPolygon(polygon1, polygon2): 440 """ 441 Returns whether the polygons intersect on at least one of their segments. 442 """ 443 if (len(polygon1) < 2 or len(polygon2) < 2): 444 return False 445 for i in range(len(polygon1) - 1): 446 p11 = polygon1[i] 447 p12 = polygon1[i + 1] 448 for j in range(len(polygon2) - 1): 449 p21 = polygon2[j] 450 p22 = polygon2[j + 1] 451 if intersectsLineSegment(p11, p12, p21, p22): 452 return True 453 return False 454 455 456# @see src/utils/geom/PositionVector::intersects(p11, p12, p21, p22 ...) 457def intersectsLineSegment(p11, p12, p21, p22, withinDist=0.0, pIntersection=None, storeEndPointsIfCoincident=False): 458 """ 459 Returns whether the line segments defined by Line p11,p12 and Line p21,p22 intersect. 460 If not set to 'None', 'pIntersection' serves as a storage for the intersection point(s). 461 Parameter 'storeEndPointsIfCoincident' is an option for storing the endpoints of the 462 line segment defined by the intersecting set of line1 and line2 if applicable. 463 """ 464 eps = sys.float_info.epsilon 465 # dy2 * dx1 - dx2 * dy1 466 denominator = (p22[1] - p21[1]) * (p12[0] - p11[0]) - (p22[0] - p21[0]) * (p12[1] - p11[1]) 467 # dx2 * (p11.y - p21.y) - dy2 * (p11.x - p21.x) 468 numera = (p22[0] - p21[0]) * (p11[1] - p21[1]) - (p22[1] - p21[1]) * (p11[0] - p21[0]) 469 # dx1 * (p11.y - p21.y) - dy1 * (p11.x - p21.x) 470 numerb = (p12[0] - p11[0]) * (p11[1] - p21[1]) - (p12[1] - p11[1]) * (p11[0] - p21[0]) 471 # Are the lines coincident? 472 if (math.fabs(numera) < eps and math.fabs(numerb) < eps and math.fabs(denominator) < eps): 473 a1 = 0.0 474 a2 = 0.0 475 a3 = 0.0 476 a4 = 0.0 477 a = -1e12 478 isVertical = (p11[0] == p12[0]) 479 if not isVertical: 480 # line1 and line2 are not vertical 481 a1 = p11[0] if p11[0] < p12[0] else p12[0] 482 a2 = p12[0] if p11[0] < p12[0] else p11[0] 483 a3 = p21[0] if p21[0] < p22[0] else p22[0] 484 a4 = p22[0] if p21[0] < p22[0] else p21[0] 485 else: 486 # line1 and line2 are vertical 487 a1 = p11[1] if p11[1] < p12[1] else p12[1] 488 a2 = p12[1] if p11[1] < p12[1] else p11[1] 489 a3 = p21[1] if p21[1] < p22[1] else p22[1] 490 a4 = p22[1] if p21[1] < p22[1] else p21[1] 491 if a1 <= a3 and a3 <= a2: 492 # one endpoint of line2 lies on line1 493 if a4 <= a2: 494 # line2 is a subset of line1 495 a = (a3 + a4) / 2.0 496 if storeEndPointsIfCoincident and pIntersection is not None: 497 pIntersection[0] = p21[0] 498 pIntersection[1] = p21[1] 499 pIntersection.append(p22[0]) 500 pIntersection.append(p22[1]) 501 return True 502 else: 503 # the other endpoint of line2 lies beyond line1 504 a = (a3 + a2) / 2.0 505 if storeEndPointsIfCoincident and pIntersection is not None: 506 if not isVertical: 507 pIntersection[0] = a3 508 pIntersection[1] = p21[1] if p21[0] < p22[0] else p22[1] 509 pIntersection.append(a2) 510 pIntersection.append(p12[1] if p11[0] < p12[0] else p11[1]) 511 else: 512 pIntersection[0] = p21[0] if p21[1] < p22[1] else p22[0] 513 pIntersection[1] = a3 514 pIntersection.append(p12[0] if p11[1] < p12[1] else p11[0]) 515 pIntersection.append(a2) 516 return True 517 if a3 <= a1 and a1 <= a4: 518 # one endpoint of line1 lies on line2 519 if a2 <= a4: 520 # line1 is a subset of line2 521 a = (a1 + a2) / 2.0 522 if storeEndPointsIfCoincident and pIntersection is not None: 523 pIntersection[0] = p11[0] 524 pIntersection[1] = p11[1] 525 pIntersection.append(p12[0]) 526 pIntersection.append(p12[1]) 527 return True 528 else: 529 # the other endpoint of line1 lies beyond line2 530 a = (a1 + a4) / 2.0 531 if storeEndPointsIfCoincident and pIntersection is not None: 532 if not isVertical: 533 pIntersection[0] = a1 534 pIntersection[1] = p11[1] if p11[0] < p12[0] else p12[1] 535 pIntersection.append(a4) 536 pIntersection.append(p22[1] if p21[0] < p22[0] else p21[1]) 537 else: 538 pIntersection[0] = p11[0] if p11[1] < p12[1] else p12[0] 539 pIntersection[1] = a1 540 pIntersection.append(p22[0] if p21[1] < p22[1] else p21[0]) 541 pIntersection.append(a4) 542 return True 543 if a != -1e12: 544 if pIntersection is not None: 545 if not isVertical: 546 # line1 and line2 are not vertical 547 mu = (a - p11[0]) / (p12[0] - p11[0]) 548 x = a 549 y = p11[1] + mu * (p12[1] - p11[1]) 550 else: 551 # line1 and line2 are vertical 552 x = p11[0] 553 y = a 554 if p12[1] == p11[1]: 555 mu = 0 556 else: 557 mu = (a - p11[1]) / (p12[1] - p11[1]) 558 pIntersection[0] = x 559 pIntersection[1] = y 560 return True 561 return False 562 # Are the lines parallel? 563 if math.fabs(denominator) < eps: 564 return False 565 # Is the intersection along the segments? 566 mua = numera / denominator 567 # Reduce rounding errors for lines ending in the same point 568 if math.fabs(p12[0] - p22[0]) < eps and math.fabs(p12[1] - p22[1]) < eps: 569 mua = 1.0 570 else: 571 offseta = withinDist / distance(p11, p12) 572 offsetb = withinDist / distance(p21, p22) 573 mub = numerb / denominator 574 if (mua < -offseta or mua > 1 + offseta or mub < -offsetb or mub > 1 + offsetb): 575 return False 576 if pIntersection is not None: 577 x = p11[0] + mua * (p12[0] - p11[0]) 578 y = p11[1] + mua * (p12[1] - p11[1]) 579 mu = mua 580 pIntersection[0] = x 581 pIntersection[1] = y 582 return True 583 584 585def debugPrint(polygon, polyID): 586 """print polygon in a way that can be processed with debug2poly.py""" 587 print("%s=%s" % (polyID, ' '.join(['%s,%s' % p for p in polygon])))
45def addToBoundingBox(coordList, bbox=None): 46 if bbox is None: 47 minX = 1e400 48 minY = 1e400 49 maxX = -1e400 50 maxY = -1e400 51 else: 52 minX, minY, maxX, maxY = bbox 53 for x, y in coordList: 54 minX = min(x, minX) 55 minY = min(y, minY) 56 maxX = max(x, maxX) 57 maxY = max(y, maxY) 58 return minX, minY, maxX, maxY
66def lineOffsetWithMinimumDistanceToPoint(point, line_start, line_end, perpendicular=False): 67 """Return the offset from line (line_start, line_end) where the distance to 68 point is minimal""" 69 d = distance(line_start, line_end) 70 u = ((point[0] - line_start[0]) * (line_end[0] - line_start[0]) 71 + (point[1] - line_start[1]) * (line_end[1] - line_start[1])) 72 if d == 0. or u < 0. or u > d * d: 73 if perpendicular: 74 return INVALID_DISTANCE 75 if u < 0.: 76 return 0. 77 return d 78 return u / d
Return the offset from line (line_start, line_end) where the distance to point is minimal
81def polygonOffsetAndDistanceToPoint(point, polygon, perpendicular=False): 82 """Return the offset and the distance from the polygon start where the distance to the point is minimal""" 83 p = point 84 s = polygon 85 seen = 0 86 minDist = 1e400 87 minOffset = INVALID_DISTANCE 88 for i in range(len(s) - 1): 89 pos = lineOffsetWithMinimumDistanceToPoint( 90 p, s[i], s[i + 1], perpendicular) 91 dist = minDist if pos == INVALID_DISTANCE else distance( 92 p, positionAtOffset(s[i], s[i + 1], pos)) 93 if dist < minDist: 94 minDist = dist 95 minOffset = pos + seen 96 if perpendicular and i != 0 and pos == INVALID_DISTANCE: 97 # even if perpendicular is set we still need to check the distance 98 # to the inner points 99 cornerDist = distance(p, s[i]) 100 if cornerDist < minDist: 101 pos1 = lineOffsetWithMinimumDistanceToPoint( 102 p, s[i - 1], s[i], False) 103 pos2 = lineOffsetWithMinimumDistanceToPoint( 104 p, s[i], s[i + 1], False) 105 if pos1 == distance(s[i - 1], s[i]) and pos2 == 0.: 106 minOffset = seen 107 minDist = cornerDist 108 seen += distance(s[i], s[i + 1]) 109 return minOffset, minDist
Return the offset and the distance from the polygon start where the distance to the point is minimal
112def polygonOffsetWithMinimumDistanceToPoint(point, polygon, perpendicular=False): 113 """Return the offset from the polygon start where the distance to the point is minimal""" 114 return polygonOffsetAndDistanceToPoint(point, polygon, perpendicular)[0]
Return the offset from the polygon start where the distance to the point is minimal
117def distancePointToLine(point, line_start, line_end, perpendicular=False): 118 """Return the minimum distance between point and the line (line_start, line_end)""" 119 p1 = line_start 120 p2 = line_end 121 offset = lineOffsetWithMinimumDistanceToPoint( 122 point, line_start, line_end, perpendicular) 123 if offset == INVALID_DISTANCE: 124 return INVALID_DISTANCE 125 if offset == 0: 126 return distance(point, p1) 127 u = offset / distance(line_start, line_end) 128 intersection = (p1[0] + u * (p2[0] - p1[0]), p1[1] + u * (p2[1] - p1[1])) 129 return distance(point, intersection)
Return the minimum distance between point and the line (line_start, line_end)
132def distancePointToPolygon(point, polygon, perpendicular=False): 133 """Return the minimum distance between point and polygon""" 134 p = point 135 s = polygon 136 minDist = None 137 for i in range(0, len(s) - 1): 138 dist = distancePointToLine(p, s[i], s[i + 1], perpendicular) 139 if dist == INVALID_DISTANCE and perpendicular and i != 0: 140 # distance to inner corner 141 dist = distance(point, s[i]) 142 if dist != INVALID_DISTANCE: 143 if minDist is None or dist < minDist: 144 minDist = dist 145 if minDist is not None: 146 return minDist 147 else: 148 return INVALID_DISTANCE
Return the minimum distance between point and polygon
151def positionAtOffset(p1, p2, offset): 152 if isclose(offset, 0.): # for pathological cases with dist == 0 and offset == 0 153 return p1 154 155 dist = distance(p1, p2) 156 157 if isclose(dist, offset): 158 return p2 159 160 if offset > dist: 161 return None 162 163 return (p1[0] + (p2[0] - p1[0]) * (offset / dist), p1[1] + (p2[1] - p1[1]) * (offset / dist))
166def indexAtShapeOffset(shape, offset): 167 """Returns the index of the shape segment which contains the offset and 168 the cumulated length of the shape up to the start point of the segment. 169 If the offset is less or equal to 0, it returns (0, 0.) If the offset is 170 larger than the shape length it returns (None, length of the shape)""" 171 seenLength = 0. 172 curr = shape[0] 173 for idx, p in enumerate(shape[1:]): 174 nextLength = distance(curr, p) 175 if seenLength + nextLength > offset: 176 return idx, seenLength 177 seenLength += nextLength 178 curr = p 179 return None, seenLength
Returns the index of the shape segment which contains the offset and the cumulated length of the shape up to the start point of the segment. If the offset is less or equal to 0, it returns (0, 0.) If the offset is larger than the shape length it returns (None, length of the shape)
232def isWithin(pos, shape): 233 """Returns whether the given pos coordinate is inside the polygon shape with ray-casting.""" 234 x, y = pos 235 inside = False 236 n = len(shape) 237 for i in range(n): 238 x1, y1 = shape[i] 239 x2, y2 = shape[(i + 1) % n] 240 241 if min(x1, x2) <= x <= max(x1, x2) and min(y1, y2) <= y <= max(y1, y2): 242 if abs((x2 - x1) * (y - y1) - (y2 - y1) * (x - x1)) < 1e-12: 243 return True 244 245 if (y1 > y) != (y2 > y) and x < (x2 - x1) * (y - y1) / (y2 - y1) + x1: 246 inside = not inside 247 248 return inside
Returns whether the given pos coordinate is inside the polygon shape with ray-casting.
303def narrow(fromPos, pos, toPos, amount): 304 """detect narrow turns which cannot be shifted regularly""" 305 a = sub(toPos, pos) 306 b = sub(pos, fromPos) 307 c = add(a, b) 308 dPac = dotProduct(a, c) 309 if dPac == 0: 310 return True 311 x = dotProduct(a, a) * length(c) / dPac 312 return x < amount
detect narrow turns which cannot be shifted regularly
315def line2boundary(shape, width): 316 """expand center line by width/2 on both sides to obtain a (closed) boundary shape 317 (i.e. for an edge or lane) 318 """ 319 left = move2side(shape, width / 2) 320 right = move2side(shape, -width / 2) 321 322 return left + list(reversed(right)) + [left[0]]
expand center line by width/2 on both sides to obtain a (closed) boundary shape (i.e. for an edge or lane)
325def move2side(shape, amount): 326 shape = [s for i, s in enumerate(shape) if i == 0 or shape[i-1] != s] 327 if len(shape) < 2: 328 return shape 329 if polyLength(shape) == 0: 330 return shape 331 result = [] 332 for i, pos in enumerate(shape): 333 if i == 0: 334 fromPos = pos 335 toPos = shape[i + 1] 336 if fromPos != toPos: 337 result.append(sub(fromPos, sideOffset(fromPos, toPos, amount))) 338 elif i == len(shape) - 1: 339 fromPos = shape[i - 1] 340 toPos = pos 341 if fromPos != toPos: 342 result.append(sub(toPos, sideOffset(fromPos, toPos, amount))) 343 else: 344 fromPos = shape[i - 1] 345 toPos = shape[i + 1] 346 # check for narrow turns 347 if narrow(fromPos, pos, toPos, amount): 348 # print("narrow at i=%s pos=%s" % (i, pos)) 349 pass 350 else: 351 a = sideOffset(fromPos, pos, -amount) 352 b = sideOffset(pos, toPos, -amount) 353 c = orthoIntersection(a, b) 354 if c is not None: 355 pos2 = add(pos, c) 356 else: 357 extend = norm(sub(pos, fromPos)) 358 pos2 = add(pos, mul(extend, amount)) 359 result.append(pos2) 360 # print("move2side", amount) 361 # print(shape) 362 # print(result) 363 # print() 364 return result
371def splitPolygonAtLengths2D(polygon, lengths): 372 """ 373 Returns the polygon segments split at the given 2D-lengths. 374 """ 375 if (len(polygon) <= 1 or len(lengths) == 0): 376 return [polygon] 377 offsets = [offset for offset in sorted(lengths) if offset > 0.0 and offset < polyLength(polygon)] 378 ret = [] 379 seenLength = 0 380 curr = polygon[0] 381 polygonIndex = 0 382 for offset in offsets: 383 currSlice = [curr] 384 while polygonIndex < len(polygon) - 1: 385 p = polygon[polygonIndex + 1] 386 if offset < seenLength + distance(curr, p): 387 splitPos = positionAtOffset(curr, p, offset - seenLength) 388 if not isclose(distance(currSlice[-1], splitPos), 0): 389 currSlice.append(splitPos) 390 seenLength += distance(curr, splitPos) 391 curr = splitPos 392 break 393 else: 394 if not isclose(distance(currSlice[-1], p), 0): 395 currSlice.append(p) 396 seenLength += distance(curr, p) 397 curr = p 398 polygonIndex += 1 399 ret.append(currSlice) 400 if polygonIndex < len(polygon) - 1: 401 finalSlice = [curr] + polygon[polygonIndex + 1:] 402 ret.append(finalSlice) 403 return ret
Returns the polygon segments split at the given 2D-lengths.
406def intersectsAtLengths2D(polygon1, polygon2): 407 """ 408 Returns the 2D-length from polygon1's start to all intersections between polygon1 and polygon2. 409 """ 410 ret = [] 411 if (len(polygon1) == 0 or len(polygon2) == 0): 412 return ret 413 polygon1Length = polyLength(polygon1) 414 for j in range(len(polygon2) - 1): 415 p21 = polygon2[j] 416 p22 = polygon2[j + 1] 417 pos = 0.0 418 for i in range(len(polygon1) - 1): 419 p11 = polygon1[i] 420 p12 = polygon1[i + 1] 421 pIntersection = [0.0, 0.0] 422 if intersectsLineSegment(p11, p12, p21, p22, 0.0, pIntersection, True): 423 for k in range(0, len(pIntersection), 2): 424 length = pos + distance(p11, (pIntersection[k], pIntersection[k + 1])) 425 # special case for closed polygons 426 if isClosedPolygon(polygon1) and isclose(length, polygon1Length): 427 length = 0.0 428 # check for duplicate 429 isDuplicate = False 430 for result in ret: 431 if isclose(length, result): 432 isDuplicate = True 433 break 434 if not isDuplicate: 435 ret.append(length) 436 pos += distance(p11, p12) 437 return ret
Returns the 2D-length from polygon1's start to all intersections between polygon1 and polygon2.
440def intersectsPolygon(polygon1, polygon2): 441 """ 442 Returns whether the polygons intersect on at least one of their segments. 443 """ 444 if (len(polygon1) < 2 or len(polygon2) < 2): 445 return False 446 for i in range(len(polygon1) - 1): 447 p11 = polygon1[i] 448 p12 = polygon1[i + 1] 449 for j in range(len(polygon2) - 1): 450 p21 = polygon2[j] 451 p22 = polygon2[j + 1] 452 if intersectsLineSegment(p11, p12, p21, p22): 453 return True 454 return False
Returns whether the polygons intersect on at least one of their segments.
458def intersectsLineSegment(p11, p12, p21, p22, withinDist=0.0, pIntersection=None, storeEndPointsIfCoincident=False): 459 """ 460 Returns whether the line segments defined by Line p11,p12 and Line p21,p22 intersect. 461 If not set to 'None', 'pIntersection' serves as a storage for the intersection point(s). 462 Parameter 'storeEndPointsIfCoincident' is an option for storing the endpoints of the 463 line segment defined by the intersecting set of line1 and line2 if applicable. 464 """ 465 eps = sys.float_info.epsilon 466 # dy2 * dx1 - dx2 * dy1 467 denominator = (p22[1] - p21[1]) * (p12[0] - p11[0]) - (p22[0] - p21[0]) * (p12[1] - p11[1]) 468 # dx2 * (p11.y - p21.y) - dy2 * (p11.x - p21.x) 469 numera = (p22[0] - p21[0]) * (p11[1] - p21[1]) - (p22[1] - p21[1]) * (p11[0] - p21[0]) 470 # dx1 * (p11.y - p21.y) - dy1 * (p11.x - p21.x) 471 numerb = (p12[0] - p11[0]) * (p11[1] - p21[1]) - (p12[1] - p11[1]) * (p11[0] - p21[0]) 472 # Are the lines coincident? 473 if (math.fabs(numera) < eps and math.fabs(numerb) < eps and math.fabs(denominator) < eps): 474 a1 = 0.0 475 a2 = 0.0 476 a3 = 0.0 477 a4 = 0.0 478 a = -1e12 479 isVertical = (p11[0] == p12[0]) 480 if not isVertical: 481 # line1 and line2 are not vertical 482 a1 = p11[0] if p11[0] < p12[0] else p12[0] 483 a2 = p12[0] if p11[0] < p12[0] else p11[0] 484 a3 = p21[0] if p21[0] < p22[0] else p22[0] 485 a4 = p22[0] if p21[0] < p22[0] else p21[0] 486 else: 487 # line1 and line2 are vertical 488 a1 = p11[1] if p11[1] < p12[1] else p12[1] 489 a2 = p12[1] if p11[1] < p12[1] else p11[1] 490 a3 = p21[1] if p21[1] < p22[1] else p22[1] 491 a4 = p22[1] if p21[1] < p22[1] else p21[1] 492 if a1 <= a3 and a3 <= a2: 493 # one endpoint of line2 lies on line1 494 if a4 <= a2: 495 # line2 is a subset of line1 496 a = (a3 + a4) / 2.0 497 if storeEndPointsIfCoincident and pIntersection is not None: 498 pIntersection[0] = p21[0] 499 pIntersection[1] = p21[1] 500 pIntersection.append(p22[0]) 501 pIntersection.append(p22[1]) 502 return True 503 else: 504 # the other endpoint of line2 lies beyond line1 505 a = (a3 + a2) / 2.0 506 if storeEndPointsIfCoincident and pIntersection is not None: 507 if not isVertical: 508 pIntersection[0] = a3 509 pIntersection[1] = p21[1] if p21[0] < p22[0] else p22[1] 510 pIntersection.append(a2) 511 pIntersection.append(p12[1] if p11[0] < p12[0] else p11[1]) 512 else: 513 pIntersection[0] = p21[0] if p21[1] < p22[1] else p22[0] 514 pIntersection[1] = a3 515 pIntersection.append(p12[0] if p11[1] < p12[1] else p11[0]) 516 pIntersection.append(a2) 517 return True 518 if a3 <= a1 and a1 <= a4: 519 # one endpoint of line1 lies on line2 520 if a2 <= a4: 521 # line1 is a subset of line2 522 a = (a1 + a2) / 2.0 523 if storeEndPointsIfCoincident and pIntersection is not None: 524 pIntersection[0] = p11[0] 525 pIntersection[1] = p11[1] 526 pIntersection.append(p12[0]) 527 pIntersection.append(p12[1]) 528 return True 529 else: 530 # the other endpoint of line1 lies beyond line2 531 a = (a1 + a4) / 2.0 532 if storeEndPointsIfCoincident and pIntersection is not None: 533 if not isVertical: 534 pIntersection[0] = a1 535 pIntersection[1] = p11[1] if p11[0] < p12[0] else p12[1] 536 pIntersection.append(a4) 537 pIntersection.append(p22[1] if p21[0] < p22[0] else p21[1]) 538 else: 539 pIntersection[0] = p11[0] if p11[1] < p12[1] else p12[0] 540 pIntersection[1] = a1 541 pIntersection.append(p22[0] if p21[1] < p22[1] else p21[0]) 542 pIntersection.append(a4) 543 return True 544 if a != -1e12: 545 if pIntersection is not None: 546 if not isVertical: 547 # line1 and line2 are not vertical 548 mu = (a - p11[0]) / (p12[0] - p11[0]) 549 x = a 550 y = p11[1] + mu * (p12[1] - p11[1]) 551 else: 552 # line1 and line2 are vertical 553 x = p11[0] 554 y = a 555 if p12[1] == p11[1]: 556 mu = 0 557 else: 558 mu = (a - p11[1]) / (p12[1] - p11[1]) 559 pIntersection[0] = x 560 pIntersection[1] = y 561 return True 562 return False 563 # Are the lines parallel? 564 if math.fabs(denominator) < eps: 565 return False 566 # Is the intersection along the segments? 567 mua = numera / denominator 568 # Reduce rounding errors for lines ending in the same point 569 if math.fabs(p12[0] - p22[0]) < eps and math.fabs(p12[1] - p22[1]) < eps: 570 mua = 1.0 571 else: 572 offseta = withinDist / distance(p11, p12) 573 offsetb = withinDist / distance(p21, p22) 574 mub = numerb / denominator 575 if (mua < -offseta or mua > 1 + offseta or mub < -offsetb or mub > 1 + offsetb): 576 return False 577 if pIntersection is not None: 578 x = p11[0] + mua * (p12[0] - p11[0]) 579 y = p11[1] + mua * (p12[1] - p11[1]) 580 mu = mua 581 pIntersection[0] = x 582 pIntersection[1] = y 583 return True
Returns whether the line segments defined by Line p11,p12 and Line p21,p22 intersect. If not set to 'None', 'pIntersection' serves as a storage for the intersection point(s). Parameter 'storeEndPointsIfCoincident' is an option for storing the endpoints of the line segment defined by the intersecting set of line1 and line2 if applicable.
586def debugPrint(polygon, polyID): 587 """print polygon in a way that can be processed with debug2poly.py""" 588 print("%s=%s" % (polyID, ' '.join(['%s,%s' % p for p in polygon])))
print polygon in a way that can be processed with debug2poly.py