1# Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo 2# Copyright (C) 2016-2019 German Aerospace Center (DLR) and others. 3# SUMOPy module 4# Copyright (C) 2012-2017 University of Bologna - DICAM 5# This program and the accompanying materials 6# are made available under the terms of the Eclipse Public License v2.0 7# which accompanies this distribution, and is available at 8# http://www.eclipse.org/legal/epl-v20.html 9# SPDX-License-Identifier: EPL-2.0 10 11# @file geometry.py 12# @author Joerg Schweizer 13# @date 14# @version $Id$ 15 16import numpy as np 17 18 19def get_norm_2d(vertex3d): 20 # print 'get_norm_2d',vertex3d.shape 21 return np.sqrt(np.sum(vertex3d[:, :2]**2, 1)) 22 # print ' r',r.shape 23 # return r 24 25 26def get_length_polylines(polylines): 27 # print 'get_length_polylines' 28 # v = np.array([[[0.0,0.0,0.0],[1,0.0,0.0]],[[1,0.0,0.0],[1,2,0.0]],[[1,2.0,0.0],[1,2,3.0]] ]) 29 lengths = np.zeros(len(polylines), np.float) 30 i = 0 31 for v in polylines: 32 # print ' v=\n',v,v.shape 33 # print ' v[:,0,:]\n',v[:,0,:] 34 # print ' v[:,1,:]\n',v[:,1,:] 35 lengths[i] = np.sum(np.sqrt(np.sum((v[:, 1, :]-v[:, 0, :])**2, 1))) 36 i += 1 37 return lengths 38 39 40def get_length_polypoints(polylines): 41 # print 'get_length_polypoints' 42 # v = np.array([[[0.0,0.0,0.0],[1,0.0,0.0]],[[1,0.0,0.0],[1,2,0.0]],[[1,2.0,0.0],[1,2,3.0]] ]) 43 lengths = np.zeros(len(polylines), np.float) 44 i = 0 45 for v in polylines: 46 # print ' v=\n',v 47 a = np.array(v, np.float32) 48 # print ' a=\n',a,a.shape 49 lengths[i] = np.sum(np.sqrt(np.sum((a[1:, :]-a[:-1, :])**2, 1))) 50 i += 1 51 return lengths 52 53 54def polypoints_to_polylines(polypoints): 55 linevertices = np.array([None]*len(polypoints), np.object) # np.zeros((0,2,3),np.float32) 56 #polyinds = [] 57 #lineinds = [] 58 59 ind = 0 60 ind_line = 0 61 for polyline in polypoints: 62 # Important type conversion!! 63 v = np.zeros((2*len(polyline)-2, 3), np.float32) 64 v[0] = polyline[0] 65 v[-1] = polyline[-1] 66 if len(v) > 2: 67 v[1:-1] = np.repeat(polyline[1:-1], 2, 0) 68 69 #n_lines = len(v)/2 70 #polyinds += n_lines*[ind] 71 # lineinds.append(np.arange(ind_line,ind_line+n_lines)) 72 #ind_line += n_lines 73 74 linevertices[ind] = v.reshape((-1, 2, 3)) 75 #linevertices = np.concatenate((linevertices, v.reshape((-1,2,3)))) 76 77 ind += 1 78 79 return linevertices # , lineinds, polyinds 80 81 82def get_vec_on_polyline_from_pos(polyline, pos, length, angle=0.0): 83 """ 84 Returns a vector ((x1,y1,z1),(x2,y2,z2)) 85 where first coordinate is the point on the polyline at position pos 86 and the second coordinate is length meters ahead with an angle 87 with respect to the direction of the polyline. 88 89 TODO: Attention angle not yet implemented 90 """ 91 pos_edge = 0.0 92 pos_edge_pre = 0.0 93 x1, y1, z1 = polyline[0] 94 95 for j in xrange(1, len(polyline)): 96 x2, y2, z2 = polyline[j] 97 seglength = np.linalg.norm([x2-x1, y2-y1]) 98 pos_edge += seglength 99 100 if (pos >= pos_edge_pre) & (pos <= pos_edge): 101 102 dxn = (x2-x1)/seglength 103 dyn = (y2-y1)/seglength 104 u1 = (pos-pos_edge_pre) 105 106 u2 = (pos+length-pos_edge_pre) 107 return np.array([[x1 + u1 * dxn, y1 + u1 * dyn, z2], [x1 + u2 * dxn, y1 + u2 * dyn, z2]], np.float32) 108 109 x1, y1 = x2, y2 110 pos_edge_pre = pos_edge 111 112 x1, y1, z1 = polyline[-1] 113 dxn = (x2-x1)/seglength 114 dyn = (y2-y1)/seglength 115 u1 = (pos-pos_edge_pre) 116 u2 = (pos+length-pos_edge_pre) 117 return np.array([[x2, y2, z2], [x2 + u2 * dxn, y1 + u2 * dyn, z2]], np.float32) 118 119 120def get_coord_on_polyline_from_pos(polyline, pos): 121 pos_edge = 0.0 122 pos_edge_pre = 0.0 123 x1, y1, z1 = polyline[0] 124 125 for j in xrange(1, len(polyline)): 126 x2, y2, z2 = polyline[j] 127 length = np.linalg.norm([x2-x1, y2-y1]) 128 pos_edge += length 129 130 if (pos >= pos_edge_pre) & (pos <= pos_edge): 131 u = (pos-pos_edge_pre)/length 132 x = x1 + u * (x2-x1) 133 y = y1 + u * (y2-y1) 134 return np.array([x, y, z2], np.float32) 135 136 x1, y1 = x2, y2 137 pos_edge_pre = pos_edge 138 139 return np.array([x2, y2, z2], np.float32) 140 141 142def get_coord_angle_on_polyline_from_pos(polyline, pos): 143 """ 144 Return coordinate and respective angle 145 at position pos on the polygon. 146 """ 147 pos_edge = 0.0 148 pos_edge_pre = 0.0 149 x1, y1, z1 = polyline[0] 150 151 for j in xrange(1, len(polyline)): 152 x2, y2, z2 = polyline[j] 153 length = np.linalg.norm([x2-x1, y2-y1]) 154 pos_edge += length 155 156 if (pos >= pos_edge_pre) & (pos <= pos_edge): 157 u = (pos-pos_edge_pre)/length 158 dx = (x2-x1) 159 dy = (y2-y1) 160 x = x1 + u * dx 161 y = y1 + u * dy 162 return np.array([x, y, z2], np.float32), np.arctan2(dy, dx) 163 164 dx = (x2-x1) 165 dy = (y2-y1) 166 x1, y1 = x2, y2 167 pos_edge_pre = pos_edge 168 169 return np.array([x2, y2, z2], np.float32), np.arctan2(dy, dx) 170 171 172def get_pos_on_polyline_from_coord(polyline, coord): 173 xc, yc, zc = coord 174 n_segs = len(polyline) 175 176 d_min = 10.0**8 177 x_min = 0.0 178 y_min = 0.0 179 j_min = 0 180 p_min = 0.0 181 pos = 0.0 182 x1, y1, z1 = polyline[0] 183 for j in xrange(1, n_segs): 184 x2, y2, z2 = polyline[j] 185 d, xp, yp = shortest_dist(x1, y1, x2, y2, xc, yc) 186 # print ' x1,y1=(%d,%d)'%(x1,y1),',x2,y2=(%d,%d)'%(x2,y2),',xc,yc=(%d,%d)'%(xc,yc) 187 # print ' d,x,y=(%d,%d,%d)'%shortest_dist(x1,y1, x2,y2, xc,yc) 188 if d < d_min: 189 d_min = d 190 # print ' **d_min=',d_min,[xp,yp] 191 x_min = xp 192 y_min = yp 193 j_min = j 194 p_min = pos 195 # print ' pos',pos,[x2-x1,y2-y1],'p_min',p_min 196 pos += np.linalg.norm([x2-x1, y2-y1]) 197 x1, y1 = x2, y2 198 199 x1, y1, z1 = polyline[j_min-1] 200 return p_min+np.linalg.norm([x_min-x1, y_min-y1]) 201 202 203def shortest_dist(x1, y1, x2, y2, x3, y3): # x3,y3 is the point 204 """ 205 Shortest distance between point (x3,y3) and line (x1,y1-> x2,y2). 206 Returns distance and projected point on line. 207 """ 208 209 px = x2-x1 210 py = y2-y1 211 212 something = px*px + py*py 213 if something > 0: 214 u = ((x3 - x1) * px + (y3 - y1) * py) / float(something) 215 else: 216 u = 0 217 218 # clip and return infinite distance if not on the line 219 if u > 1: 220 u = 1 221 # return 10.0**8,x1 + px,y1 + py 222 223 elif u < 0: 224 u = 0 225 # return 10.0**8,x1 ,y1 226 227 x = x1 + u * px 228 y = y1 + u * py 229 230 dx = x - x3 231 dy = y - y3 232 233 # Note: If the actual distance does not matter, 234 # if you only want to compare what this function 235 # returns to other results of this function, you 236 # can just return the squared distance instead 237 # (i.e. remove the sqrt) to gain a little performance 238 239 dist = np.sqrt(dx*dx + dy*dy) 240 241 return dist, x, y 242 243 244def get_dist_point_to_segs(p, y1, x1, y2, x2, is_ending=True, 245 is_detect_initial=False, 246 is_detect_final=False, 247 #is_return_pos = False 248 ): 249 """ 250 Minimum square distance between a Point p = (x,y) and a Line segments , 251 where vectors x1, y1 are the first points and x2,y2 are the second points 252 of the line segments. 253 254 If is_detect_initial is True then a point whose projection is beyond 255 the start of the segment will result in a NaN distance. 256 257 If is_detect_final is True then a point whose projection is beyond 258 the end of the segment will result in a NaN distance. 259 260 If is_return_pos then the position on the section is returned. 261 262 Inspired by the description by Paul Bourke, October 1988 263 http://paulbourke.net/geometry/pointlineplane/ 264 265 Rewritten in vectorial form by Joerg Schweizer 266 """ 267 268 y3, x3 = p 269 270 d = np.zeros(len(y1), dtype=np.float32) 271 272 dx21 = (x2-x1) 273 dy21 = (y2-y1) 274 275 lensq21 = dx21*dx21 + dy21*dy21 276 277 # indexvector for all zero length lines 278 iz = (lensq21 == 0) 279 280 dy = y3-y1[iz] 281 dx = x3-x1[iz] 282 283 d[iz] = dx*dx + dy*dy 284 285 lensq21[iz] = 1.0 # replace zeros with 1.0 to avoid div by zero error 286 287 u = (x3-x1)*dx21 + (y3-y1)*dy21 288 u = u / lensq21 # normalize position 289 290 x = x1 + u * dx21 291 y = y1 + u * dy21 292 293 if is_ending: 294 ie = u < 0 295 x[ie] = x1[ie] 296 y[ie] = y1[ie] 297 ie = u > 1 298 x[ie] = x2[ie] 299 y[ie] = y2[ie] 300 301 dx30 = x3-x 302 dy30 = y3-y 303 d[~iz] = (dx30*dx30 + dy30*dy30)[~iz] 304 305 if is_detect_final: 306 d[iz | (u > 1)] = np.nan 307 308 if is_detect_initial: 309 d[iz | (u < 0)] = np.nan 310 311 return d 312 313 314def is_inside_triangles(p, x1, y1, x2, y2, x3, y3): 315 """ 316 Returns a binary vector with True if point p is 317 inside a triangle. 318 x1,y1,x2,y2,x3,y3 are vectors with the 3 coordiantes of the triangles. 319 """ 320 alpha = ((y2 - y3)*(p[0] - x3) + (x3 - x2)*(p[1] - y3)) \ 321 / ((y2 - y3)*(x1 - x3) + (x3 - x2)*(y1 - y3)) 322 323 beta = ((y3 - y1)*(p[0] - x3) + (x1 - x3)*(p[1] - y3)) \ 324 / ((y2 - y3)*(x1 - x3) + (x3 - x2)*(y1 - y3)) 325 326 gamma = 1.0 - alpha - beta 327 return (alpha > 0) & (beta > 0) & (gamma > 0) 328 329# from http://www.arachnoid.com/area_irregular_polygon/index.html 330 331 332def find_area_perim(array): 333 """ 334 Scalar! 335 """ 336 a = 0 337 p = 0 338 ox, oy = array[0] 339 for x, y in array[1:]: 340 a += (x*oy-y*ox) 341 p += abs((x-ox)+(y-oy)*1j) 342 ox, oy = x, y 343 return a/2, p 344 345 346def find_area(array): 347 """ 348 Single polygon with 2D coordinates. 349 """ 350 # TODO: check, there are negative A!!!! 351 # print 'find_area',array 352 a = 0 353 ox, oy = array[0] 354 for x, y in array[1:]: 355 a += (x*oy-y*ox) 356 ox, oy = x, y 357 358 # print ' =',np.abs(a/2) 359 return np.abs(a/2) 360 361 362def get_polygonarea_fast(x, y): 363 """ 364 Returns area in polygon represented by x,y vectors. 365 Attention: last point is not first point. 366 """ 367 # https://stackoverflow.com/questions/24467972/calculate-area-of-polygon-given-x-y-coordinates 368 return 0.5*np.abs(np.dot(x, np.roll(y, 1))-np.dot(y, np.roll(x, 1))) 369 370 371def is_point_in_polygon(point, poly): 372 """ 373 Scalar! 374 """ 375 is_3d = len(point) == 3 376 377 if is_3d: 378 x, y, z = point 379 p1x, p1y, p1z = poly[0] 380 else: 381 x, y = point 382 p1x, p1y = poly[0] 383 n = len(poly) 384 inside = False 385 386 for i in range(n+1): 387 if is_3d: 388 p2x, p2y, p2z = poly[i % n] 389 else: 390 p2x, p2y = poly[i % n] 391 if y > min(p1y, p2y): 392 if y <= max(p1y, p2y): 393 if x <= max(p1x, p2x): 394 if p1y != p2y: 395 xints = (y-p1y)*(p2x-p1x)/(p2y-p1y)+p1x 396 if p1x == p2x or x <= xints: 397 inside = not inside 398 p1x, p1y = p2x, p2y 399 400 return inside 401 402 403def is_polyline_intersect_polygon(polyline, polygon): 404 for p in polyline: 405 if is_point_in_polygon(p, polygon): 406 return True 407 return False 408 409 410def is_polyline_in_polygon(polyline, polygon): 411 for p in polyline: 412 if not is_point_in_polygon(p, polygon): 413 return False 414 return True 415 416 417def get_angles_perpendicular(shape): 418 """ 419 Returns the angle vector angles_perb which is perpendicular to the given shape. 420 The normalized 421 dxn = np.cos(angles_perb) 422 dyn = np.sin(angles_perb) 423 """ 424 425 n_vert = len(shape) 426 # print 'get_laneshapes',_id,n_lanes,n_vert 427 428 #width = self.widths_lanes_default[_id] 429 # print ' shape', shape ,len( shape) 430 v_ext_begin = (shape[0]-(shape[1]-shape[0])).reshape(1, 3) 431 v_ext_end = (shape[-1]+(shape[-1]-shape[-2])).reshape(1, 3) 432 433 exshape = np.concatenate((v_ext_begin, shape, v_ext_end))[:, 0:2] 434 # print ' exshape', exshape,len( exshape) 435 vertex_delta_x = exshape[1:, 0]-exshape[0:-1, 0] 436 vertex_delta_y = exshape[1:, 1]-exshape[0:-1, 1] 437 438 angles = np.arctan2(vertex_delta_y, vertex_delta_x) 439 #angles = np.mod(np.arctan2(vertex_delta_y,vertex_delta_x)+2*np.pi,2*np.pi) 440 #angles_perb = 0.5 *(angles[1:]+angles[0:-1])-np.pi/2 441 442 angles1 = angles[1:] 443 angles2 = angles[0:-1] 444 ind_discont = (angles1 < -0.5*np.pi) & ((angles2 > 0.5*np.pi)) | (angles2 < -0.5*np.pi) & ((angles1 > 0.5*np.pi)) 445 angle_sum = angles1+angles2 446 angle_sum[ind_discont] += 2*np.pi 447 448 #angles = np.mod(np.arctan2(vertex_delta_y,vertex_delta_x)+2*np.pi,2*np.pi) 449 #angle_sum = angles[1:]+angles[0:-1] 450 #ind_discont = angle_sum>2*np.pi 451 #angle_sum[ind_discont] = angle_sum[ind_discont]-2*np.pi 452 return 0.5*angle_sum-np.pi/2 453 454 455def rotate_vertices(vec_x, vec_y, alphas=None, 456 sin_alphas=None, cos_alphas=None, 457 is_array=False): 458 """ 459 Rotates all vertices around 460 """ 461 # print 'rotate_vertices',vec_x, vec_y 462 463 if alphas is not None: 464 sin_alphas = np.sin(alphas) 465 cos_alphas = np.cos(alphas) 466 # print ' sin_alphas',sin_alphas 467 # print ' cos_alphas',cos_alphas 468 #deltas = vertices - offsets 469 #vertices_rot = np.zeros(vertices.shape, np.float32) 470 #n= size(vec_x) 471 vec_x_rot = vec_x*cos_alphas - vec_y*sin_alphas 472 vec_y_rot = vec_x*sin_alphas + vec_y*cos_alphas 473 # print ' vec_x_rot',vec_x_rot 474 # print ' vec_y_rot',vec_y_rot 475 if is_array: 476 # print ' concatenate', np.concatenate((vec_x_rot.reshape(-1,1), vec_y_rot.reshape(-1,1)),1) 477 return np.concatenate((vec_x_rot.reshape(-1, 1), vec_y_rot.reshape(-1, 1)), 1) 478 else: 479 return vec_x_rot, vec_y_rot 480 481 482T = np.array([[0, -1], [1, 0]]) 483 484 485def line_intersect(a1, a2, b1, b2): 486 """ 487 Works for multiple points in each of the input arguments, i.e., a1, a2, b1, b2 can be Nx2 row arrays of 2D points. 488 https://stackoverflow.com/questions/3252194/numpy-and-line-intersections 489 """ 490 da = np.atleast_2d(a2 - a1) 491 db = np.atleast_2d(b2 - b1) 492 dp = np.atleast_2d(a1 - b1) 493 dap = np.dot(da, T) 494 denom = np.sum(dap * db, axis=1) 495 num = np.sum(dap * dp, axis=1) 496 return np.atleast_2d(num / denom).T * db + b1 497 498 499def line_intersect2(a1, a2, b1, b2): 500 """ 501 Works for multiple points in each of the input arguments, i.e., a1, a2, b1, b2 can be Nx2 row arrays of 2D points. 502 https://stackoverflow.com/questions/3252194/numpy-and-line-intersections 503 Returns also a scale factor which indicates wheter the intersection 504 is in positive or negative direction of the b vector. 505 """ 506 da = np.atleast_2d(a2 - a1) 507 db = np.atleast_2d(b2 - b1) 508 dp = np.atleast_2d(a1 - b1) 509 dap = np.dot(da, T) 510 denom = np.sum(dap * db, axis=1) 511 num = np.sum(dap * dp, axis=1) 512 la = np.atleast_2d(num / denom).T 513 return la * db + b1, la 514 515 516def get_diff_angle_clockwise(p1, p2): 517 """ 518 Returns the clockwise angle between vector 519 0 -> p1 and 0 -> p2 520 521 p1=np.array([[x11,x11,x13,...],[y11,y12,y13,...]]) 522 p2 =np.array([[x21,x21,x23,...],[y21,y22,y23,...]]) 523 524 """ 525 ang1 = np.arctan2(*p1[::-1]) 526 ang2 = np.arctan2(*p2[::-1]) 527 # return np.rad2deg((ang1 - ang2) % (2 * np.pi)) 528 return (ang1 - ang2) % (2 * np.pi) 529################################################################ 530# old 531 532# find indees where 2 arrays are identical 533#idx = np.argwhere(np.diff(np.sign(f - g)) != 0).reshape(-1) + 0 534 535 536def angle2D(p1, p2): 537 theta1 = math.atan2(p1[1], p1[0]) 538 theta2 = math.atan2(p2[1], p2[0]) 539 dtheta = theta2 - theta1 540 while dtheta > np.pi: 541 dtheta -= 2.0*np.pi 542 while dtheta < -np.pi: 543 dtheta += 2.0*np.pi 544 return dtheta 545 546 547def is_point_within_polygon(pos, shape): 548 angle = 0. 549 pos = np.array(pos, float) 550 shape = np.array(shape, float) 551 for i in range(0, len(shape)-1): 552 p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1])) 553 p2 = ((shape[i+1][0] - pos[0]), (shape[i+1][1] - pos[1])) 554 angle = angle + angle2D(p1, p2) 555 i = len(shape)-1 556 p1 = ((shape[i][0] - pos[0]), (shape[i][1] - pos[1])) 557 p2 = ((shape[0][0] - pos[0]), (shape[0][1] - pos[1])) 558 angle = angle + angle2D(p1, p2) 559 return math.fabs(angle) >= np.pi 560