diff --git a/resources/config2/navigation2.json b/resources/config2/navigation2.json index 2bb0ee8..44cecaa 100644 --- a/resources/config2/navigation2.json +++ b/resources/config2/navigation2.json @@ -113,6 +113,17 @@ [1, -5.5], [-0.2, -5.5] ] + }, + { + "name": "main_corridor", + "available": true, + "forbidden": true, + "polygon": [ + [-15, 15], + [15, 15], + [15, 2.2], + [-15, 2.2] + ] } ] } diff --git a/src/Location.cpp b/src/Location.cpp index 8076a1a..97008fe 100644 --- a/src/Location.cpp +++ b/src/Location.cpp @@ -400,6 +400,7 @@ void Location::setup() } //#ifdef SHOW_PATH buildDebugNavMeshes(); + buildDebugForbiddenMeshes(); //#endif auto planner = [this](const Eigen::Vector3f& start, const Eigen::Vector3f& end) { @@ -444,6 +445,35 @@ void Location::setup() renderer.shaderManager.PopShader(); renderer.SetMatrix(); } + + void Location::buildDebugForbiddenMeshes() + { + debugForbiddenMeshes.clear(); + const auto& areas = navigation.getAreas(); + float y = navigation.getFloorY() + 0.2f; + Eigen::Vector3f blue(0.0f, 0.0f, 1.0f); + + for (const auto& area : areas) { + if (area.forbidden && area.polygon.size() >= 3) { + VertexRenderStruct mesh; + mesh.data = CreatePolygonFloor(area.polygon, y, blue); + mesh.RefreshVBO(); + debugForbiddenMeshes.push_back(std::move(mesh)); + } + } + } + + void Location::drawDebugForbidden() + { + if (debugForbiddenMeshes.empty()) return; + renderer.shaderManager.PushShader("defaultColor"); + renderer.SetMatrix(); + for (const auto& mesh : debugForbiddenMeshes) { + renderer.DrawVertexRenderStruct(mesh); + } + renderer.shaderManager.PopShader(); + renderer.SetMatrix(); + } //#endif @@ -609,6 +639,7 @@ void Location::setup() //#ifdef SHOW_PATH drawDebugNavigation(); + drawDebugForbidden(); //#endif renderer.PopMatrix(); @@ -785,6 +816,7 @@ void Location::setup() CheckGlError(__FILE__, __LINE__); drawDebugNavigation(); + drawDebugForbidden(); renderer.PopMatrix(); renderer.PopProjectionMatrix(); diff --git a/src/Location.h b/src/Location.h index bcb85f4..21e1273 100644 --- a/src/Location.h +++ b/src/Location.h @@ -72,6 +72,10 @@ namespace ZL std::vector debugNavMeshes; void buildDebugNavMeshes(); void drawDebugNavigation(); + + std::vector debugForbiddenMeshes; + void buildDebugForbiddenMeshes(); + void drawDebugForbidden(); //#endif bool rightMouseDown = false; int lastMouseX = 0; diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 3131435..acd8ecb 100644 --- a/src/navigation/PathFinder.cpp +++ b/src/navigation/PathFinder.cpp @@ -246,6 +246,7 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi NavigationArea area; area.name = item.value("name", ""); area.available = item.value("available", true); + area.forbidden = item.value("forbidden", false); area.polygon = readPolygon(item); if (area.name.empty()) { @@ -314,10 +315,44 @@ void PathFinder::resetGridBounds() gridDepth = static_cast(std::ceil((maxZ - minZ) / cellSize)); } -void PathFinder::rebuildWalkableGrid() +/*void PathFinder::rebuildWalkableGrid() { walkable.assign(static_cast(gridWidth * gridDepth), 0); markAvailableAreasWalkable(); + markObstacleMeshesBlocked(); +}*/ +void PathFinder::rebuildWalkableGrid() +{ + walkable.assign(static_cast(gridWidth * gridDepth), 0); + + for (const NavigationArea& area : areas) { + if (area.available && !area.forbidden) { + 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); + if (pointInPolygon(center.x(), center.z(), area.polygon)) { + walkable[static_cast(indexOf(cell))] = 1; + } + } + } + } + } + + for (const NavigationArea& area : areas) { + if (area.forbidden) { + 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); + if (pointInPolygon(center.x(), center.z(), area.polygon)) { + walkable[static_cast(indexOf(cell))] = 0; + } + } + } + } + } + markObstacleMeshesBlocked(); } diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index 1ac9b9a..5b2acb0 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -22,6 +22,7 @@ public: struct NavigationArea { std::string name; bool available = true; + bool forbidden = false; std::vector polygon; };