corrected collision of tree

src/navigation/PathFinder.cpp

@@ -38,6 +38,52 @@ namespace {
 
         return polygon;
     }
+
+    float distancePointToSegment2D(const Eigen::Vector2f& p,
+                                   const Eigen::Vector2f& a,
+                                   const Eigen::Vector2f& b)
+    {
+        const Eigen::Vector2f ab = b - a;
+        const float abLenSq = ab.squaredNorm();
+        if (abLenSq <= 1e-8f) {
+            return (p - a).norm();
+        }
+        float t = (p - a).dot(ab) / abLenSq;
+        t = (std::max)(0.0f, (std::min)(1.0f, t));
+        const Eigen::Vector2f proj = a + t * ab;
+        return (p - proj).norm();
+    }
+
+    Eigen::Vector2f closestPointOnSegment2D(const Eigen::Vector2f& p,
+                                            const Eigen::Vector2f& a,
+                                            const Eigen::Vector2f& b)
+    {
+        const Eigen::Vector2f ab = b - a;
+        const float abLenSq = ab.squaredNorm();
+        if (abLenSq <= 1e-8f) {
+            return a;
+        }
+
+        float t = (p - a).dot(ab) / abLenSq;
+        t = (std::max)(0.0f, (std::min)(1.0f, t));
+        return a + t * ab;
+    }
+
+    float distancePointToPolygonEdges(const Eigen::Vector2f& p,
+                                      const std::vector<Eigen::Vector2f>& polygon)
+    {
+        if (polygon.size() < 2) {
+            return (std::numeric_limits<float>::max)();
+        }
+
+        float best = (std::numeric_limits<float>::max)();
+        for (size_t i = 0; i < polygon.size(); ++i) {
+            const Eigen::Vector2f& a = polygon[i];
+            const Eigen::Vector2f& b = polygon[(i + 1) % polygon.size()];
+            best = (std::min)(best, distancePointToSegment2D(p, a, b));
+        }
+        return best;
+    }
 }
 
 void PathFinder::build(const std::vector<ObstacleMesh>& obstacleMeshes,
@@ -369,7 +415,9 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi
     agentRadius = 0.45f;
     floorY = 0.0f;
     objectPadding = 0.25f;
+    boundaryPadding = 0.0f;
     areas.clear();
+    obstaclePolygons.clear();
 
     try {
         std::string content;
@@ -390,6 +438,7 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi
         agentRadius = root.value("agentRadius", agentRadius);
         floorY = root.value("floorY", floorY);
         objectPadding = root.value("objectPadding", objectPadding);
+        boundaryPadding = root.value("boundaryPadding", boundaryPadding);
 
         if (!root.contains("areas") || !root["areas"].is_array()) {
             std::cerr << "[nav] Navigation config has no 'areas' array: " << configPath << "\n";
@@ -413,6 +462,18 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi
 
             areas.push_back(area);
         }
+
+        if (root.contains("obstacles") && root["obstacles"].is_array()) {
+            for (const auto& item : root["obstacles"]) {
+                ObstaclePolygon obstacle;
+                obstacle.name = item.value("name", "");
+                obstacle.polygon = readPolygon(item);
+                if (obstacle.polygon.size() < 3) {
+                    continue;
+                }
+                obstaclePolygons.push_back(std::move(obstacle));
+            }
+        }
     } catch (const std::exception& e) {
         std::cerr << "[nav] Failed to load config '" << configPath << "': " << e.what() << "\n";
     }
@@ -420,6 +481,7 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi
     cellSize = (std::max)(cellSize, 0.1f);
     agentRadius = (std::max)(agentRadius, 0.0f);
     objectPadding = (std::max)(objectPadding, 0.0f);
+    boundaryPadding = (std::max)(boundaryPadding, 0.0f);
 }
 
 void PathFinder::resetGridBounds()
@@ -458,6 +520,12 @@ void PathFinder::resetGridBounds()
         }
     }
 
+    for (const ObstaclePolygon& obstacle : obstaclePolygons) {
+        for (const Eigen::Vector2f& point : obstacle.polygon) {
+            includePoint(point.x(), point.y());
+        }
+    }
+
     const float padding = cellSize * 2.0f + agentRadius + objectPadding;
     minX -= padding;
     minZ -= padding;
@@ -473,20 +541,75 @@ void PathFinder::rebuildWalkableGrid()
     walkable.assign(static_cast<size_t>(gridWidth * gridDepth), 0);
     markAvailableAreasWalkable();
     markObstacleMeshesBlocked();
