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])))
INVALID_DISTANCE = -1
def isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
31def isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
32    return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)
def distance(p1, p2):
35def distance(p1, p2):
36    dx = p1[0] - p2[0]
37    dy = p1[1] - p2[1]
38    return math.sqrt(dx * dx + dy * dy)
def polyLength(polygon):
41def polyLength(polygon):
42    return sum([distance(a, b) for a, b in zip(polygon[:-1], polygon[1:])])
def addToBoundingBox(coordList, bbox=None):
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
def isLeft(point, line_start, line_end):
61def isLeft(point, line_start, line_end):
62    return ((line_end[0] - line_start[0]) * (point[1] - line_start[1])
63            < (point[0] - line_start[0]) * (line_end[1] - line_start[1]))
def lineOffsetWithMinimumDistanceToPoint(point, line_start, line_end, perpendicular=False):
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

def polygonOffsetAndDistanceToPoint(point, polygon, perpendicular=False):
 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

def polygonOffsetWithMinimumDistanceToPoint(point, polygon, perpendicular=False):
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

def distancePointToLine(point, line_start, line_end, perpendicular=False):
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)

def distancePointToPolygon(point, polygon, perpendicular=False):
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

def positionAtOffset(p1, p2, offset):
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))
def indexAtShapeOffset(shape, offset):
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)

def positionAtShapeOffset(shape, offset):
182def positionAtShapeOffset(shape, offset):
183    idx, seen = indexAtShapeOffset(shape, offset)
184    if idx is None:
185        return shape[-1]
186    return positionAtOffset(shape[idx], shape[idx + 1], offset - seen)
def rotationAtShapeOffset(shape, offset):
189def rotationAtShapeOffset(shape, offset):
190    idx, _ = indexAtShapeOffset(shape, offset)
191    if idx is None:
192        return None
193    return math.atan2(shape[idx + 1][1] - shape[idx][1], shape[idx + 1][0] - shape[idx][0])
def angle2D(p1, p2):
196def angle2D(p1, p2):
197    theta1 = math.atan2(p1[1], p1[0])
198    theta2 = math.atan2(p2[1], p2[0])
199    dtheta = theta2 - theta1
200    while dtheta > math.pi:
201        dtheta -= 2.0 * math.pi
202    while dtheta < -math.pi:
203        dtheta += 2.0 * math.pi
204    return dtheta
def angleTo2D(p1, p2):
207def angleTo2D(p1, p2):
208    return math.atan2(p2[1] - p1[1], p2[0] - p1[0])
def fromNaviDegree(degrees):
215def fromNaviDegree(degrees):
216    return math.pi / 2. - math.radians(degrees)
def normalizeAngle(a, lower, upper, circle):
219def normalizeAngle(a, lower, upper, circle):
220    while a < lower:
221        a = a + circle
222    while a > upper:
223        a = a - circle
224    return a
def minAngleDegreeDiff(d1, d2):
227def minAngleDegreeDiff(d1, d2):
228    return min(normalizeAngle(d1 - d2, 0, 360, 360),
229               normalizeAngle(d2 - d1, 0, 360, 360))
def isWithin(pos, 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.

def sideOffset(fromPos, toPos, amount):
251def sideOffset(fromPos, toPos, amount):
252    scale = amount / distance(fromPos, toPos)
253    return (scale * (fromPos[1] - toPos[1]),
254            scale * (toPos[0] - fromPos[0]))
def sub(a, b):
257def sub(a, b):
258    return (a[0] - b[0], a[1] - b[1])
def add(a, b):
261def add(a, b):
262    return (a[0] + b[0], a[1] + b[1])
def mul(a, x):
265def mul(a, x):
266    return (a[0] * x, a[1] * x)
def dotProduct(a, b):
269def dotProduct(a, b):
270    return a[0] * b[0] + a[1] * b[1]
def crossProduct2D(a, b):
273def crossProduct2D(a, b):
274    return a[0] * b[1] - a[1] * b[0]
def orthoIntersection(a, b):
277def orthoIntersection(a, b):
278    c = add(a, b)
279    quot = dotProduct(c, a)
280    if abs(quot) > 0.001:
281        return mul(mul(c, dotProduct(a, a)), 1 / quot)
282    else:
283        return None
def rotateAround2D(p, rad, origin):
286def rotateAround2D(p, rad, origin):
287    s = math.sin(rad)
288    c = math.cos(rad)
289    tmp = sub(p, origin)
290    tmp2 = [tmp[0] * c - tmp[1] * s,
291            tmp[0] * s + tmp[1] * c]
292    return add(tmp2, origin)
def length(a):
295def length(a):
296    return math.sqrt(dotProduct(a, a))
def norm(a):
299def norm(a):
300    return mul(a, 1 / length(a))
def narrow(fromPos, pos, toPos, amount):
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

def line2boundary(shape, width):
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)

def move2side(shape, amount):
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
def isClosedPolygon(polygon):
367def isClosedPolygon(polygon):
368    return (len(polygon) >= 2) and (polygon[0] == polygon[-1])
def splitPolygonAtLengths2D(polygon, lengths):
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.

def intersectsAtLengths2D(polygon1, polygon2):
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.

def intersectsPolygon(polygon1, 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.

def intersectsLineSegment( p11, p12, p21, p22, withinDist=0.0, pIntersection=None, storeEndPointsIfCoincident=False):
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.

def debugPrint(polygon, polyID):
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