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