#!/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)