From 9bece74ffd2999d92cf60272d1eae6c8c02b1383 Mon Sep 17 00:00:00 2001 From: Xeon0X Date: Mon, 27 May 2024 17:52:49 +0200 Subject: [PATCH] Add intersections utilities --- main.py | 24 +- networks/geometry/point.py | 468 +++++++++++++++++++++++++ networks/intersections/Intersection.py | 3 + networks/roads/roads.json | 17 +- 4 files changed, 498 insertions(+), 14 deletions(-) create mode 100644 networks/geometry/point.py create mode 100644 networks/intersections/Intersection.py diff --git a/main.py b/main.py index 8955bf5..947e690 100644 --- a/main.py +++ b/main.py @@ -9,7 +9,9 @@ import json from buildings.Building import Building import random -# editor = Editor(buffering=True) +from networks.geometry.point import curveCornerIntersectionLine, curveCornerIntersectionPoints + +editor = Editor(buffering=True) # f = open('buildings\shapes.json') # shapes = json.load(f) @@ -92,14 +94,24 @@ block_list = ["blue_concrete", "red_concrete", "green_concrete", # # editor.placeBlock(coordinate, Block("yellow_concrete")) -coordinates = [(0, 0, 0), (0, 0, 10), (0, 0, 20)] +# coordinates = [(0, 0, 0), (0, 0, 10), (0, 0, 20)] -with open('networks/lines/lines.json') as f: - lines_type = json.load(f) - l = Line.Line(coordinates, lines_type.get('solid_white')) - print(l.get_surface()) +# with open('networks/lines/lines.json') as f: +# lines_type = json.load(f) +# l = Line.Line(coordinates, lines_type.get('solid_white')) +# print(l.get_surface()) # with open('networks/lanes/lanes.json') as f: # lanes_type = json.load(f) # l = Lane.Lane(coordinates, lanes_type.get('classic_lane'), 5) # print(l.get_surface()) + + +circle = curveCornerIntersectionLine( + ((-1313, 392), (-1378, 415)), ((-1371, 348), (-1341, 439)), 30, angleAdaptation=True) + +print(circle[0]) + +for coordinate in circle[0]: + editor.placeBlock( + (round(coordinate[0]), 100, round(coordinate[1])), Block("green_concrete")) diff --git a/networks/geometry/point.py b/networks/geometry/point.py new file mode 100644 index 0000000..e659af4 --- /dev/null +++ b/networks/geometry/point.py @@ -0,0 +1,468 @@ +from math import sqrt, cos, pi, sin +import numpy as np + + +def circle(xyC, r): + """ + Can be used for circle or disc. + + Args: + xyC (tuple): Coordinates of the center. + r (int): Radius of the circle. + + Returns: + dict: Keys are distance from the circle. Value is a list of all + coordinates at this distance. 0 for a circle. Negative values + for a disc, positive values for a hole. + """ + area = ( + (round(xyC[0]) - round(r), round(xyC[1]) - round(r)), + (round(xyC[0]) + round(r) + 1, round(xyC[1]) + round(r) + 1), + ) + + circle = {} + for x in range(area[0][0], area[1][0]): + for y in range(area[0][1], area[1][1]): + d = round(distance2D((x, y), (xyC))) - r + if circle.get(d) == None: + circle[d] = [] + circle[d].append((x, y)) + return circle + + +def InTriangle(point, xy0, xy1, xy2): + # https://stackoverflow.com/questions/2049582/how-to-determine-if-a-point-is-in-a-2d-triangle#:~:text=A%20simple%20way%20is%20to,point%20is%20inside%20the%20triangle. + dX = point[0] - xy0[0] + dY = point[1] - xy0[1] + dX20 = xy2[0] - xy0[0] + dY20 = xy2[1] - xy0[1] + dX10 = xy1[0] - xy0[0] + dY10 = xy1[1] - xy0[1] + + s_p = (dY20 * dX) - (dX20 * dY) + t_p = (dX10 * dY) - (dY10 * dX) + D = (dX10 * dY20) - (dY10 * dX20) + + if D > 0: + return (s_p >= 0) and (t_p >= 0) and (s_p + t_p) <= D + else: + return (s_p <= 0) and (t_p <= 0) and (s_p + t_p) >= D + + +def distance2D(A, B): # TODO : Can be better. + return sqrt((B[0] - A[0]) ** 2 + (B[1] - A[1]) ** 2) + + +def getAngle(xy0, xy1, xy2): + """ + Compute angle (in degrees) for xy0, xy1, xy2 corner. + + https://stackoverflow.com/questions/13226038/calculating-angle-between-two-vectors-in-python + + Args: + xy0 (numpy.ndarray): Points in the form of [x,y]. + xy1 (numpy.ndarray): Points in the form of [x,y]. + xy2 (numpy.ndarray): Points in the form of [x,y]. + + Returns: + float: Angle negative for counterclockwise angle, angle positive + for counterclockwise angle. + """ + if xy2 is None: + xy2 = xy1 + np.array([1, 0]) + v0 = np.array(xy0) - np.array(xy1) + v1 = np.array(xy2) - np.array(xy1) + + angle = np.math.atan2(np.linalg.det([v0, v1]), np.dot(v0, v1)) + return np.degrees(angle) + + +def circlePoints(center_point, radius, number=100): + # https://stackoverflow.com/questions/8487893/generate-all-the-points-on-the-circumference-of-a-circle + points = [ + (cos(2 * pi / number * x) * radius, sin(2 * pi / number * x) * radius) + for x in range(0, number + 1) + ] + + for i in range(len(points)): + points[i] = ( + points[i][0] + center_point[0], + points[i][1] + center_point[1], + ) + + return points + + +def optimizedPath(points, start=None): + # https://stackoverflow.com/questions/45829155/sort-points-in-order-to-have-a-continuous-curve-using-python + if start is None: + start = points[0] + pass_by = points + path = [start] + pass_by.remove(start) + while pass_by: + nearest = min(pass_by, key=lambda x: distance2D(path[-1], x)) + path.append(nearest) + pass_by.remove(nearest) + return path + + +def nearest(points, start): + return min(points, key=lambda x: distance2D(start, x)) + + +def sortRotation(points): + """ + Sort point in a rotation order. Works in 2d but supports 3d. + + https://stackoverflow.com/questions/58377015/counterclockwise-sorting-of-x-y-data + + Args: + points: List of points to sort in the form of [(x, y, z), (x, y, + z)] or [(x, y), (x, y), (x, y), (x, y)]... + + Returns: + list: List of tuples of coordinates sorted (2d or 3d). + + >>> sortRotation([(0, 45, 100), (4, -5, 5),(-5, 36, -2)]) + [(0, 45, 100), (-5, 36, -2), (4, -5, 5)] + """ + x, y = [], [] + for i in range(len(points)): + x.append(points[i][0]) + y.append(points[i][-1]) + x, y = np.array(x), np.array(y) + + x0 = np.mean(x) + y0 = np.mean(y) + + r = np.sqrt((x - x0) ** 2 + (y - y0) ** 2) + + angles = np.where( + (y - y0) > 0, + np.arccos((x - x0) / r), + 2 * np.pi - np.arccos((x - x0) / r), + ) + + mask = np.argsort(angles) + + x_sorted = list(x[mask]) + y_sorted = list(y[mask]) + + # Rearrange tuples to get the right coordinates. + sortedPoints = [] + for i in range(len(points)): + j = 0 + while (x_sorted[i] != points[j][0]) and (y_sorted[i] != points[j][-1]): + j += 1 + else: + if len(points[0]) == 3: + sortedPoints.append((x_sorted[i], points[j][1], y_sorted[i])) + else: + sortedPoints.append((x_sorted[i], y_sorted[i])) + + return sortedPoints + + +def lineIntersection(line0, line1, fullLine=True): + """ + Find (or not) intersection between two lines. Works in 2d but + supports 3d. + + https://stackoverflow.com/questions/20677795/how-do-i-compute-the-intersection-point-of-two-lines + + Args: + line0 (tuple): Tuple of tuple of coordinates. + line1 (tuple): Tuple of tuple of coordinates. + fullLine (bool, optional): True to find intersections along + full line - not just in the segment. + + Returns: + tuple: Coordinates (2d). + + >>> lineIntersection(((0, 0), (0, 5)), ((2.5, 2.5), (-2.5, 2.5))) + """ + xdiff = (line0[0][0] - line0[1][0], line1[0][0] - line1[1][0]) + ydiff = (line0[0][-1] - line0[1][-1], line1[0][-1] - line1[1][-1]) + + def det(a, b): + return a[0] * b[-1] - a[-1] * b[0] + + div = det(xdiff, ydiff) + if div == 0: + return None + + d = (det(*line0), det(*line1)) + x = det(d, xdiff) / div + y = det(d, ydiff) / div + + if not fullLine: + if ( + min(line0[0][0], line0[1][0]) <= x <= max(line0[0][0], line0[1][0]) + and min(line1[0][0], line1[1][0]) + <= x + <= max(line1[0][0], line1[1][0]) + and min(line0[0][-1], line0[1][-1]) + <= y + <= max(line0[0][-1], line0[1][-1]) + and min(line1[0][-1], line1[1][-1]) + <= y + <= max(line1[0][-1], line1[1][-1]) + ): + return x, y + else: + return None + else: + return x, y + + +def circleLineSegmentIntersection( + circleCenter, circleRadius, xy0, xy1, fullLine=True, tangentTol=1e-9 +): + """ + Find the points at which a circle intersects a line-segment. This + can happen at 0, 1, or 2 points. Works in 2d but supports 3d. + + https://stackoverflow.com/questions/30844482/what-is-most-efficient-way-to-find-the-intersection-of-a-line-and-a-circle-in-py + Note: We follow: http://mathworld.wolfram.com/Circle-LineIntersection.html + + Args: + circleCenter (tuple): The (x, y) location of the circle center. + circleRadius (int): The radius of the circle. + xy0 (tuple): The (x, y) location of the first point of the + segment. + xy1 ([tuple]): The (x, y) location of the second point of the + segment. + fullLine (bool, optional): True to find intersections along + full line - not just in the segment. False will just return + intersections within the segment. Defaults to True. + tangentTol (float, optional): Numerical tolerance at which we + decide the intersections are close enough to consider it a + tangent. Defaults to 1e-9. + + Returns: + list: A list of length 0, 1, or 2, where each element is a point + at which the circle intercepts a line segment (2d). + """ + + (p1x, p1y), (p2x, p2y), (cx, cy) = ( + (xy0[0], xy0[-1]), + (xy1[0], xy1[-1]), + (circleCenter[0], circleCenter[1]), + ) + (x1, y1), (x2, y2) = (p1x - cx, p1y - cy), (p2x - cx, p2y - cy) + dx, dy = (x2 - x1), (y2 - y1) + dr = (dx ** 2 + dy ** 2) ** 0.5 + big_d = x1 * y2 - x2 * y1 + discriminant = circleRadius ** 2 * dr ** 2 - big_d ** 2 + + if discriminant < 0: # No intersection between circle and line + return [] + else: # There may be 0, 1, or 2 intersections with the segment + intersections = [ + ( + cx + + ( + big_d * dy + + sign * (-1 if dy < 0 else 1) * dx * discriminant ** 0.5 + ) + / dr ** 2, + cy + + (-big_d * dx + sign * abs(dy) * discriminant ** 0.5) + / dr ** 2, + ) + for sign in ((1, -1) if dy < 0 else (-1, 1)) + ] # This makes sure the order along the segment is correct + if ( + not fullLine + ): # If only considering the segment, filter out intersections that do not fall within the segment + fraction_along_segment = [ + (xi - p1x) / dx if abs(dx) > abs(dy) else (yi - p1y) / dy + for xi, yi in intersections + ] + intersections = [ + pt + for pt, frac in zip(intersections, fraction_along_segment) + if 0 <= frac <= 1 + ] + if ( + len(intersections) == 2 and abs(discriminant) <= tangentTol + ): # If line is tangent to circle, return just one point (as both intersections have same location) + return [intersections[0]] + else: + return intersections + + +def perpendicular(distance, xy1, xy2): + """ + Return a tuple of the perpendicular coordinates. + + Args: + distance (int): Distance from the line[xy1;xy2]. + xy1 (tuple): First coordinates. + xy2 (tuple): Second coordinates. + + Returns: + tuple: Coordinates of the line length distance, perpendicular + to [xy1; xy2] at xy1. + """ + (x1, y1) = xy1 + (x2, y2) = xy2 + dx = x1 - x2 + dy = y1 - y2 + dist = sqrt(dx * dx + dy * dy) + dx /= dist + dy /= dist + x3 = x1 + (distance / 2) * dy + y3 = y1 - (distance / 2) * dx + x4 = x1 - (distance / 2) * dy + y4 = y1 + (distance / 2) * dx + return ((round(x3), round(y3)), (round(x4), round(y4))) + + +def curveCornerIntersectionPoints( + line0, line1, startDistance, angleAdaptation=False +): + """ + Create points between the two lines to smooth the intersection. + + Args: + line0 (tuple): Tuple of tuple. Line coordinates. Order matters. + line1 (tuple): Tuple of tuple. Line coordinates. Order matters. + startDistance (int): distance from the intersection where the + curve should starts. + angleAdaptation (bool, optional): True will adapt the + startDistance depending of the angle between the two lines. + False will force the distance to be startDistance. Defaults to + False. + + Returns: + [list]: List of tuple of coordinates (2d) that forms the curve. + Starts on the line and end on the other line. + + >>> curveCornerIntersectionPoints(((0, 0), (50, 20)), ((-5, 50), (25, -5)), 10) + """ + intersection = lineIntersection(line0, line1, fullLine=True) + + if intersection == None: + return None + + # Define automatically the distance from the intersection, where the curve + # starts. + if angleAdaptation: + angle = getAngle( + (line0[0][0], line0[0][-1]), + intersection, + (line1[0][0], line1[0][-1]), + ) + # Set here the radius of the circle for a square angle. + startDistance = startDistance * abs(1 / (angle / 90)) + + startCurvePoint = circleLineSegmentIntersection( + intersection, startDistance, line0[0], intersection, fullLine=True + )[0] + endCurvePoint = circleLineSegmentIntersection( + intersection, startDistance, line1[0], intersection, fullLine=True + )[0] + # Higher value for better precision + perpendicular0 = perpendicular(10e3, startCurvePoint, intersection)[0] + perpendicular1 = perpendicular(10e3, endCurvePoint, intersection)[1] + + center = lineIntersection( + (perpendicular0, startCurvePoint), (perpendicular1, endCurvePoint) + ) + + # Distance with startCurvePoint and endCurvePoint from the center are the + # same. + radius = distance2D(startCurvePoint, center) + + circle = circlePoints( + center, round(radius), 32 + ) # n=round((2 * pi * radius) / 32) + + # Find the correct point on the circle. + curveCornerPointsTemp = [startCurvePoint] + for point in circle: + if InTriangle(point, intersection, startCurvePoint, endCurvePoint): + curveCornerPointsTemp.append(point) + curveCornerPointsTemp.append(endCurvePoint) + + # Be sure that all the points are in correct order. + curveCornerPoints = optimizedPath(curveCornerPointsTemp, startCurvePoint) + return curveCornerPoints + + +def curveCornerIntersectionLine( + line0, line1, startDistance, angleAdaptation=False, center=() +): + """ + Create a continuous circular line between the two lines to smooth + the intersection. + + Args: + line0 (tuple): Tuple of tuple. Line coordinates. Order matters. + line1 (tuple): Tuple of tuple. Line coordinates. Order matters. + startDistance (int): distance from the intersection where the + curve should starts. + angleAdaptation (bool, optional): True will adapt the + startDistance depending of the angle between the two lines. + False will force the distance to be startDistance. Defaults to + False. + + Returns: + [list]: List of tuple of coordinates (2d) that forms the curve. + Starts on the line and end on the other line. + + TODO: + angleAdaptation : Set circle radius and not startDistance. + Polar coordinates / Unit circle instead of InTriangle. + + >>> curveCornerIntersectionLine(((0, 0), (50, 20)), ((-5, 50), (25, -5)), 10) + """ + intersection = lineIntersection(line0, line1, fullLine=True) + + if intersection == None: + return None + + # Define automatically the distance from the intersection, where the curve + # starts. + if angleAdaptation: + angle = getAngle( + (line0[0][0], line0[0][-1]), + intersection, + (line1[0][0], line1[0][-1]), + ) + # Set here the radius of the circle for a square angle. + startDistance = startDistance * abs(1 / (angle / 90)) + + startCurvePoint = circleLineSegmentIntersection( + intersection, startDistance, line0[0], intersection, fullLine=True + )[0] + endCurvePoint = circleLineSegmentIntersection( + intersection, startDistance, line1[0], intersection, fullLine=True + )[0] + # Higher value for better precision + perpendicular0 = perpendicular(10e3, startCurvePoint, intersection)[0] + perpendicular1 = perpendicular(10e3, endCurvePoint, intersection)[1] + + if center == (): + center = lineIntersection( + (perpendicular0, startCurvePoint), (perpendicular1, endCurvePoint) + ) + + # Distance with startCurvePoint and endCurvePoint from the center + # are almost the same. + radius = distance2D(startCurvePoint, center) + + circleArc = circle(center, round(radius))[0] + + # Find the correct point on the circle. + curveCornerPointsTemp = [startCurvePoint] + for point in circleArc: + if InTriangle(point, intersection, startCurvePoint, endCurvePoint): + curveCornerPointsTemp.append(point) + # curveCornerPointsTemp.append(endCurvePoint) + + # Be sure that all the points are in correct order. + curveCornerPoints = optimizedPath(curveCornerPointsTemp, startCurvePoint) + return curveCornerPoints, center diff --git a/networks/intersections/Intersection.py b/networks/intersections/Intersection.py new file mode 100644 index 0000000..cdd9713 --- /dev/null +++ b/networks/intersections/Intersection.py @@ -0,0 +1,3 @@ +class Intersection: + def __init__(self, Roads): + self.Roads = Roads diff --git a/networks/roads/roads.json b/networks/roads/roads.json index 83aeae8..027af7c 100644 --- a/networks/roads/roads.json +++ b/networks/roads/roads.json @@ -1,10 +1,11 @@ { - "high_way": { - "3": {"classic_lane": 3} - - }, - "broken_white": { - "3": {"white_concrete": 3, "white_concrete_powder": 1}, - "1": {"None": 1} - } + "high_way": [ + {"lane": "classic_lane", "width": 5, "number": 3}, + {"lane": "classic_divider", "width": 5, "number": 1}, + {"lane": "classic_lane", "width": 5, "number": 3} + ], + + "modern_road": [ + {"lane": "classic_lane", "width": 5, "number": 2} + ] } \ No newline at end of file