diff --git a/proj-android/app/jni/src/CMakeLists.txt b/proj-android/app/jni/src/CMakeLists.txt index 9812c39..d4d1663 100644 --- a/proj-android/app/jni/src/CMakeLists.txt +++ b/proj-android/app/jni/src/CMakeLists.txt @@ -47,6 +47,7 @@ add_library(main SHARED ../../../../src/utils/Perlin.cpp ../../../../src/utils/TaskManager.cpp ../../../../src/utils/Utils.cpp + ../../../../src/navigation/PathFinder.cpp ../../../../src/planet/PlanetData.cpp ../../../../src/planet/PlanetObject.cpp ../../../../src/planet/StoneObject.cpp diff --git a/proj-web/CMakeLists.txt b/proj-web/CMakeLists.txt index 2d7f707..e1fd3a7 100644 --- a/proj-web/CMakeLists.txt +++ b/proj-web/CMakeLists.txt @@ -118,6 +118,8 @@ set(SOURCES ../src/GameConstants.cpp ../src/ScriptEngine.h ../src/ScriptEngine.cpp + ../src/navigation/PathFinder.h + ../src/navigation/PathFinder.cpp ) add_executable(space-game001 ${SOURCES}) diff --git a/proj-windows/CMakeLists.txt b/proj-windows/CMakeLists.txt index b89dca9..c2c602f 100644 --- a/proj-windows/CMakeLists.txt +++ b/proj-windows/CMakeLists.txt @@ -69,10 +69,12 @@ add_executable(space-game001 ../src/MenuManager.cpp # ../src/Space.h # ../src/Space.cpp - ../src/GameConstants.h - ../src/GameConstants.cpp - ../src/ScriptEngine.h - ../src/ScriptEngine.cpp + ../src/GameConstants.h + ../src/GameConstants.cpp + ../src/ScriptEngine.h + ../src/ScriptEngine.cpp + ../src/navigation/PathFinder.h + ../src/navigation/PathFinder.cpp ../src/items/GameObjectLoader.h ../src/items/GameObjectLoader.cpp ../src/items/Item.h @@ -197,4 +199,4 @@ foreach(resdir IN LISTS RUNTIME_RESOURCE_DIRS) "${CMAKE_SOURCE_DIR}/../${resdir}" "${CMAKE_BINARY_DIR}/${resdir}" ) -endforeach() \ No newline at end of file +endforeach() diff --git a/resources/config2/navigation.json b/resources/config2/navigation.json new file mode 100644 index 0000000..985774f --- /dev/null +++ b/resources/config2/navigation.json @@ -0,0 +1,178 @@ +{ + "cellSize": 0.4, + "agentRadius": 0.45, + "floorY": 0.0, + "objectPadding": 0.25, + "areas": [ + { + "name": "main_corridor", + "available": true, + "polygon": [ + [-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] + ] + }, + { + "name": "right_door_05", + "available": true, + "polygon": [ + [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] + ] + }, + { + "name": "right_room_05", + "available": true, + "polygon": [ + [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] + ] + }, + { + "name": "right_door_15", + "available": true, + "polygon": [ + [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] + ] + }, + { + "name": "right_room_15", + "available": true, + "polygon": [ + [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] + ] + }, + { + "name": "right_door_25", + "available": true, + "polygon": [ + [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] + ] + }, + { + "name": "right_room_25", + "available": true, + "polygon": [ + [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] + ] + }, + { + "name": "right_door_35", + "available": true, + "polygon": [ + [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] + ] + }, + { + "name": "right_room_35", + "available": true, + "polygon": [ + [3.0, -31.0], + [8.8, -31.0], + [8.8, -39.0], + [3.0, -39.0] + ] + } + ] +} diff --git a/src/Character.cpp b/src/Character.cpp index a512c3a..7bd9808 100644 --- a/src/Character.cpp +++ b/src/Character.cpp @@ -22,8 +22,46 @@ void Character::loadAnimation(AnimationState state, const std::string& filename, void Character::setTarget(const Eigen::Vector3f& target, std::function onArrived) { - walkTarget = target; + Eigen::Vector3f normalizedTarget(target.x(), 0.f, target.z()); + + const bool sameRequestedTarget = + (normalizedTarget - requestedWalkTarget).norm() <= TARGET_REPLAN_THRESHOLD; + const bool alreadyMovingToTarget = + !pathWaypoints.empty() || (walkTarget - normalizedTarget).norm() <= TARGET_REPLAN_THRESHOLD; + const bool stoppedAfterFailedPath = + pathWaypoints.empty() && (walkTarget - position).norm() <= TARGET_REPLAN_THRESHOLD; + + if (!onArrived && sameRequestedTarget && (alreadyMovingToTarget || stoppedAfterFailedPath)) { + return; + } + + requestedWalkTarget = normalizedTarget; onArrivedCallback = std::move(onArrived); + + if (pathPlanner) { + pathWaypoints = pathPlanner(position, normalizedTarget); + currentWaypointIndex = 0; + + if (!pathWaypoints.empty()) { + for (Eigen::Vector3f& waypoint : pathWaypoints) { + waypoint.y() = 0.f; + } + walkTarget = pathWaypoints.back(); + return; + } + + walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z()); + onArrivedCallback = nullptr; + return; + } + + pathWaypoints.clear(); + currentWaypointIndex = 0; + walkTarget = normalizedTarget; +} + +void Character::setPathPlanner(PathPlanner planner) { + pathPlanner = std::move(planner); } AnimationState Character::resolveActiveState() const { @@ -35,7 +73,12 @@ AnimationState Character::resolveActiveState() const { void Character::update(int64_t deltaMs) { //std::cout << "update called with deltaMs: " << deltaMs << std::endl; // Move toward walk target on the XZ plane - Eigen::Vector3f toTarget = walkTarget - position; + Eigen::Vector3f activeTarget = walkTarget; + if (!pathWaypoints.empty() && currentWaypointIndex < pathWaypoints.size()) { + activeTarget = pathWaypoints[currentWaypointIndex]; + } + + Eigen::Vector3f toTarget = activeTarget - position; toTarget.y() = 0.f; float dist = toTarget.norm(); @@ -43,7 +86,7 @@ void Character::update(int64_t deltaMs) { Eigen::Vector3f dir = toTarget / dist; float moveAmount = walkSpeed * static_cast(deltaMs) / 1000.f; if (moveAmount >= dist) { - position = walkTarget; + position = activeTarget; position.y() = 0.f; } else { position += dir * moveAmount; @@ -60,7 +103,17 @@ void Character::update(int64_t deltaMs) { { currentState = AnimationState::STAND; } - if (onArrivedCallback) { + + const bool hasNextWaypoint = !pathWaypoints.empty() && currentWaypointIndex + 1 < pathWaypoints.size(); + if (hasNextWaypoint) { + ++currentWaypointIndex; + } + else { + pathWaypoints.clear(); + currentWaypointIndex = 0; + } + + if (!hasNextWaypoint && onArrivedCallback) { auto cb = std::move(onArrivedCallback); onArrivedCallback = nullptr; cb(); diff --git a/src/Character.h b/src/Character.h index 58c44f8..89be71d 100644 --- a/src/Character.h +++ b/src/Character.h @@ -8,6 +8,7 @@ #include #include #include +#include namespace ZL { @@ -22,6 +23,10 @@ enum class AnimationState { class Character { public: + using PathPlanner = std::function( + const Eigen::Vector3f& start, + const Eigen::Vector3f& end)>; + void loadAnimation(AnimationState state, const std::string& filename, const std::string& zipFile = ""); // void setTexture(std::shared_ptr texture); void setTexture(std::shared_ptr texture) { @@ -32,6 +37,7 @@ public: // From Python you can chain further commands (walk, wait, trigger others). void setTarget(const Eigen::Vector3f& target, std::function onArrived = nullptr); + void setPathPlanner(PathPlanner planner); void draw(Renderer& renderer); // Public: read by Game for camera tracking and ray-cast origin @@ -70,13 +76,18 @@ private: }; std::map animations; - std::shared_ptr texture; + std::shared_ptr texture; Eigen::Vector3f walkTarget = Eigen::Vector3f(0.f, 0.f, 0.f); + Eigen::Vector3f requestedWalkTarget = Eigen::Vector3f(0.f, 0.f, 0.f); + std::vector pathWaypoints; + size_t currentWaypointIndex = 0; + PathPlanner pathPlanner; float targetFacingAngle = 0.0f; std::function onArrivedCallback; static constexpr float WALK_THRESHOLD = 0.05f; + static constexpr float TARGET_REPLAN_THRESHOLD = 0.25f; // Returns the animation state to actually play/draw, falling back to IDLE // if the requested state has no loaded animation. diff --git a/src/Game.cpp b/src/Game.cpp index 8565cf2..45717d7 100644 --- a/src/Game.cpp +++ b/src/Game.cpp @@ -233,6 +233,8 @@ namespace ZL npcs.push_back(std::move(npc02)); std::cout << "Load resurces step 12" << std::endl; + + setupNavigation(); loadingCompleted = true; @@ -279,6 +281,40 @@ namespace ZL } } + void Game::setupNavigation() + { + 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, "resources/config2/navigation.json", CONST_ZIP_FILE); + + auto planner = [this](const Eigen::Vector3f& start, const Eigen::Vector3f& end) { + return navigation.findPath(start, end); + }; + + if (player) { + player->setPathPlanner(planner); + } + + for (auto& npc : npcs) { + if (npc) { + npc->setPathPlanner(planner); + } + } + } + InteractiveObject* Game::raycastInteractiveObjects(const Eigen::Vector3f& rayOrigin, const Eigen::Vector3f& rayDir) { if (interactiveObjects.empty()) { std::cout << "[RAYCAST] No interactive objects to check" << std::endl; @@ -897,5 +933,10 @@ namespace ZL return dialogueSystem.getFlag(flag); } + bool Game::setNavigationAreaAvailable(const std::string& areaName, bool available) + { + return navigation.setAreaAvailable(areaName, available); + } + } // namespace ZL diff --git a/src/Game.h b/src/Game.h index 81535ed..0f588f2 100644 --- a/src/Game.h +++ b/src/Game.h @@ -22,6 +22,7 @@ #include "ScriptEngine.h" #include #include "dialogue/DialogueSystem.h" +#include "navigation/PathFinder.h" #include namespace ZL { @@ -40,6 +41,7 @@ namespace ZL { bool requestDialogueStart(const std::string& dialogueId); void setDialogueFlag(const std::string& flag, int value); int getDialogueFlag(const std::string& flag) const; + bool setNavigationAreaAvailable(const std::string& areaName, bool available); Renderer renderer; TaskManager taskManager; @@ -84,6 +86,7 @@ namespace ZL { // Public access for ScriptEngine MenuManager menuManager; ScriptEngine scriptEngine; + PathFinder navigation; private: bool rightMouseDown = false; @@ -98,6 +101,7 @@ namespace ZL { void drawUI(); void drawGame(); void drawLoading(); + void setupNavigation(); void handleDown(int64_t fingerId, int mx, int my); void handleUp(int64_t fingerId, int mx, int my); void handleMotion(int64_t fingerId, int mx, int my); @@ -124,4 +128,4 @@ namespace ZL { }; -} // namespace ZL \ No newline at end of file +} // namespace ZL diff --git a/src/ScriptEngine.cpp b/src/ScriptEngine.cpp index 8d49e3b..80d4ff2 100644 --- a/src/ScriptEngine.cpp +++ b/src/ScriptEngine.cpp @@ -155,6 +155,14 @@ namespace ZL { return game->getDialogueFlag(flag); }); + api.set_function("set_navigation_area_available", + [game](const std::string& areaName, bool available) { + if (!game->setNavigationAreaAvailable(areaName, available)) { + std::cerr << "[script] set_navigation_area_available: area not found: " + << areaName << "\n"; + } + }); + // receive_npc_gift(npc_index) api.set_function("receive_npc_gift", [game](int npcIndex) { std::cout << "[script] receive_npc_gift: npc index " << npcIndex << std::endl; diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp new file mode 100644 index 0000000..3131435 --- /dev/null +++ b/src/navigation/PathFinder.cpp @@ -0,0 +1,518 @@ +#include "navigation/PathFinder.h" +#include "external/nlohmann/json.hpp" +#include "utils/Utils.h" +#include +#include +#include +#include +#include + +namespace ZL { + +using json = nlohmann::json; + +namespace { + const float INF_COST = (std::numeric_limits::max)(); + + float distanceCells(const PathFinder::Cell& a, const PathFinder::Cell& b) + { + const float dx = static_cast(a.x - b.x); + const float dz = static_cast(a.z - b.z); + return std::sqrt(dx * dx + dz * dz); + } + + std::vector readPolygon(const json& item) + { + std::vector polygon; + if (!item.contains("polygon") || !item["polygon"].is_array()) { + return polygon; + } + + for (const auto& point : item["polygon"]) { + if (point.is_array() && point.size() >= 2) { + polygon.emplace_back(point[0].get(), point[1].get()); + } else if (point.is_object()) { + polygon.emplace_back(point.value("x", 0.0f), point.value("z", 0.0f)); + } + } + + return polygon; + } +} + +void PathFinder::build(const std::vector& obstacleMeshes, + const std::string& configPath, + const std::string& zipPath) +{ + ready = false; + loadedConfigPath = configPath; + loadedZipPath = zipPath; + obstacles = obstacleMeshes; + + loadConfig(configPath, zipPath); + resetGridBounds(); + + if (gridWidth <= 0 || gridDepth <= 0) { + std::cerr << "[nav] Could not build grid: no navigation polygons\n"; + walkable.clear(); + return; + } + + rebuildWalkableGrid(); + ready = true; + + std::cout << "[nav] Polygon grid built: " << gridWidth << "x" << gridDepth + << ", cell=" << cellSize << ", areas=" << areas.size() << "\n"; +} + +std::vector PathFinder::findPath(const Eigen::Vector3f& start, + const Eigen::Vector3f& end) const +{ + if (!ready || walkable.empty()) { + return {}; + } + + Cell startCell; + Cell endCell; + if (!findNearestWalkableCell(start, startCell) || + !findNearestWalkableCell(end, endCell)) { + return {}; + } + + if (startCell.x == endCell.x && startCell.z == endCell.z) { + return { cellCenter(endCell) }; + } + + struct QueueNode { + int index = 0; + float priority = 0.0f; + + bool operator<(const QueueNode& other) const + { + return priority > other.priority; + } + }; + + const int cellCount = gridWidth * gridDepth; + std::vector cost(static_cast(cellCount), INF_COST); + std::vector cameFrom(static_cast(cellCount), -1); + std::priority_queue open; + + const int startIndex = indexOf(startCell); + const int endIndex = indexOf(endCell); + + cost[static_cast(startIndex)] = 0.0f; + open.push({ startIndex, 0.0f }); + + static const int offsets[8][2] = { + { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 }, + { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 } + }; + + while (!open.empty()) { + const QueueNode current = open.top(); + open.pop(); + + if (current.index == endIndex) { + break; + } + + const Cell currentCell{ current.index % gridWidth, current.index / gridWidth }; + + for (const auto& offset : offsets) { + Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] }; + if (!isCellWalkable(next)) { + continue; + } + + const bool diagonal = offset[0] != 0 && offset[1] != 0; + if (diagonal) { + Cell horizontal{ currentCell.x + offset[0], currentCell.z }; + Cell vertical{ currentCell.x, currentCell.z + offset[1] }; + if (!isCellWalkable(horizontal) || !isCellWalkable(vertical)) { + continue; + } + } + + const int nextIndex = indexOf(next); + const float stepCost = diagonal ? 1.41421356f : 1.0f; + const float newCost = cost[static_cast(current.index)] + stepCost; + if (newCost >= cost[static_cast(nextIndex)]) { + continue; + } + + cost[static_cast(nextIndex)] = newCost; + cameFrom[static_cast(nextIndex)] = current.index; + const float priority = newCost + distanceCells(next, endCell); + open.push({ nextIndex, priority }); + } + } + + if (cameFrom[static_cast(endIndex)] == -1) { + return {}; + } + + std::vector cells; + for (int current = endIndex; current != -1; current = cameFrom[static_cast(current)]) { + cells.push_back({ current % gridWidth, current / gridWidth }); + if (current == startIndex) { + break; + } + } + std::reverse(cells.begin(), cells.end()); + cells = smoothCells(cells); + + std::vector path; + path.reserve(cells.size()); + for (const Cell& cell : cells) { + path.push_back(cellCenter(cell)); + } + + if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) { + path.erase(path.begin()); + } + + if (!path.empty()) { + Cell requestedEndCell; + if (worldToCell(end, requestedEndCell) && + requestedEndCell.x == endCell.x && + requestedEndCell.z == endCell.z) { + path.back() = Eigen::Vector3f(end.x(), floorY, end.z()); + } + } + + return path; +} + +bool PathFinder::setAreaAvailable(const std::string& areaName, bool available) +{ + bool found = false; + bool changed = false; + for (NavigationArea& area : areas) { + if (area.name == areaName) { + found = true; + changed = changed || area.available != available; + area.available = available; + } + } + + if (changed) { + rebuildWalkableGrid(); + } + + return found; +} + +bool PathFinder::isWalkable(const Eigen::Vector3f& point) const +{ + Cell cell; + return worldToCell(point, cell) && isCellWalkable(cell); +} + +void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath) +{ + cellSize = 0.4f; + agentRadius = 0.45f; + floorY = 0.0f; + objectPadding = 0.25f; + areas.clear(); + + try { + std::string content; + if (!zipPath.empty()) { + const std::vector bytes = readFileFromZIP(configPath, zipPath); + content.assign(bytes.begin(), bytes.end()); + } else { + content = readTextFile(configPath); + } + + if (content.empty()) { + std::cerr << "[nav] Navigation config is empty or missing: " << configPath << "\n"; + return; + } + + const json root = json::parse(content); + cellSize = root.value("cellSize", cellSize); + agentRadius = root.value("agentRadius", agentRadius); + floorY = root.value("floorY", floorY); + objectPadding = root.value("objectPadding", objectPadding); + + if (!root.contains("areas") || !root["areas"].is_array()) { + std::cerr << "[nav] Navigation config has no 'areas' array: " << configPath << "\n"; + return; + } + + for (const auto& item : root["areas"]) { + NavigationArea area; + area.name = item.value("name", ""); + area.available = item.value("available", true); + area.polygon = readPolygon(item); + + if (area.name.empty()) { + std::cerr << "[nav] Skipping navigation area without name\n"; + continue; + } + if (area.polygon.size() < 3) { + std::cerr << "[nav] Skipping navigation area '" << area.name << "': polygon has fewer than 3 points\n"; + continue; + } + + areas.push_back(area); + } + } catch (const std::exception& e) { + std::cerr << "[nav] Failed to load config '" << configPath << "': " << e.what() << "\n"; + } + + cellSize = (std::max)(cellSize, 0.1f); + agentRadius = (std::max)(agentRadius, 0.0f); + objectPadding = (std::max)(objectPadding, 0.0f); +} + +void PathFinder::resetGridBounds() +{ + if (areas.empty()) { + gridWidth = 0; + gridDepth = 0; + return; + } + + minX = areas.front().polygon.front().x(); + float maxX = minX; + minZ = areas.front().polygon.front().y(); + float maxZ = minZ; + + auto includePoint = [&](float x, float z) { + minX = (std::min)(minX, x); + maxX = (std::max)(maxX, x); + minZ = (std::min)(minZ, z); + maxZ = (std::max)(maxZ, z); + }; + + for (const NavigationArea& area : areas) { + for (const Eigen::Vector2f& point : area.polygon) { + includePoint(point.x(), point.y()); + } + } + + for (const ObstacleMesh& obstacle : obstacles) { + if (!obstacle.mesh) { + continue; + } + for (const Eigen::Vector3f& point : obstacle.mesh->PositionData) { + const Eigen::Vector3f worldPoint = point + obstacle.offset; + includePoint(worldPoint.x(), worldPoint.z()); + } + } + + const float padding = cellSize * 2.0f + agentRadius + objectPadding; + minX -= padding; + minZ -= padding; + maxX += padding; + maxZ += padding; + + gridWidth = static_cast(std::ceil((maxX - minX) / cellSize)); + gridDepth = static_cast(std::ceil((maxZ - minZ) / cellSize)); +} + +void PathFinder::rebuildWalkableGrid() +{ + walkable.assign(static_cast(gridWidth * gridDepth), 0); + markAvailableAreasWalkable(); + markObstacleMeshesBlocked(); +} + +void PathFinder::markAvailableAreasWalkable() +{ + 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); + + for (const NavigationArea& area : areas) { + if (!area.available) { + continue; + } + if (pointInPolygon(center.x(), center.z(), area.polygon)) { + walkable[static_cast(indexOf(cell))] = 1; + break; + } + } + } + } +} + +void PathFinder::markObstacleMeshesBlocked() +{ + const float padding = agentRadius + objectPadding; + + for (const ObstacleMesh& obstacle : obstacles) { + if (!obstacle.mesh || obstacle.mesh->PositionData.empty()) { + continue; + } + + Eigen::Vector3f minPoint = obstacle.mesh->PositionData[0] + obstacle.offset; + Eigen::Vector3f maxPoint = minPoint; + for (const Eigen::Vector3f& localPoint : obstacle.mesh->PositionData) { + const Eigen::Vector3f point = localPoint + obstacle.offset; + minPoint = minPoint.cwiseMin(point); + maxPoint = maxPoint.cwiseMax(point); + } + + Cell from; + Cell to; + worldToCell({ minPoint.x() - padding, floorY, minPoint.z() - padding }, from); + worldToCell({ maxPoint.x() + padding, floorY, maxPoint.z() + padding }, to); + from.x = (std::max)(from.x, 0); + from.z = (std::max)(from.z, 0); + to.x = (std::min)(to.x, gridWidth - 1); + to.z = (std::min)(to.z, gridDepth - 1); + + for (int z = from.z; z <= to.z; ++z) { + for (int x = from.x; x <= to.x; ++x) { + const Cell cell{ x, z }; + if (isInsideGrid(cell)) { + walkable[static_cast(indexOf(cell))] = 0; + } + } + } + } +} + +bool PathFinder::worldToCell(const Eigen::Vector3f& point, Cell& out) const +{ + if (gridWidth <= 0 || gridDepth <= 0) { + out = {}; + return false; + } + + out.x = static_cast(std::floor((point.x() - minX) / cellSize)); + out.z = static_cast(std::floor((point.z() - minZ) / cellSize)); + return isInsideGrid(out); +} + +Eigen::Vector3f PathFinder::cellCenter(const Cell& cell) const +{ + return { + minX + (static_cast(cell.x) + 0.5f) * cellSize, + floorY, + minZ + (static_cast(cell.z) + 0.5f) * cellSize + }; +} + +bool PathFinder::isInsideGrid(const Cell& cell) const +{ + return cell.x >= 0 && cell.z >= 0 && cell.x < gridWidth && cell.z < gridDepth; +} + +bool PathFinder::isCellWalkable(const Cell& cell) const +{ + return isInsideGrid(cell) && walkable[static_cast(indexOf(cell))] != 0; +} + +int PathFinder::indexOf(const Cell& cell) const +{ + return cell.z * gridWidth + cell.x; +} + +bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const +{ + Cell origin; + if (!worldToCell(point, origin)) { + const int x = static_cast(std::floor((point.x() - minX) / cellSize)); + const int z = static_cast(std::floor((point.z() - minZ) / cellSize)); + origin.x = (std::min)((std::max)(x, 0), gridWidth - 1); + origin.z = (std::min)((std::max)(z, 0), gridDepth - 1); + } + + if (isCellWalkable(origin)) { + out = origin; + return true; + } + + const int maxRadius = (std::max)(4, static_cast(std::ceil(8.0f / cellSize))); + for (int radius = 1; radius <= maxRadius; ++radius) { + for (int dz = -radius; dz <= radius; ++dz) { + for (int dx = -radius; dx <= radius; ++dx) { + if (std::abs(dx) != radius && std::abs(dz) != radius) { + continue; + } + + Cell candidate{ origin.x + dx, origin.z + dz }; + if (isCellWalkable(candidate)) { + out = candidate; + return true; + } + } + } + } + + return false; +} + +bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const +{ + const Eigen::Vector3f a = cellCenter(from); + const Eigen::Vector3f b = cellCenter(to); + const float distance = (b - a).norm(); + const int steps = (std::max)(1, static_cast(std::ceil(distance / (cellSize * 0.5f)))); + + for (int i = 0; i <= steps; ++i) { + const float t = static_cast(i) / static_cast(steps); + const Eigen::Vector3f p = a * (1.0f - t) + b * t; + Cell cell; + if (!worldToCell(p, cell) || !isCellWalkable(cell)) { + return false; + } + } + + return true; +} + +std::vector PathFinder::smoothCells(const std::vector& cells) const +{ + if (cells.size() <= 2) { + return cells; + } + + std::vector result; + size_t anchor = 0; + result.push_back(cells[anchor]); + + while (anchor < cells.size() - 1) { + size_t next = cells.size() - 1; + while (next > anchor + 1 && !hasLineOfSight(cells[anchor], cells[next])) { + --next; + } + + result.push_back(cells[next]); + anchor = next; + } + + return result; +} + +bool PathFinder::pointInPolygon(float x, float z, const std::vector& polygon) +{ + bool inside = false; + size_t previous = polygon.size() - 1; + + for (size_t current = 0; current < polygon.size(); ++current) { + const Eigen::Vector2f& a = polygon[current]; + const Eigen::Vector2f& b = polygon[previous]; + + const bool crossesZ = (a.y() > z) != (b.y() > z); + if (crossesZ) { + const float t = (z - a.y()) / (b.y() - a.y()); + const float intersectionX = a.x() + t * (b.x() - a.x()); + if (x < intersectionX) { + inside = !inside; + } + } + + previous = current; + } + + return inside; +} + +} // namespace ZL diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h new file mode 100644 index 0000000..48d2cae --- /dev/null +++ b/src/navigation/PathFinder.h @@ -0,0 +1,76 @@ +#pragma once + +#include "render/Renderer.h" +#include +#include +#include + +namespace ZL { + +class PathFinder { +public: + struct ObstacleMesh { + const VertexDataStruct* mesh = nullptr; + Eigen::Vector3f offset = Eigen::Vector3f::Zero(); + }; + + struct Cell { + int x = 0; + int z = 0; + }; + + void build(const std::vector& obstacleMeshes, + const std::string& configPath, + const std::string& zipPath = ""); + + std::vector findPath(const Eigen::Vector3f& start, + const Eigen::Vector3f& end) const; + + bool setAreaAvailable(const std::string& areaName, bool available); + bool isReady() const { return ready; } + bool isWalkable(const Eigen::Vector3f& point) const; + +private: + struct NavigationArea { + std::string name; + bool available = true; + std::vector polygon; + }; + + float cellSize = 0.4f; + float agentRadius = 0.45f; + float floorY = 0.0f; + float objectPadding = 0.25f; + + float minX = 0.0f; + float minZ = 0.0f; + int gridWidth = 0; + int gridDepth = 0; + bool ready = false; + + std::string loadedConfigPath; + std::string loadedZipPath; + std::vector obstacles; + std::vector walkable; + std::vector areas; + + void loadConfig(const std::string& configPath, const std::string& zipPath); + void resetGridBounds(); + void rebuildWalkableGrid(); + void markAvailableAreasWalkable(); + void markObstacleMeshesBlocked(); + + bool worldToCell(const Eigen::Vector3f& point, Cell& out) const; + Eigen::Vector3f cellCenter(const Cell& cell) const; + bool isInsideGrid(const Cell& cell) const; + bool isCellWalkable(const Cell& cell) const; + int indexOf(const Cell& cell) const; + + bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const; + bool hasLineOfSight(const Cell& from, const Cell& to) const; + std::vector smoothCells(const std::vector& cells) const; + + static bool pointInPolygon(float x, float z, const std::vector& polygon); +}; + +} // namespace ZL