178 lines
5.2 KiB
Python
Executable File
178 lines
5.2 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, segments_intersection, distance
|
|
|
|
FLAGS= HWSURFACE | DOUBLEBUF #| FULLSCREEN
|
|
|
|
GX = 800
|
|
GY = 800
|
|
CELL_COLOR = (80,80,80)
|
|
CAR_SIZE=50
|
|
pygame.init()
|
|
IMG = pygame.image.load("car50.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 = (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
|
|
|
|
# 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.reset_probes()
|
|
|
|
|
|
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]]]
|
|
|
|
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]]]
|
|
|
|
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]]]
|
|
|
|
|
|
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()
|
|
|
|
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)
|
|
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()
|
|
|
|
|
|
|
|
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 = 270
|
|
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)
|
|
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) |