works for one sensor

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)
+    clock.tick(3)

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)