allow quadtree to work w/ overlapping positions

src/swarmsim/sensors/BinaryFOVSensor.py

@@ -111,16 +111,23 @@ def checkForLOSCollisions(self, world: RectangularWorld) -> None:
 
         self.time_since_last_sensing = 0
 
-        if not world.quad:
-            self.determineState(False, None, world)
-            return
-
-        sensor_origin = self.agent.getPosition()
+        sensor_origin = self.agent.pos
 
-        # use world.quad that tracks agent positions to retrieve the agents within the minimal rectangle that contains the FOV sector
-        quadpoints = [point.data for point in world.quad.within_bb(quads.BoundingBox(*self.getAARectContainingSector(world)))]
+        if world.quad:
+            # use world.quad that tracks agent positions to retrieve the agents within the minimal rectangle that contains the FOV sector
+            quadpoints = [point.data for point in world.quad.within_bb(
+                quads.BoundingBox(*self.getAARectContainingSector(world)))]
+            agents = np.array([agent for data in quadpoints for agent in data], dtype=object)
+        else:
+            agents = np.array(world.population, dtype=object)
         # filter agents to those within the sensing radius
-        bag = [agent for agent in quadpoints if self.withinRadiusExclusiveFast(sensor_origin, agent.getPosition(), self.r)]
+        if agents.size:
+            positions = np.array([agent.pos for agent in agents])
+            distances = np.linalg.norm(positions - sensor_origin, axis=1)
+            is_close = distances < self.r
+            bag = agents[is_close]
+        else:
+            bag = []
 
         # get left and right whiskers
         e_left, e_right = self.getSectorVectors()
@@ -201,15 +208,6 @@ def checkForLOSCollisions(self, world: RectangularWorld) -> None:
         self.determineState(False, None, world)
         return
 
-        # OLD CODE (below is unreachable)
-        if not consideration_set:
-            self.determineState(False, None, world)
-            return
-        # consideration_set.sort()
-        # print(consideration_set)
-        _score, val = consideration_set.pop(0)
-        self.determineState(True, val, world)
-
     # get the smallest rectangle that contains the sensor fov sector
     def getAARectContainingSector(self, world: RectangularWorld):
         angle: float = self.agent.angle + self.bias  # global sensor angle

src/swarmsim/world/RectangularWorld.py

@@ -170,28 +170,39 @@ def update_quadtree(self):
             self.quad = None
             return
 
+        positions = np.array([agent.pos for agent in self.population])
+        mins = np.min(positions, axis=0)
+        maxs = np.max(positions, axis=0)
+        wh = maxs - mins
+        quadsize = np.ceil(wh) + 4
+        middle = (mins + maxs) / 2
+        quadcenter = np.ceil(middle)
+
         # procedure to find the bounds of the quad
-        def minMax(arr):
-            minimum = arr[0]
-            maximum = arr[0]
-            for e in arr:
-                if e < minimum:
-                    minimum = e
-                if maximum < e:
-                    maximum = e
-            return minimum, maximum
-        xMin, xMax = minMax([agent.pos[0] for agent in self.population])
-        yMin, yMax = minMax([agent.pos[1] for agent in self.population])
-        middle = (np.trunc((xMin + xMax) / 2), np.trunc((yMin + yMax) / 2))
+        # def minMax(arr):
+        #     minimum = arr[0]
+        #     maximum = arr[0]
+        #     for e in arr:
+        #         if e < minimum:
+        #             minimum = e
+        #         if maximum < e:
+        #             maximum = e
+        #     return minimum, maximum
+        # xMin, xMax = minMax([agent.pos[0] for agent in self.population])
+        # yMin, yMax = minMax([agent.pos[1] for agent in self.population])
+        # middle = (np.trunc((xMin + xMax) / 2), np.trunc((yMin + yMax) / 2))
 
         # create quad that nicely contains the current population
-        newQuad = quads.QuadTree(middle, np.ceil(xMax - xMin) + 4, np.ceil(yMax - yMin) + 4)
+        qt = quads.QuadTree(quadcenter.tolist(), *quadsize)
 
         # add the agents to the quad
         for agent in self.population:
-            newQuad.insert(point=agent.pos.tolist(), data=agent)
-        self.quad = newQuad
-
+            pos = agent.pos.tolist()
+            if (existing := qt.find(pos)):
+                existing.data.append(agent)
+            else:
+                qt.insert(point=pos, data=[agent])
+        self.quad = qt
 
     def setup_objects(self, objects):
         StaticObject, StaticObjectConfig = store.agent_types['StaticObject']