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 defined in anticlockwise order."""
233    angle = 0.
234    for i in range(0, len(shape) - 1):
235        p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1]))
236        p2 = ((shape[i + 1][0] - pos[0]), (shape[i + 1][1] - pos[1]))
237        angle = angle + angle2D(p1, p2)
238    i = len(shape) - 1
239    p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1]))
240    p2 = ((shape[0][0] - pos[0]), (shape[0][1] - pos[1]))
241    angle = angle + angle2D(p1, p2)
242    return math.fabs(angle) >= math.pi
243
244
245def sideOffset(fromPos, toPos, amount):
246    scale = amount / distance(fromPos, toPos)
247    return (scale * (fromPos[1] - toPos[1]),
248            scale * (toPos[0] - fromPos[0]))
249
250
251def sub(a, b):
252    return (a[0] - b[0], a[1] - b[1])
253
254
255def add(a, b):
256    return (a[0] + b[0], a[1] + b[1])
257
258
259def mul(a, x):
260    return (a[0] * x, a[1] * x)
261
262
263def dotProduct(a, b):
264    return a[0] * b[0] + a[1] * b[1]
265
266
267def crossProduct2D(a, b):
268    return a[0] * b[1] - a[1] * b[0]
269
270
271def orthoIntersection(a, b):
272    c = add(a, b)
273    quot = dotProduct(c, a)
274    if abs(quot) > 0.001:
275        return mul(mul(c, dotProduct(a, a)), 1 / quot)
276    else:
277        return None
278
279
280def rotateAround2D(p, rad, origin):
281    s = math.sin(rad)
282    c = math.cos(rad)
283    tmp = sub(p, origin)
284    tmp2 = [tmp[0] * c - tmp[1] * s,
285            tmp[0] * s + tmp[1] * c]
286    return add(tmp2, origin)
287
288
289def length(a):
290    return math.sqrt(dotProduct(a, a))
291
292
293def norm(a):
294    return mul(a, 1 / length(a))
295
296
297def narrow(fromPos, pos, toPos, amount):
298    """detect narrow turns which cannot be shifted regularly"""
299    a = sub(toPos, pos)
300    b = sub(pos, fromPos)
301    c = add(a, b)
302    dPac = dotProduct(a, c)
303    if dPac == 0:
304        return True
305    x = dotProduct(a, a) * length(c) / dPac
306    return x < amount
307
308
309def line2boundary(shape, width):
310    """expand center line by width/2 on both sides to obtain a (closed) boundary shape
311    (i.e. for an edge or lane)
312    """
313    left = move2side(shape, width / 2)
314    right = move2side(shape, -width / 2)
315
316    return left + list(reversed(right)) + [left[0]]
317
318
319def move2side(shape, amount):
320    shape = [s for i, s in enumerate(shape) if i == 0 or shape[i-1] != s]
321    if len(shape) < 2:
322        return shape
323    if polyLength(shape) == 0:
324        return shape
325    result = []
326    for i, pos in enumerate(shape):
327        if i == 0:
328            fromPos = pos
329            toPos = shape[i + 1]
330            if fromPos != toPos:
331                result.append(sub(fromPos, sideOffset(fromPos, toPos, amount)))
332        elif i == len(shape) - 1:
333            fromPos = shape[i - 1]
334            toPos = pos
335            if fromPos != toPos:
336                result.append(sub(toPos, sideOffset(fromPos, toPos, amount)))
337        else:
338            fromPos = shape[i - 1]
339            toPos = shape[i + 1]
340            # check for narrow turns
341            if narrow(fromPos, pos, toPos, amount):
342                # print("narrow at i=%s pos=%s" % (i, pos))
343                pass
344            else:
345                a = sideOffset(fromPos, pos, -amount)
346                b = sideOffset(pos, toPos, -amount)
347                c = orthoIntersection(a, b)
348                if c is not None:
349                    pos2 = add(pos, c)
350                else:
351                    extend = norm(sub(pos, fromPos))
352                    pos2 = add(pos, mul(extend, amount))
353                result.append(pos2)
354    # print("move2side", amount)
355    # print(shape)
356    # print(result)
357    # print()
358    return result
359
360
361def isClosedPolygon(polygon):
362    return (len(polygon) >= 2) and (polygon[0] == polygon[-1])
363
364
365def splitPolygonAtLengths2D(polygon, lengths):
366    """
367    Returns the polygon segments split at the given 2D-lengths.
368    """
369    if (len(polygon) <= 1 or len(lengths) == 0):
370        return [polygon]
371    offsets = [offset for offset in sorted(lengths) if offset > 0.0 and offset < polyLength(polygon)]
372    ret = []
373    seenLength = 0
374    curr = polygon[0]
375    polygonIndex = 0
376    for offset in offsets:
377        currSlice = [curr]
378        while polygonIndex < len(polygon) - 1:
379            p = polygon[polygonIndex + 1]
380            if offset < seenLength + distance(curr, p):
381                splitPos = positionAtOffset(curr, p, offset - seenLength)
382                if not isclose(distance(currSlice[-1], splitPos), 0):
383                    currSlice.append(splitPos)
384                seenLength += distance(curr, splitPos)
385                curr = splitPos
386                break
387            else:
388                if not isclose(distance(currSlice[-1], p), 0):
389                    currSlice.append(p)
390                seenLength += distance(curr, p)
391                curr = p
392                polygonIndex += 1
393        ret.append(currSlice)
394    if polygonIndex < len(polygon) - 1:
395        finalSlice = [curr] + polygon[polygonIndex + 1:]
396        ret.append(finalSlice)
397    return ret
398
399
400def intersectsAtLengths2D(polygon1, polygon2):
401    """
402    Returns the 2D-length from polygon1's start to all intersections between polygon1 and polygon2.
403    """
404    ret = []
405    if (len(polygon1) == 0 or len(polygon2) == 0):
406        return ret
407    polygon1Length = polyLength(polygon1)
408    for j in range(len(polygon2) - 1):
409        p21 = polygon2[j]
410        p22 = polygon2[j + 1]
411        pos = 0.0
412        for i in range(len(polygon1) - 1):
413            p11 = polygon1[i]
414            p12 = polygon1[i + 1]
415            pIntersection = [0.0, 0.0]
416            if intersectsLineSegment(p11, p12, p21, p22, 0.0, pIntersection, True):
417                for k in range(0, len(pIntersection), 2):
418                    length = pos + distance(p11, (pIntersection[k], pIntersection[k + 1]))
419                    # special case for closed polygons
420                    if isClosedPolygon(polygon1) and isclose(length, polygon1Length):
421                        length = 0.0
422                    # check for duplicate
423                    isDuplicate = False
424                    for result in ret:
425                        if isclose(length, result):
426                            isDuplicate = True
427                            break
428                    if not isDuplicate:
429                        ret.append(length)
430            pos += distance(p11, p12)
431    return ret
432
433
434def intersectsPolygon(polygon1, polygon2):
435    """
436    Returns whether the polygons intersect on at least one of their segments.
437    """
438    if (len(polygon1) < 2 or len(polygon2) < 2):
439        return False
440    for i in range(len(polygon1) - 1):
441        p11 = polygon1[i]
442        p12 = polygon1[i + 1]
443        for j in range(len(polygon2) - 1):
444            p21 = polygon2[j]
445            p22 = polygon2[j + 1]
446            if intersectsLineSegment(p11, p12, p21, p22):
447                return True
448    return False
449
450
451# @see src/utils/geom/PositionVector::intersects(p11, p12, p21, p22 ...)
452def intersectsLineSegment(p11, p12, p21, p22, withinDist=0.0, pIntersection=None, storeEndPointsIfCoincident=False):
453    """
454    Returns whether the line segments defined by Line p11,p12 and Line p21,p22 intersect.
455    If not set to 'None', 'pIntersection' serves as a storage for the intersection point(s).
456    Parameter 'storeEndPointsIfCoincident' is an option for storing the endpoints of the
457    line segment defined by the intersecting set of line1 and line2 if applicable.
458    """
459    eps = sys.float_info.epsilon
460    # dy2 * dx1 - dx2 * dy1
461    denominator = (p22[1] - p21[1]) * (p12[0] - p11[0]) - (p22[0] - p21[0]) * (p12[1] - p11[1])
462    # dx2 * (p11.y - p21.y) - dy2 * (p11.x - p21.x)
463    numera = (p22[0] - p21[0]) * (p11[1] - p21[1]) - (p22[1] - p21[1]) * (p11[0] - p21[0])
464    # dx1 * (p11.y - p21.y) - dy1 * (p11.x - p21.x)
465    numerb = (p12[0] - p11[0]) * (p11[1] - p21[1]) - (p12[1] - p11[1]) * (p11[0] - p21[0])
466    # Are the lines coincident?
467    if (math.fabs(numera) < eps and math.fabs(numerb) < eps and math.fabs(denominator) < eps):
468        a1 = 0.0
469        a2 = 0.0
470        a3 = 0.0
471        a4 = 0.0
472        a = -1e12
473        isVertical = (p11[0] == p12[0])
474        if not isVertical:
475            # line1 and line2 are not vertical
476            a1 = p11[0] if p11[0] < p12[0] else p12[0]
477            a2 = p12[0] if p11[0] < p12[0] else p11[0]
478            a3 = p21[0] if p21[0] < p22[0] else p22[0]
479            a4 = p22[0] if p21[0] < p22[0] else p21[0]
480        else:
481            # line1 and line2 are vertical
482            a1 = p11[1] if p11[1] < p12[1] else p12[1]
483            a2 = p12[1] if p11[1] < p12[1] else p11[1]
484            a3 = p21[1] if p21[1] < p22[1] else p22[1]
485            a4 = p22[1] if p21[1] < p22[1] else p21[1]
486        if a1 <= a3 and a3 <= a2:
487            # one endpoint of line2 lies on line1
488            if a4 <= a2:
489                # line2 is a subset of line1
490                a = (a3 + a4) / 2.0
491                if storeEndPointsIfCoincident and pIntersection is not None:
492                    pIntersection[0] = p21[0]
493                    pIntersection[1] = p21[1]
494                    pIntersection.append(p22[0])
495                    pIntersection.append(p22[1])
496                    return True
497            else:
498                # the other endpoint of line2 lies beyond line1
499                a = (a3 + a2) / 2.0
500                if storeEndPointsIfCoincident and pIntersection is not None:
501                    if not isVertical:
502                        pIntersection[0] = a3
503                        pIntersection[1] = p21[1] if p21[0] < p22[0] else p22[1]
504                        pIntersection.append(a2)
505                        pIntersection.append(p12[1] if p11[0] < p12[0] else p11[1])
506                    else:
507                        pIntersection[0] = p21[0] if p21[1] < p22[1] else p22[0]
508                        pIntersection[1] = a3
509                        pIntersection.append(p12[0] if p11[1] < p12[1] else p11[0])
510                        pIntersection.append(a2)
511                    return True
512        if a3 <= a1 and a1 <= a4:
513            # one endpoint of line1 lies on line2
514            if a2 <= a4:
515                # line1 is a subset of line2
516                a = (a1 + a2) / 2.0
517                if storeEndPointsIfCoincident and pIntersection is not None:
518                    pIntersection[0] = p11[0]
519                    pIntersection[1] = p11[1]
520                    pIntersection.append(p12[0])
521                    pIntersection.append(p12[1])
522                    return True
523            else:
524                # the other endpoint of line1 lies beyond line2
525                a = (a1 + a4) / 2.0
526                if storeEndPointsIfCoincident and pIntersection is not None:
527                    if not isVertical:
528                        pIntersection[0] = a1
529                        pIntersection[1] = p11[1] if p11[0] < p12[0] else p12[1]
530                        pIntersection.append(a4)
531                        pIntersection.append(p22[1] if p21[0] < p22[0] else p21[1])
532                    else:
533                        pIntersection[0] = p11[0] if p11[1] < p12[1] else p12[0]
534                        pIntersection[1] = a1
535                        pIntersection.append(p22[0] if p21[1] < p22[1] else p21[0])
536                        pIntersection.append(a4)
537                    return True
538        if a != -1e12:
539            if pIntersection is not None:
540                if not isVertical:
541                    # line1 and line2 are not vertical
542                    mu = (a - p11[0]) / (p12[0] - p11[0])
543                    x = a
544                    y = p11[1] + mu * (p12[1] - p11[1])
545                else:
546                    # line1 and line2 are vertical
547                    x = p11[0]
548                    y = a
549                    if p12[1] == p11[1]:
550                        mu = 0
551                    else:
552                        mu = (a - p11[1]) / (p12[1] - p11[1])
553                pIntersection[0] = x
554                pIntersection[1] = y
555            return True
556        return False
557    # Are the lines parallel?
558    if math.fabs(denominator) < eps:
559        return False
560    # Is the intersection along the segments?
561    mua = numera / denominator
562    # Reduce rounding errors for lines ending in the same point
563    if math.fabs(p12[0] - p22[0]) < eps and math.fabs(p12[1] - p22[1]) < eps:
564        mua = 1.0
565    else:
566        offseta = withinDist / distance(p11, p12)
567        offsetb = withinDist / distance(p21, p22)
568        mub = numerb / denominator
569        if (mua < -offseta or mua > 1 + offseta or mub < -offsetb or mub > 1 + offsetb):
570            return False
571    if pIntersection is not None:
572        x = p11[0] + mua * (p12[0] - p11[0])
573        y = p11[1] + mua * (p12[1] - p11[1])
574        mu = mua
575        pIntersection[0] = x
576        pIntersection[1] = y
577    return True
578
579
580def debugPrint(polygon, polyID):
581    """print polygon in a way that can be processed with debug2poly.py"""
582    print("%s=%s" % (polyID, ' '.join(['%s,%s' % p for p in polygon])))