+    markObstaclePolygonsBlocked();
 }
 
 void PathFinder::markAvailableAreasWalkable()
 {
+    const auto insideAvailableArea = [this](const Eigen::Vector2f& point) {
+        for (const NavigationArea& area : areas) {
+            if (!area.available) {
+                continue;
+            }
+            if (pointInPolygon(point.x(), point.y(), area.polygon)) {
+                return true;
+            }
+        }
+        return false;
+    };
+
+    const auto externalBoundaryDistance = [this, &insideAvailableArea](const Eigen::Vector2f& point) {
+        if (boundaryPadding <= 0.0f) {
+            return (std::numeric_limits<float>::max)();
+        }
+
+        const float sampleOffset = (std::max)(cellSize * 0.25f, 0.05f);
+        float best = (std::numeric_limits<float>::max)();
+
+        for (const NavigationArea& area : areas) {
+            if (!area.available || area.polygon.size() < 2) {
+                continue;
+            }
+
+            for (size_t i = 0; i < area.polygon.size(); ++i) {
+                const Eigen::Vector2f& a = area.polygon[i];
+                const Eigen::Vector2f& b = area.polygon[(i + 1) % area.polygon.size()];
+                const Eigen::Vector2f edge = b - a;
+                if (edge.squaredNorm() <= 1e-8f) {
+                    continue;
+                }
+
+                const Eigen::Vector2f closest = closestPointOnSegment2D(point, a, b);
+                Eigen::Vector2f normal(-edge.y(), edge.x());
+                normal.normalize();
+
+                const bool sideAInside = insideAvailableArea(closest + normal * sampleOffset);
+                const bool sideBInside = insideAvailableArea(closest - normal * sampleOffset);
+                if (sideAInside && sideBInside) {
+                    continue;
+                }
+
+                best = (std::min)(best, distancePointToSegment2D(point, a, b));
+            }
+        }
+
+        return best;
+    };
+
     for (int z = 0; z < gridDepth; ++z) {
         for (int x = 0; x < gridWidth; ++x) {
             const Cell cell{ x, z };
             const Eigen::Vector3f center = cellCenter(cell);
+            const Eigen::Vector2f point(center.x(), center.z());
 
             for (const NavigationArea& area : areas) {
                 if (!area.available) {
                     continue;
                 }
                 if (pointInPolygon(center.x(), center.z(), area.polygon)) {
+                    if (boundaryPadding > 0.0f && externalBoundaryDistance(point) < boundaryPadding) {
+                        continue;
+                    }
                     walkable[static_cast<size_t>(indexOf(cell))] = 1;
                     break;
                 }
@@ -495,8 +618,51 @@ void PathFinder::markAvailableAreasWalkable()
     }
 }
 
+void PathFinder::markObstaclePolygonsBlocked()
+{
+    if (obstaclePolygons.empty()) {
+        return;
+    }
+
+    const float padding = agentRadius + objectPadding;
+
+    for (int z = 0; z < gridDepth; ++z) {
+        for (int x = 0; x < gridWidth; ++x) {
+            const Cell cell{ x, z };
+            const int idx = indexOf(cell);
+            if (idx < 0 || idx >= static_cast<int>(walkable.size())) {
+                continue;
+            }
+            if (!walkable[static_cast<size_t>(idx)]) {
+                continue;
+            }
+
+            const Eigen::Vector3f center3 = cellCenter(cell);
+            const Eigen::Vector2f center(center3.x(), center3.z());
+
+            for (const ObstaclePolygon& obstacle : obstaclePolygons) {
+                if (pointInPolygon(center.x(), center.y(), obstacle.polygon)) {
+                    walkable[static_cast<size_t>(idx)] = 0;
+                    break;
+                }
+                if (padding > 0.0f &&
+                    distancePointToPolygonEdges(center, obstacle.polygon) <= padding) {
+                    walkable[static_cast<size_t>(idx)] = 0;
+                    break;
+                }
+            }
+        }
+    }
+}
+
 void PathFinder::markObstacleMeshesBlocked()
 {
+    if (!obstaclePolygons.empty()) {
+        // Prefer JSON-defined obstacle polygons over mesh-derived blockers.
+        // Mesh blockers tend to over-block (e.g. tree crowns) and are harder to author/tune.
+        return;
+    }
+
     const float padding = agentRadius + objectPadding;
 
     for (const ObstacleMesh& obstacle : obstacles) {

src/navigation/PathFinder.h

@@ -14,6 +14,11 @@ public:
         Eigen::Vector3f offset = Eigen::Vector3f::Zero();
     };
 
+    struct ObstaclePolygon {
+        std::string name;
+        std::vector<Eigen::Vector2f> polygon;
+    };
+
 	struct DynamicObstacle {
 		Eigen::Vector3f position = Eigen::Vector3f::Zero();
 		float radius = 0.0f;
@@ -53,6 +58,7 @@ private:
     float agentRadius = 0.45f;
     float floorY = 0.0f;
     float objectPadding = 0.25f;
+    float boundaryPadding = 0.0f;
 
     float minX = 0.0f;
     float minZ = 0.0f;
@@ -63,6 +69,7 @@ private:
     std::string loadedConfigPath;
     std::string loadedZipPath;
     std::vector<ObstacleMesh> obstacles;
+    std::vector<ObstaclePolygon> obstaclePolygons;
     std::vector<unsigned char> walkable;
     std::vector<NavigationArea> areas;
 
@@ -71,6 +78,7 @@ private:
     void rebuildWalkableGrid();
     void markAvailableAreasWalkable();
     void markObstacleMeshesBlocked();
+    void markObstaclePolygonsBlocked();
 
     bool worldToCell(const Eigen::Vector3f& point, Cell& out) const;
     Eigen::Vector3f cellCenter(const Cell& cell) const;