From 22cf69150f701c5cacb8222032e5d63642c19595 Mon Sep 17 00:00:00 2001 From: Abhishek Gandhi Date: Sun, 20 Mar 2022 16:52:06 +0530 Subject: [PATCH] adding arcs to make circle --- src/tests/test_4.py | 24 ++++++ src/trafficSimulator/road.py | 27 ++++++- src/trafficSimulator/simulation.py | 4 +- src/trafficSimulator/window.py | 120 ++++++++++++++++++++--------- 4 files changed, 134 insertions(+), 41 deletions(-) create mode 100644 src/tests/test_4.py diff --git a/src/tests/test_4.py b/src/tests/test_4.py new file mode 100644 index 0000000..4907d9f --- /dev/null +++ b/src/tests/test_4.py @@ -0,0 +1,24 @@ +from trafficSimulator import * + +# Create simulation +sim = Simulation() + +# Add multiple roads +sim.create_roads([ + ((0, 100), (140, 100)), + ((150, 110), (150, 200)), + ((140, 100), (150, 110), 3,(140, 110)) +]) + +sim.create_gen({ + 'vehicle_rate': 5, + 'vehicles': [ + {"path": [0, 2, 1]} + ] +}) + + +# Start simulation +win = Window(sim) +win.offset = (-150, -110) +win.run(steps_per_update=5) \ No newline at end of file diff --git a/src/trafficSimulator/road.py b/src/trafficSimulator/road.py index 5898123..c4ec040 100644 --- a/src/trafficSimulator/road.py +++ b/src/trafficSimulator/road.py @@ -1,17 +1,40 @@ from scipy.spatial import distance from collections import deque +import math class Road: - def __init__(self, start, end): + def __init__(self, start, end, type = 1, center = None): self.start = start self.end = end + # type = 1 single lane straint + # type = 1 double lane straint + # type = 3 single lane circular + # type = 4 double lane circular ## TODO make it eclipse rather than circular + self.type = type + + # if type = 1 or 2 then centre is None + self.center = center + self.vehicles = deque() self.init_properties() + + def angle(self, A, B, C): + Ax, Ay = A[0]-B[0], A[1]-B[1] + Cx, Cy = C[0]-B[0], C[1]-B[1] + a = math.atan2(Ay, Ax) + c = math.atan2(Cy, Cx) + if a < 0: a += math.pi*2 + if c < 0: c += math.pi*2 + return (math.pi*2 + c - a) if a > c else (c - a) def init_properties(self): - self.length = distance.euclidean(self.start, self.end) + if(self.type<=2): + self.length = distance.euclidean(self.start, self.end) + else: + self.radius = distance.euclidean(self.center, self.end) + self.length = distance.euclidean(self.start, self.center) * self.angle(self.start,self.center,self.end) self.angle_sin = (self.end[1]-self.start[1]) / self.length self.angle_cos = (self.end[0]-self.start[0]) / self.length # self.angle = np.arctan2(self.end[1]-self.start[1], self.end[0]-self.start[0]) diff --git a/src/trafficSimulator/simulation.py b/src/trafficSimulator/simulation.py index 762579c..8200bde 100644 --- a/src/trafficSimulator/simulation.py +++ b/src/trafficSimulator/simulation.py @@ -24,8 +24,8 @@ def set_default_config(self): self.traffic_signals = [] self.integ = 0 - def create_road(self, start, end): - road = Road(start, end) + def create_road(self, start, end, type=1, center=None): + road = Road(start, end, type, center) self.roads.append(road) return road diff --git a/src/trafficSimulator/window.py b/src/trafficSimulator/window.py index f59fcb7..36fe00e 100644 --- a/src/trafficSimulator/window.py +++ b/src/trafficSimulator/window.py @@ -2,6 +2,8 @@ from pygame import gfxdraw import numpy as np import random +from scipy.spatial import distance +import math class Window: def __init__(self, sim, config={}): @@ -152,7 +154,15 @@ def circle(self, pos, radius, color, filled=True): gfxdraw.aacircle(self.screen, *pos, radius, color) if filled: gfxdraw.filled_circle(self.screen, *pos, radius, color) + + def draw_arc(self, center, start_angle, end_angle, radius_in, color_out): + center = self.convert(*center) + rect = pygame.Rect(center[0]-self.zoom*radius_in,center[1]-self.zoom*radius_in,2*radius_in*self.zoom, 2*radius_in*self.zoom) + pygame.draw.arc(self.screen,color_out,rect,-end_angle,-start_angle, 6*self.zoom) + # gfxdraw.pie(self.screen,*self.convert(*center), radius_in,start_angle,end_angle, color_in) + # gfxdraw.arc(self.screen,*self.convert(*center), 11,0,3, color_out) + #i = 0 def polygon(self, vertices, color, filled=True): @@ -164,9 +174,15 @@ def rotated_box(self, pos, size, angle=None, cos=None, sin=None, centered=True, """Draws a rectangle center at *pos* with size *size* rotated anti-clockwise by *angle*.""" x, y = pos l, h = size + #print(pos) + #print(size) if angle: cos, sin = np.cos(angle), np.sin(angle) + #print(x) + #print(y) + #print(cos) + #print(sin) vertex = lambda e1, e2: ( x + (e1*l*cos + e2*h*sin)/2, @@ -182,6 +198,7 @@ def rotated_box(self, pos, size, angle=None, cos=None, sin=None, centered=True, [vertex(*e) for e in [(0,-1), (0, 1), (2,1), (2,-1)]] ) + #print(vertices) self.polygon(vertices, color, filled=filled) #pygame.draw.rect(self.screen, color, (x, y, l, h)) @@ -250,50 +267,79 @@ def draw_grid(self, unit=50, color=(150,150,150)): def draw_roads(self): for road in self.sim.roads: # Draw road background - self.rotated_box( - road.start, - (road.length, 6), - cos=road.angle_cos, - sin=road.angle_sin, - color=(128, 128, 128), - centered=False - ) - # Draw road lines - # self.rotated_box( - # road.start, - # (road.length, 0.25), - # cos=road.angle_cos, - # sin=road.angle_sin, - # color=(0, 0, 0), - # centered=False - # ) - - # Draw road arrow - if road.length > 5: - for i in np.arange(-0.5*road.length, 0.5*road.length, 10): - pos = ( - road.start[0] + (road.length/2 + i + 3) * road.angle_cos, - road.start[1] + (road.length/2 + i + 3) * road.angle_sin - ) - - self.rotated_box( - pos, - (-1.25, 0.2), - cos=road.angle_cos, - sin=road.angle_sin, - color=(255, 255, 255) - ) + #print(road.start) + if road.type <=2 : + self.rotated_box( + road.start, + (road.length, 6*road.type), + cos=road.angle_cos, + sin=road.angle_sin, + color=(128, 128, 128), + centered=False + ) + # Draw road lines + # self.rotated_box( + # road.start, + # (road.length, 0.25), + # cos=road.angle_cos, + # sin=road.angle_sin, + # color=(0, 0, 0), + # centered=False + # ) + + # Draw road arrow + if road.length > 5: + for i in np.arange(-0.5*road.length, 0.5*road.length, 10): + pos = ( + road.start[0] + (road.length/2 + i + 3) * road.angle_cos, + road.start[1] + (road.length/2 + i + 3) * road.angle_sin + ) + + self.rotated_box( + pos, + (-1.25, 0.2), + cos=road.angle_cos, + sin=road.angle_sin, + color=(255, 255, 255) + ) + + else: + start_angle = np.arctan2(road.start[1] - road.center[1],road.start[0] - road.center[0]) + end_angle = np.arctan2(road.end[1] - road.center[1],road.end[0] - road.center[0]) + self.draw_arc(road.center, + (start_angle), + (end_angle), + (distance.euclidean(road.center,road.start)+3*(road.type-2)), + (128, 128, 128) + ) # TODO: Draw road arrow def draw_vehicle(self, vehicle, road): + l, h = vehicle.l, 2 - sin, cos = road.angle_sin, road.angle_cos - - x = road.start[0] + cos * vehicle.x - y = road.start[1] + sin * vehicle.x + if road.type<=2: + sin, cos = road.angle_sin, road.angle_cos + x = road.start[0] + cos * vehicle.x + y = road.start[1] + sin * vehicle.x + + else: + theta = vehicle.x/road.radius + #print(theta) + start_angle = -np.arctan2(road.start[1] - road.center[1],road.start[0] - road.center[0]) + end_angle = -np.arctan2(road.end[1] - road.center[1],road.end[0] - road.center[0]) + #if(start_angle) + if(start_angle > end_angle): + final_angle = start_angle-theta + else: + final_angle = start_angle+theta + + sin, cos = math.sin(math.pi/2-final_angle), math.cos(math.pi/2-final_angle) + x = road.center[0] + sin* road.radius + y = road.center[1] - cos * road.radius + self.rotated_box((x, y), (l, h), cos=cos, sin=sin, centered=True)