From 88ee3732b64c175b9af59c471e29025f2593f637 Mon Sep 17 00:00:00 2001
From: yggdrasil75 <cblackburn7557@gmail.com>
Date: Tue, 3 Feb 2026 05:44:48 -0500
Subject: [PATCH] another minor feature

---
 util/grid/grid3eigen.hpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/util/grid/grid3eigen.hpp b/util/grid/grid3eigen.hpp
index a64c75d..f9c86d2 100644
--- a/util/grid/grid3eigen.hpp
+++ b/util/grid/grid3eigen.hpp
@@ -690,7 +690,7 @@ public:
         return removeNode(root_.get());
     }
 
-    std::vector<std::shared_ptr<NodeData>> findInRadius(const PointType& center, float radius) const {
+    std::vector<std::shared_ptr<NodeData>> findInRadius(const PointType& center, float radius, int objectid = -1) const {
         std::vector<std::shared_ptr<NodeData>> results;
         if (!root_) return results;
         
@@ -714,7 +714,7 @@ public:
                     if (!pointData->active) continue;
                     
                     float pointDistSq = (pointData->position - center).squaredNorm();
-                    if (pointDistSq <= radiusSq) {
+                    if (pointDistSq <= radiusSq && (pointData->objectId == objectid || objectid == -1)) {
                         results.emplace_back(pointData);
                     }
                 }