From 04222b61488185bc49fbcc09085361e9f7e3df3d Mon Sep 17 00:00:00 2001
From: alexandre <alexandre@micouleau.info>
Date: Thu, 10 Oct 2019 17:47:35 +0200
Subject: [PATCH] home made func

---
 main.py  |  6 +++---
 trigo.py | 43 ++++++++++++++++++++++++-------------------
 2 files changed, 27 insertions(+), 22 deletions(-)

diff --git a/main.py b/main.py
index 2d560c5..9a2809b 100755
--- a/main.py
+++ b/main.py
@@ -106,7 +106,7 @@ class Car(pygame.sprite.Sprite):
             for i,s in enumerate(self.sensors) :
                     ip = segments_intersection(s,l)
                     print(s, l)
-                    if ip is not None :
+                    if ip :
                         # print("ip", ip)
                         d = distance(ip, self.left_sensor[1])
                         # print('d',d)
@@ -137,7 +137,7 @@ 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]))
+# print(math.hypot(ip[0] - car2.rect.center[0], ip[1] - car2.rect.center[1]))
 
 # stress test
 # for x in range(100):
@@ -175,4 +175,4 @@ while True :
     for line in lines :
         pygame.draw.line(screen, (255,255,255), line[0], line[1])
     pygame.display.flip()
-    clock.tick(2)
\ No newline at end of file
+    clock.tick(10)
\ No newline at end of file
diff --git a/trigo.py b/trigo.py
index 366adc8..dbb9f99 100644
--- a/trigo.py
+++ b/trigo.py
@@ -6,30 +6,35 @@ def angle_to_vector(angle):
     return [math.cos(angle), math.sin(angle)]
 
 
+def get_line_feats(point1, point2):
+    x1,y1 = point1
+    x2,y2 = point2
+
+    a = (y1-y2)/(x1-x2)
+    b = y2 - a * x2
+    return a,b
+
+
+
 def segments_intersection(line1, line2):
-    xdiff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])
-    ydiff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])
+    p1,p2 = line1
+    p3,p4 = line2
 
-    def det(a, b):
-        return a[0] * b[1] - a[1] * b[0]
+    a1,b1 = get_line_feats(p1,p2)
+    a2,b2 = get_line_feats(p3,p4)
 
-    div = det(xdiff, ydiff)
-    if div == 0:
-       return None
+    if a1==a2 :
+        return None # parrallel lines
+    
+    x = (b2-b1)/(a1-a2)
+
+    if min(p1[0], p2[0]) <= x <= max (p1[0], p2[0]) and min(p3[0], p4[0]) <= x <= max (p3[0], p4[0]) :
+        y = a1 * x + b1
+        return x,y
+    else : 
+        return None # intersect is outside segments
 
-    d = (det(*line1), det(*line2))
-    x = det(d, xdiff) / div
-    y = det(d, ydiff) / div
 
-    if (
-        min(line1[0][0],line1[1][0]) <= x <= max(line1[0][0],line1[1][0]) 
-        and min(line2[0][0],line2[1][0]) <= x <= max(line2[0][0],line2[1][0]) 
-        and min(line1[0][1],line1[1][1]) <= y <= max(line1[0][1],line1[1][1])
-        and min(line2[0][1],line2[1][1]) <= y <= max(line2[0][1],line2[1][1])  
-        
-        ):
-        return x, y
-    return x, y
 
 def distance(point1, point2):
     return math.hypot(point1[0] - point2[0], point1[1] - point2[1])
\ No newline at end of file