diff --git a/main.py b/main.py index 45a2a19..2d560c5 100755 --- a/main.py +++ b/main.py @@ -105,9 +105,10 @@ class Car(pygame.sprite.Sprite): for l in lines : for i,s in enumerate(self.sensors) : ip = segments_intersection(s,l) + print(s, l) if ip is not None : # print("ip", ip) - d = distance(ip, self.rect.center) + 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) diff --git a/trigo.py b/trigo.py index ccc8b7a..366adc8 100644 --- a/trigo.py +++ b/trigo.py @@ -7,13 +7,8 @@ def angle_to_vector(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) + 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]) def det(a, b): return a[0] * b[1] - a[1] * b[0] @@ -26,9 +21,15 @@ def segments_intersection(line1, line2): x = det(d, xdiff) / div y = det(d, ydiff) / div - if min(x1,x2) <= x <= max(x1,x2) : + 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 None + return x, y def distance(point1, point2): return math.hypot(point1[0] - point2[0], point1[1] - point2[1]) \ No newline at end of file