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