From 34734dd73419628fbbb07caaf52bf0248715b3e1 Mon Sep 17 00:00:00 2001 From: alexandre Date: Wed, 9 Oct 2019 17:32:04 +0200 Subject: [PATCH] segment intersections does not work, algorithm tbt --- main.py | 104 +++++++++++++++++++++++++++++++++++++++++++++++-------- trigo.py | 34 ++++++++++++++++++ 2 files changed, 123 insertions(+), 15 deletions(-) create mode 100644 trigo.py diff --git a/main.py b/main.py index 0e1de30..45a2a19 100755 --- a/main.py +++ b/main.py @@ -1,7 +1,9 @@ #!/usr/bin/env python +import math import pygame from pygame.locals import HWSURFACE, DOUBLEBUF -import math +import random +from trigo import angle_to_vector, segments_intersection, distance FLAGS= HWSURFACE | DOUBLEBUF #| FULLSCREEN @@ -10,17 +12,15 @@ GY = 800 CELL_COLOR = (80,80,80) CAR_SIZE=50 pygame.init() - IMG = pygame.image.load("car50.png")#.convert() -def angle_to_vector(angle): - angle=angle*math.pi/180 - return [math.cos(angle), math.sin(angle)] + + class Car(pygame.sprite.Sprite): def __init__(self): pygame.sprite.Sprite.__init__(self) - self.surface = pygame.Surface((CAR_SIZE, CAR_SIZE)) + self.top_surface = pygame.Surface((CAR_SIZE, CAR_SIZE)) self.original_image = IMG # self.image = pygame.Surface((CAR_SIZE, CAR_SIZE)) # self.image.fill((0,255,0)) @@ -28,14 +28,14 @@ class Car(pygame.sprite.Sprite): self.image = self.original_image - self.rect = self.surface.get_rect() + self.rect = self.image.get_rect() self.rect.center = (GX / 2, GY / 2) self.speed = 5 self.heading = 0 self.heading_change = 0 self.vision_length = 200 # line liength self.vision_span = 22 # degrees - self.draw_sensors=True + self.draw_sensors = True # lets add 3 sensors as a start # 1 straight ahead @@ -44,8 +44,15 @@ class Car(pygame.sprite.Sprite): # we will give each of them a max lenght to # and we will eventually detect any line crossing the sensor and # retain the min value as a distance to collision input + self.center_sensor = None + self.left_sensor = None + self.right_sensor = None self.update_sensors() - + self.sensors = [self.left_sensor]#, self.center_sensor, self.right_sensor] + + self.reset_probes() + + def update_sensors(self): center = self.rect.center vc = angle_to_vector(self.heading) @@ -63,10 +70,9 @@ class Car(pygame.sprite.Sprite): old_center = self.rect.center self.rect.center = (self.speed * vec[0] + old_center[0], -self.speed * vec[1] + old_center[1]) self.update_sensors() - if self.draw_sensors: - pygame.draw.line(screen, (255,0,0), self.center_sensor[0], self.center_sensor[1]) - pygame.draw.line(screen, (0,255,0), self.left_sensor[0], self.left_sensor[1]) - pygame.draw.line(screen, (0,0,255), self.right_sensor[0], self.right_sensor[1]) + + + def update(self): # rotate @@ -78,9 +84,46 @@ class Car(pygame.sprite.Sprite): self.heading += self.heading_change self.heading = self.heading % 360 + super().update() + + + def show_features(self): + if self.draw_sensors: + pygame.draw.line(screen, (255,0,0), self.center_sensor[0], self.center_sensor[1]) + pygame.draw.line(screen, (0,255,0), self.left_sensor[0], self.left_sensor[1]) + pygame.draw.line(screen, (0,0,255), self.right_sensor[0], self.right_sensor[1]) + pygame.draw.circle(screen, (125,255,125), self.rect.center, 4, 2) + + + def reset_probes(self): + self.probes = [self.vision_length*2]#*3 + + + def probe_lines_proximity(self, lines): + self.reset_probes() + # print([x for x in zip(self.sensors, self.probes)]) + for l in lines : + for i,s in enumerate(self.sensors) : + ip = segments_intersection(s,l) + if ip is not None : + # print("ip", ip) + d = distance(ip, self.rect.center) + # print('d',d) + # print('p was', p, 'is' , min(p,d)) + self.probes[i] = min(self.probes[i],d) + # print() + else : + print('nothing found') + pass + # print(f"D to line {l} :", self.probes) + print(f"probes :", self.probes) + # print() + screen = pygame.display.set_mode((GX, GY), FLAGS) +screen.set_alpha(None) + all_cars = pygame.sprite.Group() # car = Car() # car.heading = 0 @@ -91,13 +134,44 @@ car2.heading_change = 3 car2.speed = 5 all_cars.add(car2) +ip = segments_intersection(car2.center_sensor, car2.left_sensor) +print(ip) +print(math.hypot(ip[0] - car2.rect.center[0], ip[1] - car2.rect.center[1])) + +# stress test +# for x in range(100): +# car = Car() +# car.heading=x +# car.heading_change = int(x)/30 +# car.speed = int(random.random()*6) +# all_cars.add(car) + +lines = [ + [ + ( + int(random.random()*GX), + int(random.random()*GY) + ),( + int(random.random()*GX), + int(random.random()*GY) + ) + ] +for x in range(1) +] + +print(lines) + clock = pygame.time.Clock() while True : screen.fill(CELL_COLOR) all_cars.update() all_cars.draw(screen) - pygame.draw.rect(screen, (125,255,125),pygame.Rect(car2.rect.center, (3,3))) + for c in all_cars : + c.show_features() + c.probe_lines_proximity(lines) + for line in lines : + pygame.draw.line(screen, (255,255,255), line[0], line[1]) pygame.display.flip() - clock.tick(10) \ No newline at end of file + clock.tick(2) \ No newline at end of file diff --git a/trigo.py b/trigo.py new file mode 100644 index 0000000..ccc8b7a --- /dev/null +++ b/trigo.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python +import math + +def angle_to_vector(angle): + angle=angle*math.pi/180 + return [math.cos(angle), math.sin(angle)] + + +def segments_intersection(line1, line2): + x1,y1 = line1[0] + x2,y2 = line1[1] + x3,y3 = line2[0] + x4,y4 = line2[1] + + xdiff = (x1 - x2, x3 - x4) + ydiff = (y1 - y2, y3 - y4) + + def det(a, b): + return a[0] * b[1] - a[1] * b[0] + + div = det(xdiff, ydiff) + if div == 0: + return None + + d = (det(*line1), det(*line2)) + x = det(d, xdiff) / div + y = det(d, ydiff) / div + + if min(x1,x2) <= x <= max(x1,x2) : + return x, y + return None + +def distance(point1, point2): + return math.hypot(point1[0] - point2[0], point1[1] - point2[1]) \ No newline at end of file