From 0342d17b905edd7808225629757983836f746b12 Mon Sep 17 00:00:00 2001 From: leo Date: Wed, 23 Nov 2022 22:00:21 +0100 Subject: [PATCH] area ray casting --- laby.py | 37 +++++++++++++++++++++++++++++++++++-- robot.py | 10 ++++++---- simulateur.py | 8 ++++++-- 3 files changed, 47 insertions(+), 8 deletions(-) diff --git a/laby.py b/laby.py index 0074c38..8fcc386 100644 --- a/laby.py +++ b/laby.py @@ -1,6 +1,6 @@ import pygame -from math import dist +from math import dist, cos, sin, atan2, pi class Laby: L1=1.50 @@ -57,6 +57,13 @@ class Laby: closest = inters[index_min] return closest + def get_closest_wall_dist(self, center, dir, lgt, scatter): + inters = [self.get_inter_area(center, dir, lgt, scatter, w) for w in self.walls] + inters = list(filter(None, inters)) + if len(inters) == 0: + return None + return self.closest_point(center, inters) + def get_inter(self,l1,l2): (x1,y1),(x2,y2) = l1 (x3,y3),(x4,y4) = l2 @@ -73,4 +80,30 @@ class Laby: return None pint = (x1+t*(x2-x1),y1+t*(y2-y1)) return pint - \ No newline at end of file + + def get_inter_area(self, center, dir, lgt, scatter, l): + endL = (center[0]+lgt*sin(dir-scatter), center[1]+lgt*cos(dir-scatter)) + endR = (center[0]+lgt*sin(dir+scatter), center[1]+lgt*cos(dir+scatter)) + interL = self.get_inter((center,endL), l) + interR = self.get_inter((center,endR), l) + interE = self.get_inter((endL, endR), l) + inters = [interL, interR, interE] + inters = list(filter(None, inters)) + line_points_in_area = list(filter(lambda p: self.point_in_area(center, dir, lgt, scatter, p), l)) + inters += line_points_in_area + if len(inters) == 0: + return None + return self.closest_point(center, inters) + + def point_in_area(self, center, dir, lgt, scatter, p): + pdir = atan2(p[1]-center[1], p[0]-center[0]) + if abs((dir+pi/2)%(2*pi)-pdir)%(2*pi)>scatter: + return False + if dist(center, p) > lgt: + return False + return True + + def closest_point(self, pc, ps): + dists = [dist(pc, p) for p in ps] + index_min = min(range(len(dists)), key=dists.__getitem__) + return ps[index_min] diff --git a/robot.py b/robot.py index 6316c6e..35b5dc9 100644 --- a/robot.py +++ b/robot.py @@ -1,4 +1,4 @@ -from math import cos, sin, pi, atan2, tan, dist +from math import cos, sin, pi, atan2, tan, dist, radians class Robot: # position @@ -38,6 +38,7 @@ class Robot: # rayon de mesure des capteurs ultrason ultr_sense_max = 4 ultr_sense_rays = (pi/2, -pi/2) + ultr_sense_scatter = radians(6) # TODO: check dists = [None] * len(ultr_sense_rays) tr = [0]*len(ultr_sense_rays) ci = [0]*len(tr) @@ -101,9 +102,10 @@ class Robot: def update_ultr_sense(self): for ind,r in enumerate(self.ultr_sense_rays): - tr = (self.pos[0]+self.ultr_sense_max*sin(r+self.rot), self.pos[1]+self.ultr_sense_max*cos(r+self.rot)) - self.tr[ind] = tr - self.ci[ind] = self.laby.get_closest_wall_dist((self.pos, tr)) + #tr = (self.pos[0]+self.ultr_sense_max*sin(r+self.rot), self.pos[1]+self.ultr_sense_max*cos(r+self.rot)) + #self.tr[ind] = tr + self.ci[ind] = self.laby.get_closest_wall_dist(self.pos, r+self.rot, self.ultr_sense_max, self.ultr_sense_scatter) + #self.ci[ind] = self.laby.get_closest_wall_dist((self.pos, tr)) if self.ci[ind]: self.dists[ind] = dist(self.pos, self.ci[ind]) else: diff --git a/simulateur.py b/simulateur.py index bc43d01..5c01bc9 100644 --- a/simulateur.py +++ b/simulateur.py @@ -73,8 +73,12 @@ while True: # light pygame.draw.polygon(screen, (255,255,0), ([100*a for a in r.light_pos],[100*(a+b) for a,b in zip(r.light_pos,(-0.1,0.1))],[100*(a+b) for a,b in zip(r.light_pos,(-0.1,-0.1))])) # ultrasonic sensors - for usrs in r.tr: - pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in usrs]) + for usrs in r.ultr_sense_rays: + endl = (r.pos[0]+r.ultr_sense_max*sin(r.rot+usrs+r.ultr_sense_scatter), r.pos[1]+r.ultr_sense_max*cos(r.rot+usrs+r.ultr_sense_scatter)) + endr = (r.pos[0]+r.ultr_sense_max*sin(r.rot+usrs-r.ultr_sense_scatter), r.pos[1]+r.ultr_sense_max*cos(r.rot+usrs-r.ultr_sense_scatter)) + pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in endl]) + pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in endr]) + pygame.draw.line(screen, (255,0,0), [p*100 for p in endl], [p*100 for p in endr]) for c in r.ci: if c: pygame.draw.circle(screen, (255,0,0), [p*100 for p in c], 3)