1# Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
2# Copyright (C) 2013-2019 German Aerospace Center (DLR) and others.
3# This program and the accompanying materials
4# are made available under the terms of the Eclipse Public License v2.0
5# which accompanies this distribution, and is available at
6# http://www.eclipse.org/legal/epl-v20.html
7# SPDX-License-Identifier: EPL-2.0
8
9# @file    geomhelper.py
10# @author  Daniel Krajzewicz
11# @author  Jakob Erdmann
12# @author  Michael Behrisch
13# @date    2013-02-25
14# @version $Id$
15
16from __future__ import absolute_import
17import math
18
19INVALID_DISTANCE = -1
20
21
22def distance(p1, p2):
23    dx = p1[0] - p2[0]
24    dy = p1[1] - p2[1]
25    return math.sqrt(dx * dx + dy * dy)
26
27
28def polyLength(polygon):
29    return sum([distance(a, b) for a, b in zip(polygon[:-1], polygon[1:])])
30
31
32def lineOffsetWithMinimumDistanceToPoint(point, line_start, line_end, perpendicular=False):
33    """Return the offset from line (line_start, line_end) where the distance to
34    point is minimal"""
35    p = point
36    p1 = line_start
37    p2 = line_end
38    d = distance(p1, p2)
39    u = ((p[0] - p1[0]) * (p2[0] - p1[0])) + ((p[1] - p1[1]) * (p2[1] - p1[1]))
40    if d == 0. or u < 0. or u > d * d:
41        if perpendicular:
42            return INVALID_DISTANCE
43        if u < 0.:
44            return 0.
45        return d
46    return u / d
47
48
49def polygonOffsetAndDistanceToPoint(point, polygon, perpendicular=False):
50    """Return the offset and the distance from the polygon start where the distance to the point is minimal"""
51    p = point
52    s = polygon
53    seen = 0
54    minDist = 1e400
55    minOffset = INVALID_DISTANCE
56    for i in range(len(s) - 1):
57        pos = lineOffsetWithMinimumDistanceToPoint(
58            p, s[i], s[i + 1], perpendicular)
59        dist = minDist if pos == INVALID_DISTANCE else distance(
60            p, positionAtOffset(s[i], s[i + 1], pos))
61        if dist < minDist:
62            minDist = dist
63            minOffset = pos + seen
64        if perpendicular and i != 0 and pos == INVALID_DISTANCE:
65            # even if perpendicular is set we still need to check the distance
66            # to the inner points
67            cornerDist = distance(p, s[i])
68            if cornerDist < minDist:
69                pos1 = lineOffsetWithMinimumDistanceToPoint(
70                    p, s[i - 1], s[i], False)
71                pos2 = lineOffsetWithMinimumDistanceToPoint(
72                    p, s[i], s[i + 1], False)
73                if pos1 == distance(s[i - 1], s[i]) and pos2 == 0.:
74                    minOffset = seen
75                    minDist = cornerDist
76        seen += distance(s[i], s[i + 1])
77    return minOffset, minDist
78
79
80def polygonOffsetWithMinimumDistanceToPoint(point, polygon, perpendicular=False):
81    """Return the offset from the polygon start where the distance to the point is minimal"""
82    return polygonOffsetAndDistanceToPoint(point, polygon, perpendicular)[0]
83
84
85def distancePointToLine(point, line_start, line_end, perpendicular=False):
86    """Return the minimum distance between point and the line (line_start, line_end)"""
87    p1 = line_start
88    p2 = line_end
89    offset = lineOffsetWithMinimumDistanceToPoint(
90        point, line_start, line_end, perpendicular)
91    if offset == INVALID_DISTANCE:
92        return INVALID_DISTANCE
93    if offset == 0:
94        return distance(point, p1)
95    u = offset / distance(line_start, line_end)
96    intersection = (p1[0] + u * (p2[0] - p1[0]), p1[1] + u * (p2[1] - p1[1]))
97    return distance(point, intersection)
98
99
100def distancePointToPolygon(point, polygon, perpendicular=False):
101    """Return the minimum distance between point and polygon"""
102    p = point
103    s = polygon
104    minDist = None
105    for i in range(0, len(s) - 1):
106        dist = distancePointToLine(p, s[i], s[i + 1], perpendicular)
107        if dist == INVALID_DISTANCE and perpendicular and i != 0:
108            # distance to inner corner
109            dist = distance(point, s[i])
110        if dist != INVALID_DISTANCE:
111            if minDist is None or dist < minDist:
112                minDist = dist
113    if minDist is not None:
114        return minDist
115    else:
116        return INVALID_DISTANCE
117
118
119def positionAtOffset(p1, p2, offset):
120    if offset == 0.:  # for pathological cases with dist == 0 and offset == 0
121        return p1
122    dist = distance(p1, p2)
123    if dist < offset:
124        return None
125    return (p1[0] + (p2[0] - p1[0]) * (offset / dist), p1[1] + (p2[1] - p1[1]) * (offset / dist))
126
127
128def positionAtShapeOffset(shape, offset):
129    seenLength = 0
130    curr = shape[0]
131    for next in shape[1:]:
132        nextLength = distance(curr, next)
133        if seenLength + nextLength > offset:
134            return positionAtOffset(curr, next, offset - seenLength)
135        seenLength += nextLength
136        curr = next
137    return shape[-1]
138
139
140def angle2D(p1, p2):
141    theta1 = math.atan2(p1[1], p1[0])
142    theta2 = math.atan2(p2[1], p2[0])
143    dtheta = theta2 - theta1
144    while dtheta > math.pi:
145        dtheta -= 2.0 * math.pi
146    while dtheta < -math.pi:
147        dtheta += 2.0 * math.pi
148    return dtheta
149
150
151def isWithin(pos, shape):
152    angle = 0.
153    for i in range(0, len(shape) - 1):
154        p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1]))
155        p2 = ((shape[i + 1][0] - pos[0]), (shape[i + 1][1] - pos[1]))
156        angle = angle + angle2D(p1, p2)
157    i = len(shape) - 1
158    p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1]))
159    p2 = ((shape[0][0] - pos[0]), (shape[0][1] - pos[1]))
160    angle = angle + angle2D(p1, p2)
161    return math.fabs(angle) >= math.pi
162
163
164def sideOffset(fromPos, toPos, amount):
165    scale = amount / distance(fromPos, toPos)
166    return (scale * (fromPos[1] - toPos[1]),
167            scale * (toPos[0] - fromPos[0]))
168
169
170def sub(a, b):
171    return (a[0] - b[0], a[1] - b[1])
172
173
174def add(a, b):
175    return (a[0] + b[0], a[1] + b[1])
176
177
178def mul(a, x):
179    return (a[0] * x, a[1] * x)
180
181
182def dotProduct(a, b):
183    return a[0] * b[0] + a[1] * b[1]
184
185
186def orthoIntersection(a, b):
187    c = add(a, b)
188    quot = dotProduct(c, a)
189    if quot != 0:
190        return mul(mul(c, dotProduct(a, a)), 1 / quot)
191    else:
192        return None
193
194
195def length(a):
196    return math.sqrt(dotProduct(a, a))
197
198
199def norm(a):
200    return mul(a, 1 / length(a))
201
202
203def narrow(fromPos, pos, toPos, amount):
204    """detect narrow turns which cannot be shifted regularly"""
205    a = sub(toPos, pos)
206    b = sub(pos, fromPos)
207    c = add(a, b)
208    dPac = dotProduct(a, c)
209    if dPac == 0:
210        return True
211    x = dotProduct(a, a) * length(c) / dPac
212    return x < amount
213
214
215def move2side(shape, amount):
216    shape = [s for i, s in enumerate(shape) if i == 0 or shape[i-1] != s]
217    if len(shape) < 2:
218        return shape
219    if polyLength(shape) == 0:
220        return shape
221    result = []
222    for i, pos in enumerate(shape):
223        if i == 0:
224            fromPos = pos
225            toPos = shape[i + 1]
226            if fromPos != toPos:
227                result.append(sub(fromPos, sideOffset(fromPos, toPos, amount)))
228        elif i == len(shape) - 1:
229            fromPos = shape[i - 1]
230            toPos = pos
231            if fromPos != toPos:
232                result.append(sub(toPos, sideOffset(fromPos, toPos, amount)))
233        else:
234            fromPos = shape[i - 1]
235            toPos = shape[i + 1]
236            # check for narrow turns
237            if narrow(fromPos, pos, toPos, amount):
238                # print("narrow at i=%s pos=%s" % (i, pos))
239                pass
240            else:
241                a = sideOffset(fromPos, pos, -amount)
242                b = sideOffset(pos, toPos, -amount)
243                c = orthoIntersection(a, b)
244                if orthoIntersection is not None:
245                    pos2 = add(pos, c)
246                else:
247                    extend = norm(sub(pos, fromPos))
248                    pos2 = add(pos, mul(extend, amount))
249                result.append(pos2)
250    # print("move2side", amount)
251    # print(shape)
252    # print(result)
253    # print()
254    return result
255