From e18bd1aae67664d1340ab684411d2050db3a41e8 Mon Sep 17 00:00:00 2001 From: alexandre Date: Fri, 11 Oct 2019 10:28:16 +0200 Subject: [PATCH] works for one sensor --- main.py | 71 +++++++++++++++++++++++++++++++++++--------------------- trigo.py | 7 ++++++ 2 files changed, 52 insertions(+), 26 deletions(-) diff --git a/main.py b/main.py index 9a2809b..961b76b 100755 --- a/main.py +++ b/main.py @@ -3,7 +3,7 @@ import math import pygame from pygame.locals import HWSURFACE, DOUBLEBUF import random -from trigo import angle_to_vector, segments_intersection, distance +from trigo import angle_to_vector, get_line_feats, segments_intersection, distance FLAGS= HWSURFACE | DOUBLEBUF #| FULLSCREEN @@ -56,13 +56,13 @@ class Car(pygame.sprite.Sprite): def update_sensors(self): center = self.rect.center vc = angle_to_vector(self.heading) - self.center_sensor = [center, [self.vision_length * vc[0] + center[0], -self.vision_length * vc[1] + center[1]]] + self.center_sensor = [center, (int(self.vision_length * vc[0] + center[0]), int(-self.vision_length * vc[1] + center[1]))] vl = angle_to_vector(self.heading+self.vision_span) - self.left_sensor = [center, [self.vision_length * vl[0] + center[0], -self.vision_length * vl[1] + center[1]]] + self.left_sensor = [center, (int(self.vision_length * vl[0] + center[0]), int(-self.vision_length * vl[1] + center[1]))] vr = angle_to_vector(self.heading-self.vision_span) - self.right_sensor = [center, [self.vision_length * vr[0] + center[0], -self.vision_length * vr[1] + center[1]]] + self.right_sensor = [center, (int(self.vision_length * vr[0] + center[0]), int(-self.vision_length * vr[1] + center[1]))] def update_position(self): @@ -90,8 +90,8 @@ class Car(pygame.sprite.Sprite): 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.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) @@ -100,24 +100,31 @@ class Car(pygame.sprite.Sprite): 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) - print(s, l) - if ip : - # print("ip", ip) - d = distance(ip, self.left_sensor[1]) - # 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(self.center_sensor, lines[0]) + ip = segments_intersection(self.center_sensor, lines[0]) + # print(ip) + if ip : + pygame.draw.circle(screen, (125,125,255), ip, 4, 2) + print(distance(ip,self.rect.center)) + + # 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) + # print(s, l) + # if ip : + # # print("ip", ip) + # d = distance(ip, self.left_sensor[1]) + # # 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() @@ -136,7 +143,10 @@ 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 @@ -159,6 +169,10 @@ lines = [ ] for x in range(1) ] +lines = [ + [(int(GX/2)+100, int(GY/2+225)), (int(GX/2)+100, int(GY/2-225))] + +] print(lines) @@ -174,5 +188,10 @@ while True : for line in lines : pygame.draw.line(screen, (255,255,255), line[0], line[1]) + + # point = (int(GX/2), int(GY/2+25)) + # print(distance(point, car2.rect.center)) + # pygame.draw.circle(screen, (125,255,125), point, 4, 2) + pygame.display.flip() - clock.tick(10) \ No newline at end of file + clock.tick(3) \ No newline at end of file diff --git a/trigo.py b/trigo.py index dbb9f99..67122e4 100644 --- a/trigo.py +++ b/trigo.py @@ -10,6 +10,8 @@ def get_line_feats(point1, point2): x1,y1 = point1 x2,y2 = point2 + # if x1 == x2 : + # x1=x1+1 a = (y1-y2)/(x1-x2) b = y2 - a * x2 return a,b @@ -19,6 +21,11 @@ def get_line_feats(point1, point2): def segments_intersection(line1, line2): p1,p2 = line1 p3,p4 = line2 + if p1[0] == p2[0] : + p1 = (p1[0] + 1, p1[1]) + if p3[0] == p4[0] : + p3 = (p3[0] + 1, p3[1]) + a1,b1 = get_line_feats(p1,p2) a2,b2 = get_line_feats(p3,p4)