Merge pull request #202 from inverted-ai/large_map_utilities

Large map utilities

examples/area_drive/regions.py

@@ -27,11 +27,6 @@ def __init__(
         self.agents_in_fov = []
 
     def pre_drive(self):
-        # agents_in_fov = []
-        # for car in self.npcs:
-        #     agents_in_fov.extend(filter(lambda x: x not in self.npcs, car.fov_agents))
-        # agents_in_fov = list(set(agents_in_fov)) #Remove duplicates
-
         agent_states = []
         agent_attributes = []
         recurrent_states = []
@@ -149,7 +144,7 @@ def __init__(
             convertors=self.convertors
         )
         self.region = Region(boundary=self.boundary_buffer, cfg=cfg)
-        self.particles_boundary = []
+        self.particles_buffer = []
 
     def subdivide(self):
         parent = self.boundary
@@ -224,44 +219,47 @@ def subdivide(self):
         self.leaf = False
 
         for particle in self.particles:
-            is_inserted = []
-            is_inserted.append(self.northWest.insert(particle))
-            is_inserted.append(self.northEast.insert(particle))
-            is_inserted.append(self.southWest.insert(particle))
-            is_inserted.append(self.southEast.insert(particle))
-            is_inserted = any(is_inserted)
+            is_inserted = self.insert_particle_in_leaf_nodes(particle)
+        for particle in self.particles_buffer:
+            is_inserted = self.insert_particle_in_leaf_nodes(particle)
         self.particles = []
+        self.particles_buffer = []
 
+    def insert_particle_in_leaf_nodes(self,particle):
+        is_inserted = False
+        is_inserted = self.northWest.insert(particle,is_inserted) or is_inserted
+        is_inserted = self.northEast.insert(particle,is_inserted) or is_inserted
+        is_inserted = self.southWest.insert(particle,is_inserted) or is_inserted
+        is_inserted = self.southEast.insert(particle,is_inserted) or is_inserted
+
+        return is_inserted
 
-    def insert(self, particle):
+    def insert(self, particle, is_particle_placed=False):
         is_in_boundary = self.boundary.containsParticle(particle)
-        is_in_buffer = self.boundary_buffer.containsParticle(particle) and not is_in_boundary
+        is_in_buffer = self.boundary_buffer.containsParticle(particle)
 
-        if not is_in_boundary and not is_in_buffer:
+        if (not is_in_boundary) and (not is_in_buffer):
             return False
 
-        if (len(self.particles) + len(self.particles_boundary)) < self.capacity and self.leaf:
-            if is_in_boundary:
+        if (len(self.particles) + len(self.particles_buffer)) < self.capacity and self.leaf:
+            if is_in_boundary and not is_particle_placed:
                 self.particles.append(particle)
                 self.region.insert(particle)
                 particle.region = self.region
                 return True
 
             else: # Particle is within the buffer region of this leaf node
-                self.particles_boundary.append(particle)
+                self.particles_buffer.append(particle)
+                self.region.insert_fov_agent(particle)
                 return False
 
         else:
             if self.leaf:
                 self.subdivide()
 
-            is_inserted = []
-            is_inserted.append(self.northWest.insert(particle))
-            is_inserted.append(self.northEast.insert(particle))
-            is_inserted.append(self.southWest.insert(particle))
-            is_inserted.append(self.southEast.insert(particle))
+            is_inserted = self.insert_particle_in_leaf_nodes(particle)
 
-            return any(is_inserted)
+            return is_inserted
 
     def get_regions(self):
         if self.leaf:
@@ -270,6 +268,13 @@ def get_regions(self):
             return self.northWest.get_regions() + self.northEast.get_regions() + \
                 self.southWest.get_regions() + self.southEast.get_regions()
 
+    def get_leaf_nodes(self):
+        if self.leaf:
+            return [self]
+        else:
+            return self.northWest.get_leaf_nodes() + self.northEast.get_leaf_nodes() + \
+                self.southWest.get_leaf_nodes() + self.southEast.get_leaf_nodes()
+
     def queryRange(self, query_range):
         particlesInRange = []
         if self.leaf:
@@ -288,8 +293,11 @@ def Show(self, screen):
         self.boundary.color = self.color
         self.boundary.lineThickness = self.lineThickness
         self.boundary.Draw(screen)
+
         if not self.leaf:
             self.northWest.Show(screen)
             self.northEast.Show(screen)
             self.southWest.Show(screen)
             self.southEast.Show(screen)
+
+

examples/area_drive/utils.py

@@ -16,37 +16,43 @@ def get_pygame_convertors(x_min, x_max, y_min, y_max, H, W):
     def convert_to_pygame_coords(x, y):
         x_range = x_max - x_min
         y_range = y_max - y_min
-        pygame_x = int((x - x_min) * W / x_range)
-        pygame_y = int((y - y_min) * H / y_range)
+        pygame_x = round((x - x_min) * W / x_range)
+        pygame_y = round((y - y_min) * H / y_range)
         return (pygame_x, pygame_y)
 
     def convert_to_pygame_scales(w, h):
         x_range = x_max - x_min
         y_range = y_max - y_min
-        pygame_w = int(w * W / x_range)
-        pygame_h = int(h * H / y_range)
+        pygame_w = round(w * W / x_range)
+        pygame_h = round(h * H / y_range)
         return (pygame_w, pygame_h)
 
     return convert_to_pygame_coords, convert_to_pygame_scales
 
 class Rotations:
     @ staticmethod
     def rotationX(angle):
-        return [[1, 0, 0],
-                [0, math.cos(angle), -math.sin(angle)],
-                [0, math.sin(angle), math.cos(angle)]]
+        return [
+            [1, 0, 0],
+            [0, math.cos(angle), -math.sin(angle)],
+            [0, math.sin(angle), math.cos(angle)]
+        ]
 
     @ staticmethod
     def rotationY(angle):
-        return [[math.cos(angle), 0, -math.sin(angle)],
-                [0, 1, 0],
-                [math.sin(angle), 0, math.cos(angle)]]
+        return [
+            [math.cos(angle), 0, -math.sin(angle)],
+            [0, 1, 0],
+            [math.sin(angle), 0, math.cos(angle)]
+        ]
 
     @ staticmethod
     def rotationZ(angle):
-        return [[math.cos(angle), -math.sin(angle), 0],
-                [math.sin(angle), math.cos(angle), 0],
-                [0, 0, 1]]
+        return [
+            [math.cos(angle), -math.sin(angle), 0],
+            [math.sin(angle), math.cos(angle), 0],
+            [0, 0, 1]
+        ]
 
 class Rectangle:
     def __init__(self, position: Tuple[float, float], scale: Tuple[float, float], convertors=None):
@@ -61,7 +67,7 @@ def containsParticle(self, particle):
         x, y = particle.position.x, particle.position.y
         bx, by = self.position
         w, h = self.scale
-        if x > bx and x < bx+w and y > by and y < by+h:
+        if x >= bx and x <= bx+w and y >= by and y <= by+h:
             return True
         else:
             return False
@@ -77,7 +83,6 @@ def intersects(self, other):
             return False
 
     def Draw(self, screen):
-
         if self.convertors:
             x, y = self.convertors[0](*self.position)
             w, h = self.convertors[1](*self.scale)

examples/large_map_example.py

@@ -26,15 +26,13 @@ def main(args):
         location = args.location, 
         agent_density = args.agent_density, 
         scaling_factor = 1.0,
-        width = args.width,
-        height = args.height,
+        width = int(args.width/2),
+        height = int(args.height/2),
         map_center = map_center
     )
 
     print(f"Set up simulation.")    
-    map_width = max([abs(pt.x) for pt in location_info_response.bounding_polygon])
-    map_height = max([abs(pt.y) for pt in location_info_response.bounding_polygon]) 
-    map_extent = max([map_width,map_height])
+    map_extent = max([args.width,args.height])
     cfg = AreaDriverConfig(
         location = args.location,
         area_center = map_center,
@@ -128,13 +126,13 @@ def main(args):
     argparser.add_argument(
         '--width',
         type=int,
-        help=f"Width of the area to initialize.",
+        help=f"Full width of the area to initialize.",
         default=100
     )
     argparser.add_argument(
         '--height',
         type=int,
-        help=f"Height of the area to initialize",
+        help=f"Full height of the area to initialize",
         default=100
     )
     argparser.add_argument(