From 2bf85ccfdff8347bec2b9584cafd93e523b368bf Mon Sep 17 00:00:00 2001 From: kenblu24 Date: Wed, 17 Sep 2025 23:35:33 -0400 Subject: [PATCH 1/2] allow quadtree to work w/ overlapping positions --- src/swarmsim/sensors/BinaryFOVSensor.py | 32 +++++++++--------- src/swarmsim/world/RectangularWorld.py | 43 ++++++++++++++++--------- 2 files changed, 42 insertions(+), 33 deletions(-) diff --git a/src/swarmsim/sensors/BinaryFOVSensor.py b/src/swarmsim/sensors/BinaryFOVSensor.py index 2d2abbeb..cf299660 100644 --- a/src/swarmsim/sensors/BinaryFOVSensor.py +++ b/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.pos - sensor_origin = self.agent.getPosition() - - # 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 not 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 diff --git a/src/swarmsim/world/RectangularWorld.py b/src/swarmsim/world/RectangularWorld.py index f724c32b..30f1a611 100644 --- a/src/swarmsim/world/RectangularWorld.py +++ b/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'] From 60ed6f97fcf38aa759d063e9af348d571f0495b9 Mon Sep 17 00:00:00 2001 From: kenblu24 Date: Wed, 17 Sep 2025 23:38:54 -0400 Subject: [PATCH 2/2] fix quads not being used --- src/swarmsim/sensors/BinaryFOVSensor.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/swarmsim/sensors/BinaryFOVSensor.py b/src/swarmsim/sensors/BinaryFOVSensor.py index cf299660..6c97f184 100644 --- a/src/swarmsim/sensors/BinaryFOVSensor.py +++ b/src/swarmsim/sensors/BinaryFOVSensor.py @@ -113,7 +113,7 @@ def checkForLOSCollisions(self, world: RectangularWorld) -> None: sensor_origin = self.agent.pos - if not world.quad: + 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)))]