area ray casting

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
-            
+        
+    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]

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))
+            #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.tr[ind] = tr
-            self.ci[ind] = self.laby.get_closest_wall_dist((self.pos, 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:

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:
+    for usrs in r.ultr_sense_rays:
-        pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in usrs])
+        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)