226 lines
6.6 KiB
Python
Executable File
226 lines
6.6 KiB
Python
Executable File
#!/usr/bin/env python
|
|
import math
|
|
import pygame
|
|
from pygame.locals import HWSURFACE, DOUBLEBUF
|
|
import random
|
|
from trigo import angle_to_vector, get_line_feats, segments_intersection, distance
|
|
|
|
FLAGS= HWSURFACE | DOUBLEBUF #| FULLSCREEN
|
|
|
|
GX = 1000
|
|
GY = 1000
|
|
CELL_COLOR = (80,80,80)
|
|
CAR_SIZE=25
|
|
VISION_LENGTH = 50
|
|
VISION_SPAN = 25 # degrees
|
|
THROTTLE_POWER = 3
|
|
pygame.init()
|
|
IMG = pygame.image.load("car25.png")#.convert()
|
|
|
|
|
|
|
|
|
|
class Car(pygame.sprite.Sprite):
|
|
def __init__(self):
|
|
pygame.sprite.Sprite.__init__(self)
|
|
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))
|
|
# self.original_image = self.image
|
|
|
|
self.image = self.original_image
|
|
|
|
self.rect = self.image.get_rect()
|
|
self.rect.center = (75, GY -50)
|
|
self.speed = 5
|
|
self.heading = 0
|
|
self.heading_change = 0
|
|
self.vision_length = VISION_LENGTH # line liength
|
|
self.vision_span = VISION_SPAN # degrees
|
|
self.draw_sensors = True
|
|
|
|
# lets add 3 sensors as a start
|
|
# 1 straight ahead
|
|
# 2 left 15°
|
|
# 3 right 15 °
|
|
# 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.probes = [self.vision_length] *3
|
|
|
|
self.heading_change = 0
|
|
self.throttle = 0
|
|
|
|
|
|
def update_sensors(self):
|
|
center = self.rect.center
|
|
vc = angle_to_vector(self.heading)
|
|
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, (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, (int(self.vision_length * vr[0] + center[0]), int(-self.vision_length * vr[1] + center[1]))]
|
|
|
|
|
|
def update_position(self):
|
|
vec = angle_to_vector(self.heading)
|
|
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()
|
|
|
|
|
|
|
|
def update(self):
|
|
# rotate
|
|
old_center = self.rect.center
|
|
self.image = pygame.transform.rotate(self.original_image, self.heading)
|
|
self.rect = self.image.get_rect()
|
|
self.rect.center = old_center
|
|
self.update_position()
|
|
|
|
if self.speed :
|
|
self.heading += self.heading_change / self.speed
|
|
self.heading = self.heading % 360
|
|
|
|
if self.throttle :
|
|
self.speed += THROTTLE_POWER
|
|
else :
|
|
self.speed -= THROTTLE_POWER
|
|
|
|
self.speed = max(0, self.speed)
|
|
|
|
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 probe_lines_proximity(self, lines):
|
|
# print(self.center_sensor, lines[0])
|
|
self.probes = [self.vision_length*2] *3
|
|
for idx,sensor in enumerate([self.left_sensor, self.center_sensor, self.right_sensor]) :
|
|
for line in lines :
|
|
ip = segments_intersection(sensor, line)
|
|
# print(ip)
|
|
if ip :
|
|
pygame.draw.circle(screen, (125,125,255), ip, 4, 2)
|
|
dist = int(distance(ip,self.rect.center))
|
|
self.probes[idx] = min(dist, self.probes[idx])
|
|
# else :
|
|
# self.probes[idx] = self.vision_length * 2
|
|
print(self.probes)
|
|
|
|
|
|
|
|
|
|
|
|
screen = pygame.display.set_mode((GX, GY), FLAGS)
|
|
screen.set_alpha(None)
|
|
|
|
all_cars = pygame.sprite.Group()
|
|
# car = Car()
|
|
# car.heading = 0
|
|
# all_cars.add(car)
|
|
car2 = Car()
|
|
car2.heading = 0
|
|
car2.heading_change = 30
|
|
car2.speed = 25
|
|
all_cars.add(car2)
|
|
|
|
ip = segments_intersection(car2.center_sensor, car2.left_sensor)
|
|
|
|
|
|
|
|
|
|
# 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(10)
|
|
# ]
|
|
|
|
path = [
|
|
(25, int(GY-25)),
|
|
(int(GX/2), int(GY-25)),
|
|
(int(GX/2 + 75), int(GY-150)),
|
|
(int(GX/2 + 150), int(GY-150)),
|
|
(int(GX -75), int(GY/2)),
|
|
(int(GX/2), int(GY/2 - 75)),
|
|
(int(GX/2), int(GY/2 - 150)),
|
|
(int(GX -50), int( GY/4 )),
|
|
(int(3*GX/4 - 50), int(50)),
|
|
(int(50), int(50)),
|
|
(int(100), int(GY/2)),
|
|
(25, int(GY-25)),
|
|
]
|
|
|
|
path2 = [
|
|
(100, int(GY-85)),
|
|
(int(GX/2 - 50 ), int(GY-85)),
|
|
(int(GX/2 + 50), int(GY-210)),
|
|
(int(GX/2 + 110), int(GY-210)),
|
|
(int(GX - 170), int(GY/2 + 30)),
|
|
(int(GX/2 - 60 ), int(GY/2 - 20)),
|
|
(int(GX/2 - 60), int(GY/2 - 200)),
|
|
(int(GX -170), int( GY/4 -20)),
|
|
(int(3*GX/4 - 100), int(120)),
|
|
(int(120), int(120)),
|
|
(int(175), int(GY/2)),
|
|
(100, int(GY-85)),
|
|
]
|
|
|
|
lines = [[path[i], path[i+1]] for i in range(len(path)-1)]
|
|
lines2 = [[path2[i], path2[i+1]] for i in range(len(path2)-1)]
|
|
|
|
lines = lines + lines2
|
|
print(lines)
|
|
|
|
|
|
clock = pygame.time.Clock()
|
|
while True :
|
|
screen.fill(CELL_COLOR)
|
|
all_cars.update()
|
|
all_cars.draw(screen)
|
|
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])
|
|
|
|
# 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(1) |