segment intersections does not work, algorithm tbt

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,7 +44,14 @@ 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
@@ -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)
+    clock.tick(2)

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])