From 8b0a18a6cfbb7980bbbda8defdcef2675ff07a5f Mon Sep 17 00:00:00 2001 From: Ariari04 Date: Fri, 1 May 2026 16:01:32 +0600 Subject: [PATCH] corrected collision of tree --- resources/config2/navigation.json | 433 ++++++++++++++++--- resources/config2/navigation2.json | 666 ++++++++++++++++++++++++++++- src/Location.cpp | 19 +- src/navigation/PathFinder.cpp | 166 +++++++ src/navigation/PathFinder.h | 8 + 5 files changed, 1204 insertions(+), 88 deletions(-) diff --git a/resources/config2/navigation.json b/resources/config2/navigation.json index 985774f..877a74a 100644 --- a/resources/config2/navigation.json +++ b/resources/config2/navigation.json @@ -3,175 +3,472 @@ "agentRadius": 0.45, "floorY": 0.0, "objectPadding": 0.25, + "boundaryPadding": 0.35, "areas": [ { "name": "main_corridor", "available": true, "polygon": [ - [-2.2, 0.8], - [2.2, 0.8], - [2.2, -40.8], - [-2.2, -40.8] + [ + -2.2, + 0.8 + ], + [ + 2.2, + 0.8 + ], + [ + 2.2, + -40.8 + ], + [ + -2.2, + -40.8 + ] ] }, { "name": "left_door_05", "available": true, "polygon": [ - [-3.4, -4.2], - [-2.0, -4.2], - [-2.0, -5.8], - [-3.4, -5.8] + [ + -3.4, + -4.2 + ], + [ + -2.0, + -4.2 + ], + [ + -2.0, + -5.8 + ], + [ + -3.4, + -5.8 + ] ] }, { "name": "right_door_05", "available": true, "polygon": [ - [2.0, -4.2], - [3.4, -4.2], - [3.4, -5.8], - [2.0, -5.8] + [ + 2.0, + -4.2 + ], + [ + 3.4, + -4.2 + ], + [ + 3.4, + -5.8 + ], + [ + 2.0, + -5.8 + ] ] }, { "name": "left_room_05", "available": true, "polygon": [ - [-8.8, -1.0], - [-3.0, -1.0], - [-3.0, -9.0], - [-8.8, -9.0] + [ + -8.8, + -1.0 + ], + [ + -3.0, + -1.0 + ], + [ + -3.0, + -9.0 + ], + [ + -8.8, + -9.0 + ] ] }, { "name": "right_room_05", "available": true, "polygon": [ - [3.0, -1.0], - [8.8, -1.0], - [8.8, -9.0], - [3.0, -9.0] + [ + 3.0, + -1.0 + ], + [ + 8.8, + -1.0 + ], + [ + 8.8, + -9.0 + ], + [ + 3.0, + -9.0 + ] ] }, { "name": "left_door_15", "available": true, "polygon": [ - [-3.4, -14.2], - [-2.0, -14.2], - [-2.0, -15.8], - [-3.4, -15.8] + [ + -3.4, + -14.2 + ], + [ + -2.0, + -14.2 + ], + [ + -2.0, + -15.8 + ], + [ + -3.4, + -15.8 + ] ] }, { "name": "right_door_15", "available": true, "polygon": [ - [2.0, -14.2], - [3.4, -14.2], - [3.4, -15.8], - [2.0, -15.8] + [ + 2.0, + -14.2 + ], + [ + 3.4, + -14.2 + ], + [ + 3.4, + -15.8 + ], + [ + 2.0, + -15.8 + ] ] }, { "name": "left_room_15", "available": true, "polygon": [ - [-8.8, -11.0], - [-3.0, -11.0], - [-3.0, -19.0], - [-8.8, -19.0] + [ + -8.8, + -11.0 + ], + [ + -3.0, + -11.0 + ], + [ + -3.0, + -19.0 + ], + [ + -8.8, + -19.0 + ] ] }, { "name": "right_room_15", "available": true, "polygon": [ - [3.0, -11.0], - [8.8, -11.0], - [8.8, -19.0], - [3.0, -19.0] + [ + 3.0, + -11.0 + ], + [ + 8.8, + -11.0 + ], + [ + 8.8, + -19.0 + ], + [ + 3.0, + -19.0 + ] ] }, { "name": "left_door_25", "available": true, "polygon": [ - [-3.4, -24.2], - [-2.0, -24.2], - [-2.0, -25.8], - [-3.4, -25.8] + [ + -3.4, + -24.2 + ], + [ + -2.0, + -24.2 + ], + [ + -2.0, + -25.8 + ], + [ + -3.4, + -25.8 + ] ] }, { "name": "right_door_25", "available": true, "polygon": [ - [2.0, -24.2], - [3.4, -24.2], - [3.4, -25.8], - [2.0, -25.8] + [ + 2.0, + -24.2 + ], + [ + 3.4, + -24.2 + ], + [ + 3.4, + -25.8 + ], + [ + 2.0, + -25.8 + ] ] }, { "name": "left_room_25", "available": true, "polygon": [ - [-8.8, -21.0], - [-3.0, -21.0], - [-3.0, -29.0], - [-8.8, -29.0] + [ + -8.8, + -21.0 + ], + [ + -3.0, + -21.0 + ], + [ + -3.0, + -29.0 + ], + [ + -8.8, + -29.0 + ] ] }, { "name": "right_room_25", "available": true, "polygon": [ - [3.0, -21.0], - [8.8, -21.0], - [8.8, -29.0], - [3.0, -29.0] + [ + 3.0, + -21.0 + ], + [ + 8.8, + -21.0 + ], + [ + 8.8, + -29.0 + ], + [ + 3.0, + -29.0 + ] ] }, { "name": "left_door_35", "available": true, "polygon": [ - [-3.4, -34.2], - [-2.0, -34.2], - [-2.0, -35.8], - [-3.4, -35.8] + [ + -3.4, + -34.2 + ], + [ + -2.0, + -34.2 + ], + [ + -2.0, + -35.8 + ], + [ + -3.4, + -35.8 + ] ] }, { "name": "right_door_35", "available": true, "polygon": [ - [2.0, -34.2], - [3.4, -34.2], - [3.4, -35.8], - [2.0, -35.8] + [ + 2.0, + -34.2 + ], + [ + 3.4, + -34.2 + ], + [ + 3.4, + -35.8 + ], + [ + 2.0, + -35.8 + ] ] }, { "name": "left_room_35", "available": true, "polygon": [ - [-8.8, -31.0], - [-3.0, -31.0], - [-3.0, -39.0], - [-8.8, -39.0] + [ + -8.8, + -31.0 + ], + [ + -3.0, + -31.0 + ], + [ + -3.0, + -39.0 + ], + [ + -8.8, + -39.0 + ] ] }, { "name": "right_room_35", "available": true, "polygon": [ - [3.0, -31.0], - [8.8, -31.0], - [8.8, -39.0], - [3.0, -39.0] + [ + 3.0, + -31.0 + ], + [ + 8.8, + -31.0 + ], + [ + 8.8, + -39.0 + ], + [ + 3.0, + -39.0 + ] + ] + } + ], + "obstacles": [ + { + "name": "firebox", + "polygon": [ + [ + -2.49468, + -32.87888 + ], + [ + -2.05776, + -32.87888 + ], + [ + -2.05776, + -31.51618 + ], + [ + -2.49468, + -31.51618 + ] + ] + }, + { + "name": "bench", + "polygon": [ + [ + -2.54485, + -9.31999 + ], + [ + -2.51786, + -9.3557 + ], + [ + -2.48369, + -9.37177 + ], + [ + -2.45574, + -9.38357 + ], + [ + -2.17907, + -9.39566 + ], + [ + -1.80829, + -9.40749 + ], + [ + -1.7521, + -9.36413 + ], + [ + -1.66462, + -9.28728 + ], + [ + -1.65801, + -9.14512 + ], + [ + -1.65114, + -7.22169 + ], + [ + -1.65535, + -6.51944 + ], + [ + -1.72626, + -6.42306 + ], + [ + -1.76837, + -6.39498 + ], + [ + -2.47166, + -6.39746 + ], + [ + -2.5309, + -6.44135 + ], + [ + -2.53691, + -7.18464 + ] ] } ] diff --git a/resources/config2/navigation2.json b/resources/config2/navigation2.json index f503138..d0cefc6 100644 --- a/resources/config2/navigation2.json +++ b/resources/config2/navigation2.json @@ -8,10 +8,668 @@ "name": "main_corridor", "available": true, "polygon": [ - [-200, 200], - [200, 200], - [200, -200], - [-200, -200] + [ + -200, + 200 + ], + [ + 200, + 200 + ], + [ + 200, + -200 + ], + [ + -200, + -200 + ] + ] + } + ], + "obstacles": [ + { + "name": "door", + "polygon": [ + [ + -5.2, + 7.08001 + ], + [ + -5.19904, + 7.07025 + ], + [ + -5.19619, + 7.06087 + ], + [ + -5.19157, + 7.05223 + ], + [ + -5.18536, + 7.04465 + ], + [ + -5.17778, + 7.03843 + ], + [ + -5.16913, + 7.03381 + ], + [ + -5.15975, + 7.03097 + ], + [ + -5.15, + 7.03001 + ], + [ + -3.75, + 7.13001 + ], + [ + -3.625, + 7.43001 + ], + [ + -3.625, + 8.83001 + ], + [ + -3.75, + 9.13001 + ], + [ + -5.15, + 9.23001 + ], + [ + -5.15975, + 9.22905 + ], + [ + -5.16913, + 9.2262 + ], + [ + -5.17778, + 9.22158 + ], + [ + -5.18536, + 9.21536 + ], + [ + -5.19157, + 9.20778 + ], + [ + -5.19619, + 9.19914 + ], + [ + -5.19904, + 9.18976 + ], + [ + -5.2, + 9.18001 + ] + ] + }, + { + "name": "inai", + "polygon": [ + [ + -3.75, + -15.0 + ], + [ + 3.75, + -15.0 + ], + [ + 3.75, + 15.0 + ], + [ + -3.75, + 15.0 + ] + ] + }, + { + "name": "tree001", + "polygon": [ + [ + 10.45, + 12.0 + ], + [ + 10.38971, + 12.225 + ], + [ + 10.225, + 12.38971 + ], + [ + 10.0, + 12.45 + ], + [ + 9.775, + 12.38971 + ], + [ + 9.61029, + 12.225 + ], + [ + 9.55, + 12.0 + ], + [ + 9.61029, + 11.775 + ], + [ + 9.775, + 11.61029 + ], + [ + 10.0, + 11.55 + ], + [ + 10.225, + 11.61029 + ], + [ + 10.38971, + 11.775 + ] + ] + }, + { + "name": "tree002", + "polygon": [ + [ + -11.55, + 19.0 + ], + [ + -11.61029, + 19.225 + ], + [ + -11.775, + 19.38971 + ], + [ + -12.0, + 19.45 + ], + [ + -12.225, + 19.38971 + ], + [ + -12.38971, + 19.225 + ], + [ + -12.45, + 19.0 + ], + [ + -12.38971, + 18.775 + ], + [ + -12.225, + 18.61029 + ], + [ + -12.0, + 18.55 + ], + [ + -11.775, + 18.61029 + ], + [ + -11.61029, + 18.775 + ] + ] + }, + { + "name": "tree003", + "polygon": [ + [ + -11.55, + 8.0 + ], + [ + -11.61029, + 8.225 + ], + [ + -11.775, + 8.38971 + ], + [ + -12.0, + 8.45 + ], + [ + -12.225, + 8.38971 + ], + [ + -12.38971, + 8.225 + ], + [ + -12.45, + 8.0 + ], + [ + -12.38971, + 7.775 + ], + [ + -12.225, + 7.61029 + ], + [ + -12.0, + 7.55 + ], + [ + -11.775, + 7.61029 + ], + [ + -11.61029, + 7.775 + ] + ] + }, + { + "name": "tree004", + "polygon": [ + [ + -11.55, + 0.0 + ], + [ + -11.61029, + 0.225 + ], + [ + -11.775, + 0.38971 + ], + [ + -12.0, + 0.45 + ], + [ + -12.225, + 0.38971 + ], + [ + -12.38971, + 0.225 + ], + [ + -12.45, + 0.0 + ], + [ + -12.38971, + -0.225 + ], + [ + -12.225, + -0.38971 + ], + [ + -12.0, + -0.45 + ], + [ + -11.775, + -0.38971 + ], + [ + -11.61029, + -0.225 + ] + ] + }, + { + "name": "tree005", + "polygon": [ + [ + -11.55, + -8.0 + ], + [ + -11.61029, + -7.775 + ], + [ + -11.775, + -7.61029 + ], + [ + -12.0, + -7.55 + ], + [ + -12.225, + -7.61029 + ], + [ + -12.38971, + -7.775 + ], + [ + -12.45, + -8.0 + ], + [ + -12.38971, + -8.225 + ], + [ + -12.225, + -8.38971 + ], + [ + -12.0, + -8.45 + ], + [ + -11.775, + -8.38971 + ], + [ + -11.61029, + -8.225 + ] + ] + }, + { + "name": "tree006", + "polygon": [ + [ + 8.94915, + -2.59884 + ], + [ + 8.88886, + -2.37384 + ], + [ + 8.72415, + -2.20913 + ], + [ + 8.49915, + -2.14884 + ], + [ + 8.27415, + -2.20913 + ], + [ + 8.10944, + -2.37384 + ], + [ + 8.04915, + -2.59884 + ], + [ + 8.10944, + -2.82384 + ], + [ + 8.27415, + -2.98855 + ], + [ + 8.49915, + -3.04884 + ], + [ + 8.72415, + -2.98855 + ], + [ + 8.88886, + -2.82384 + ] + ] + }, + { + "name": "tree007", + "polygon": [ + [ + 15.0436, + 5.3401 + ], + [ + 14.98331, + 5.5651 + ], + [ + 14.8186, + 5.72981 + ], + [ + 14.5936, + 5.7901 + ], + [ + 14.3686, + 5.72981 + ], + [ + 14.20389, + 5.5651 + ], + [ + 14.1436, + 5.3401 + ], + [ + 14.20389, + 5.1151 + ], + [ + 14.3686, + 4.95039 + ], + [ + 14.5936, + 4.8901 + ], + [ + 14.8186, + 4.95039 + ], + [ + 14.98331, + 5.1151 + ] + ] + }, + { + "name": "tree008", + "polygon": [ + [ + 24.3795, + 9.00583 + ], + [ + 24.31921, + 9.23083 + ], + [ + 24.1545, + 9.39554 + ], + [ + 23.9295, + 9.45583 + ], + [ + 23.7045, + 9.39554 + ], + [ + 23.53979, + 9.23083 + ], + [ + 23.4795, + 9.00583 + ], + [ + 23.53979, + 8.78083 + ], + [ + 23.7045, + 8.61612 + ], + [ + 23.9295, + 8.55583 + ], + [ + 24.1545, + 8.61612 + ], + [ + 24.31921, + 8.78083 + ] + ] + }, + { + "name": "tree009", + "polygon": [ + [ + 30.2628, + -1.45278 + ], + [ + 30.20251, + -1.22778 + ], + [ + 30.0378, + -1.06307 + ], + [ + 29.8128, + -1.00278 + ], + [ + 29.5878, + -1.06307 + ], + [ + 29.42309, + -1.22778 + ], + [ + 29.3628, + -1.45278 + ], + [ + 29.42309, + -1.67778 + ], + [ + 29.5878, + -1.84249 + ], + [ + 29.8128, + -1.90278 + ], + [ + 30.0378, + -1.84249 + ], + [ + 30.20251, + -1.67778 + ] + ] + }, + { + "name": "tree010", + "polygon": [ + [ + 33.6271, + 14.609 + ], + [ + 33.56681, + 14.834 + ], + [ + 33.4021, + 14.99871 + ], + [ + 33.1771, + 15.059 + ], + [ + 32.9521, + 14.99871 + ], + [ + 32.78739, + 14.834 + ], + [ + 32.7271, + 14.609 + ], + [ + 32.78739, + 14.384 + ], + [ + 32.9521, + 14.21929 + ], + [ + 33.1771, + 14.159 + ], + [ + 33.4021, + 14.21929 + ], + [ + 33.56681, + 14.384 + ] ] } ] diff --git a/src/Location.cpp b/src/Location.cpp index 0b4ed07..af04760 100644 --- a/src/Location.cpp +++ b/src/Location.cpp @@ -197,22 +197,9 @@ namespace ZL void Location::setupNavigation(const std::string& navigationJsonPath) { - std::vector obstacles; - obstacles.reserve(gameObjects.size() + interactiveObjects.size()); - - for (const auto& item : gameObjects) { - const LoadedGameObject& gameObj = item.second; - obstacles.push_back({ &gameObj.mesh.data, Eigen::Vector3f::Zero() }); - } - - for (const InteractiveObject& intObj : interactiveObjects) { - if (!intObj.isActive) { - continue; - } - obstacles.push_back({ &intObj.mesh.data, intObj.position }); - } - - navigation.build(obstacles, navigationJsonPath, CONST_ZIP_FILE); + // Static navigation blockers are defined in the navigation JSON as polygons. + // NPC + player are handled as dynamic obstacles, so they are intentionally NOT in JSON. + navigation.build({}, navigationJsonPath, CONST_ZIP_FILE); #ifdef SHOW_PATH buildDebugNavMeshes(); diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 20ab1db..51c6e9a 100644 --- a/src/navigation/PathFinder.cpp +++ b/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& polygon) + { + if (polygon.size() < 2) { + return (std::numeric_limits::max)(); + } + + float best = (std::numeric_limits::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& 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(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::max)(); + } + + const float sampleOffset = (std::max)(cellSize * 0.25f, 0.05f); + float best = (std::numeric_limits::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(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(walkable.size())) { + continue; + } + if (!walkable[static_cast(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(idx)] = 0; + break; + } + if (padding > 0.0f && + distancePointToPolygonEdges(center, obstacle.polygon) <= padding) { + walkable[static_cast(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) { diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index 8cb11e7..60134f0 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -14,6 +14,11 @@ public: Eigen::Vector3f offset = Eigen::Vector3f::Zero(); }; + struct ObstaclePolygon { + std::string name; + std::vector 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 obstacles; + std::vector obstaclePolygons; std::vector walkable; std::vector 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;