From dc9ea361f0e3fbe8f6b6c11f5256aa756a1a053d Mon Sep 17 00:00:00 2001 From: Vladislav Khorev Date: Sun, 15 Nov 2026 00:13:07 +0600 Subject: [PATCH] Refactoring --- cmake/FetchDependencies.cmake | 3 + cmake/FetchDependenciesLinux.cmake | 3 + cmake/FetchDependenciesServer.cmake | 3 + cmake/FetchNavigationDependencies.cmake | 84 ++ cmake/ThirdParty.cmake | 1 + proj-linux/CMakeLists.txt | 4 + proj-web/CMakeLists.txt | 6 +- proj-windows/CMakeLists.txt | 9 +- server/CMakeLists.txt | 8 +- server/tests/PathFinderTests.cpp | 278 +---- src/LocationState.cpp | 15 - src/LocationState.h | 5 - src/navigation/PathFinder.cpp | 1388 +++++++---------------- src/navigation/PathFinder.h | 743 ++---------- src/navigation/fpmutils.cpp | 134 +++ src/navigation/fpmutils.h | 67 ++ 16 files changed, 818 insertions(+), 1933 deletions(-) create mode 100644 cmake/FetchNavigationDependencies.cmake create mode 100644 src/navigation/fpmutils.cpp create mode 100644 src/navigation/fpmutils.h diff --git a/cmake/FetchDependencies.cmake b/cmake/FetchDependencies.cmake index a8de79d..06b21b2 100644 --- a/cmake/FetchDependencies.cmake +++ b/cmake/FetchDependencies.cmake @@ -2,6 +2,9 @@ set(THIRDPARTY_DIR "${CMAKE_CURRENT_LIST_DIR}/../thirdparty") +include("${CMAKE_CURRENT_LIST_DIR}/FetchNavigationDependencies.cmake") +frg_fetch_navigation_dependencies() + if(NOT EXISTS "${THIRDPARTY_DIR}") file(MAKE_DIRECTORY "${THIRDPARTY_DIR}") endif() diff --git a/cmake/FetchDependenciesLinux.cmake b/cmake/FetchDependenciesLinux.cmake index 402df09..8d01124 100644 --- a/cmake/FetchDependenciesLinux.cmake +++ b/cmake/FetchDependenciesLinux.cmake @@ -2,6 +2,9 @@ set(THIRDPARTY_DIR "${CMAKE_CURRENT_LIST_DIR}/../thirdparty") +include("${CMAKE_CURRENT_LIST_DIR}/FetchNavigationDependencies.cmake") +frg_fetch_navigation_dependencies() + if(NOT EXISTS "${THIRDPARTY_DIR}") file(MAKE_DIRECTORY "${THIRDPARTY_DIR}") endif() diff --git a/cmake/FetchDependenciesServer.cmake b/cmake/FetchDependenciesServer.cmake index 7727707..4ac1bc4 100644 --- a/cmake/FetchDependenciesServer.cmake +++ b/cmake/FetchDependenciesServer.cmake @@ -6,6 +6,9 @@ set(THIRDPARTY_DIR "${CMAKE_CURRENT_LIST_DIR}/../thirdparty") file(MAKE_DIRECTORY "${THIRDPARTY_DIR}") +include("${CMAKE_CURRENT_LIST_DIR}/FetchNavigationDependencies.cmake") +frg_fetch_navigation_dependencies() + macro(server_check_and_download URL ARCHIVE_NAME EXTRACTED_DIR_NAME CHECK_FILE) set(_archive_path "${THIRDPARTY_DIR}/${ARCHIVE_NAME}") set(_src_path "${THIRDPARTY_DIR}/${EXTRACTED_DIR_NAME}") diff --git a/cmake/FetchNavigationDependencies.cmake b/cmake/FetchNavigationDependencies.cmake new file mode 100644 index 0000000..1c2b635 --- /dev/null +++ b/cmake/FetchNavigationDependencies.cmake @@ -0,0 +1,84 @@ +set(FRG_NAVIGATION_THIRDPARTY_DIR + "${CMAKE_CURRENT_LIST_DIR}/../thirdparty") + +file(MAKE_DIRECTORY "${FRG_NAVIGATION_THIRDPARTY_DIR}") + +function(frg_fetch_navigation_dependencies) + set(_dependencies + "https://github.com/AngusJohnson/Clipper2/archive/refs/tags/Clipper2_2.0.1.zip|Clipper2_2.0.1.zip|Clipper2-Clipper2_2.0.1|CPP/CMakeLists.txt" + "https://github.com/artem-ogre/CDT/archive/refs/tags/2.0.2.zip|CDT-2.0.2.zip|CDT-2.0.2|CDT/include/CDT.h" + "https://github.com/MikeLankamp/fpm/archive/refs/tags/v1.1.0.zip|fpm-v1.1.0.zip|fpm-1.1.0|include/fpm/fixed.hpp" + ) + + foreach(_dependency IN LISTS _dependencies) + string(REPLACE "|" ";" _parts "${_dependency}") + list(GET _parts 0 _url) + list(GET _parts 1 _archive_name) + list(GET _parts 2 _source_name) + list(GET _parts 3 _check_file) + + set(_archive "${FRG_NAVIGATION_THIRDPARTY_DIR}/${_archive_name}") + set(_source "${FRG_NAVIGATION_THIRDPARTY_DIR}/${_source_name}") + + if(NOT EXISTS "${_source}/${_check_file}") + if(NOT EXISTS "${_archive}") + message(STATUS "Downloading ${_archive_name}...") + file(DOWNLOAD "${_url}" "${_archive}" + SHOW_PROGRESS + STATUS _download_status) + list(GET _download_status 0 _download_code) + if(NOT _download_code EQUAL 0) + file(REMOVE "${_archive}") + message(FATAL_ERROR + "Failed to download ${_url}: ${_download_status}") + endif() + endif() + + message(STATUS "Extracting ${_archive_name}...") + execute_process( + COMMAND ${CMAKE_COMMAND} -E tar xf "${_archive}" + WORKING_DIRECTORY "${FRG_NAVIGATION_THIRDPARTY_DIR}" + RESULT_VARIABLE _extract_result) + if(NOT _extract_result EQUAL 0) + message(FATAL_ERROR + "Failed to extract ${_archive}") + endif() + endif() + endforeach() +endfunction() + +function(frg_configure_navigation_dependencies) + frg_fetch_navigation_dependencies() + + set(_clipper_source + "${FRG_NAVIGATION_THIRDPARTY_DIR}/Clipper2-Clipper2_2.0.1") + if(NOT TARGET Clipper2) + set(CLIPPER2_UTILS OFF CACHE BOOL "" FORCE) + set(CLIPPER2_EXAMPLES OFF CACHE BOOL "" FORCE) + set(CLIPPER2_TESTS OFF CACHE BOOL "" FORCE) + set(CLIPPER2_USINGZ OFF CACHE STRING "" FORCE) + set(BUILD_SHARED_LIBS OFF CACHE BOOL "" FORCE) + add_subdirectory("${_clipper_source}/CPP" + "${CMAKE_BINARY_DIR}/thirdparty/clipper2" EXCLUDE_FROM_ALL) + endif() + + if(NOT TARGET frg_navigation_cdt) + add_library(frg_navigation_cdt INTERFACE) + target_include_directories(frg_navigation_cdt INTERFACE + "${FRG_NAVIGATION_THIRDPARTY_DIR}/CDT-2.0.2/CDT/include" + "${FRG_NAVIGATION_THIRDPARTY_DIR}/CDT-2.0.2/include" + "${FRG_NAVIGATION_THIRDPARTY_DIR}/CDT-2.0.2") + endif() + + if(NOT TARGET frg_navigation_fpm) + add_library(frg_navigation_fpm INTERFACE) + target_include_directories(frg_navigation_fpm INTERFACE + "${FRG_NAVIGATION_THIRDPARTY_DIR}/fpm-1.1.0/include") + endif() + + if(NOT TARGET frg_navigation_dependencies) + add_library(frg_navigation_dependencies INTERFACE) + target_link_libraries(frg_navigation_dependencies INTERFACE + Clipper2 frg_navigation_cdt frg_navigation_fpm) + endif() +endfunction() diff --git a/cmake/ThirdParty.cmake b/cmake/ThirdParty.cmake index 693cbe4..2bbf7f7 100644 --- a/cmake/ThirdParty.cmake +++ b/cmake/ThirdParty.cmake @@ -1,6 +1,7 @@ # cmake/ThirdParty.cmake include("${CMAKE_CURRENT_LIST_DIR}/FetchDependencies.cmake") +frg_configure_navigation_dependencies() macro(log msg) message(STATUS "[ThirdParty] ${msg}") diff --git a/proj-linux/CMakeLists.txt b/proj-linux/CMakeLists.txt index fb8994f..fdb8bc7 100644 --- a/proj-linux/CMakeLists.txt +++ b/proj-linux/CMakeLists.txt @@ -33,6 +33,7 @@ pkg_check_modules(LIBZIP REQUIRED libzip) pkg_check_modules(LUA REQUIRED lua5.4) include("${CMAKE_CURRENT_SOURCE_DIR}/../cmake/FetchDependenciesLinux.cmake") +frg_configure_navigation_dependencies() # =========================================== @@ -144,6 +145,8 @@ add_executable(ShadowOverBishkekDemo ../src/ScriptEngine.cpp ../src/navigation/PathFinder.h ../src/navigation/PathFinder.cpp + ../src/navigation/fpmutils.h + ../src/navigation/fpmutils.cpp ../src/items/GameObjectLoader.h ../src/items/GameObjectLoader.cpp ../src/items/Item.h @@ -221,6 +224,7 @@ target_link_libraries(ShadowOverBishkekDemo PRIVATE ${SDL2_MIXER_LIBRARIES} ${LIBZIP_LIBRARIES} ${LUA_LIBRARIES} + frg_navigation_dependencies pthread ) diff --git a/proj-web/CMakeLists.txt b/proj-web/CMakeLists.txt index 5609054..f1bffe2 100644 --- a/proj-web/CMakeLists.txt +++ b/proj-web/CMakeLists.txt @@ -23,6 +23,7 @@ endif() # --- АВТО-ЗАГРУЗКА ЗАВИСИМОСТЕЙ --- include("${CMAKE_CURRENT_SOURCE_DIR}/../cmake/FetchDependencies.cmake") +frg_configure_navigation_dependencies() # Теперь гарантированно есть папка ../thirdparty со всеми исходниками # =========================================== @@ -146,6 +147,8 @@ set(SOURCES ../src/ScriptEngine.cpp ../src/navigation/PathFinder.h ../src/navigation/PathFinder.cpp + ../src/navigation/fpmutils.h + ../src/navigation/fpmutils.cpp ../src/items/GameObjectLoader.h ../src/items/GameObjectLoader.cpp ../src/items/Item.h @@ -212,7 +215,8 @@ set(ENABLE_COMMONCRYPTO OFF CACHE BOOL "" FORCE) add_subdirectory("../thirdparty/libzip-1.11.4" libzip-build) -target_link_libraries(ShadowOverBishkekDemo PRIVATE zip z lua_static websocket.js) +target_link_libraries(ShadowOverBishkekDemo PRIVATE + zip z lua_static websocket.js frg_navigation_dependencies) # Эмскриптен-флаги set(EMSCRIPTEN_FLAGS diff --git a/proj-windows/CMakeLists.txt b/proj-windows/CMakeLists.txt index ba7dae4..621db1c 100644 --- a/proj-windows/CMakeLists.txt +++ b/proj-windows/CMakeLists.txt @@ -88,6 +88,8 @@ add_executable(ShadowOverBishkekDemo WIN32 ../src/ScriptEngine.cpp ../src/navigation/PathFinder.h ../src/navigation/PathFinder.cpp + ../src/navigation/fpmutils.h + ../src/navigation/fpmutils.cpp ../src/items/GameObjectLoader.h ../src/items/GameObjectLoader.cpp ../src/items/Item.h @@ -131,13 +133,8 @@ set_property(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} PROPERTY VS_STARTUP_PROJECT S # include-пути проекта target_include_directories(ShadowOverBishkekDemo PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/../src" - "${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/Clipper2-Clipper2_2.0.1/CPP/build/Release/include" ) -#target_link_directories(ShadowOverBishkekDemo PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/Clipper2-Clipper2_2.0.1/CPP/build/Release/lib") - -target_link_directories(ShadowOverBishkekDemo PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/../thirdparty/Clipper2-Clipper2_2.0.1/CPP/build_debug/Debug/lib") - set_target_properties(ShadowOverBishkekDemo PROPERTIES OUTPUT_NAME "ShadowOverBishkekDemo" @@ -168,7 +165,7 @@ target_link_libraries(ShadowOverBishkekDemo PRIVATE sol2_external_lib SDL2_mixer_external_lib openssl_ssl_external_lib - Clipper2.lib + frg_navigation_dependencies ) # Линкуем OpenGL (Windows) diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt index d94cceb..32537a1 100644 --- a/server/CMakeLists.txt +++ b/server/CMakeLists.txt @@ -10,6 +10,7 @@ set(GAME_ROOT "${CMAKE_CURRENT_LIST_DIR}/..") find_package(Threads REQUIRED) include("${GAME_ROOT}/cmake/FetchDependenciesServer.cmake") +frg_configure_navigation_dependencies() find_package(Eigen3 3.3 CONFIG QUIET) if(NOT TARGET Eigen3::Eigen) set(EIGEN_BUNDLED_DIR "${GAME_ROOT}/thirdparty/eigen-5.0.0") @@ -51,6 +52,8 @@ add_library(game_server_core STATIC "${GAME_ROOT}/src/items/Item.h" "${GAME_ROOT}/src/navigation/PathFinder.cpp" "${GAME_ROOT}/src/navigation/PathFinder.h" + "${GAME_ROOT}/src/navigation/fpmutils.cpp" + "${GAME_ROOT}/src/navigation/fpmutils.h" "${GAME_ROOT}/src/quest/QuestJournal.cpp" "${GAME_ROOT}/src/quest/QuestJournal.h" "${GAME_ROOT}/src/quest/QuestTypes.h" @@ -86,6 +89,8 @@ source_group(TREE "${GAME_ROOT}/src" PREFIX "Source" FILES "${GAME_ROOT}/src/items/Item.h" "${GAME_ROOT}/src/navigation/PathFinder.cpp" "${GAME_ROOT}/src/navigation/PathFinder.h" + "${GAME_ROOT}/src/navigation/fpmutils.cpp" + "${GAME_ROOT}/src/navigation/fpmutils.h" "${GAME_ROOT}/src/quest/QuestJournal.cpp" "${GAME_ROOT}/src/quest/QuestJournal.h" "${GAME_ROOT}/src/quest/QuestTypes.h" @@ -94,7 +99,8 @@ source_group(TREE "${GAME_ROOT}/src" PREFIX "Source" FILES ) target_include_directories(game_server_core PUBLIC "${GAME_ROOT}/src") -target_link_libraries(game_server_core PUBLIC Eigen3::Eigen Threads::Threads) +target_link_libraries(game_server_core PUBLIC + Eigen3::Eigen Threads::Threads frg_navigation_dependencies) target_compile_definitions(game_server_core PRIVATE FRG_HEADLESS_SERVER=1) if(MSVC) diff --git a/server/tests/PathFinderTests.cpp b/server/tests/PathFinderTests.cpp index 7555bfc..738190e 100644 --- a/server/tests/PathFinderTests.cpp +++ b/server/tests/PathFinderTests.cpp @@ -1,269 +1,35 @@ -#include "CharacterState.h" -#include "LocationGameState.h" #include "navigation/PathFinder.h" -#include -#include -#include + +#include #include -#include -#include #include -#include -#include -#include namespace { - int fail(const char* message) { - std::cout << "PathFinderTests: " << message << std::endl; - return EXIT_FAILURE; + std::cout << "PathFinderTests: " << message << std::endl; + return EXIT_FAILURE; +} } -std::filesystem::path writeGrid( - const std::string& name, int width, int depth, bool corridor) +int main() { - const auto path = std::filesystem::temp_directory_path() / name; - std::ofstream output(path); - output << "cellSize 1\nagentRadius 0.1\nfloorY 0\n" - << "minX 0\nminZ 0\ngridWidth " << width - << "\ngridDepth " << depth << "\n"; - for (int z = 0; z < depth; ++z) { - for (int x = 0; x < width; ++x) - output << (!corridor || z == depth / 2 ? '1' : '0'); - output << '\n'; - } - return path; -} + FRG::PathFinder pathFinder; + pathFinder.build(); + if (!pathFinder.isReady()) return fail("Navmesh was not built"); -bool samePath( - const std::vector& lhs, - const std::vector& rhs) -{ - if (lhs.size() != rhs.size()) return false; - for (size_t i = 0; i < lhs.size(); ++i) - if (lhs[i] != rhs[i]) return false; - return true; -} + const Eigen::Vector3f start(-50.f, 0.f, -50.f); + const Eigen::Vector3f end(50.f, 0.f, 50.f); + const auto path = pathFinder.findPathToNearest(start, end); + if (path.size() < 2) return fail("Path query did not produce a route"); + if (!pathFinder.isWalkable(Eigen::Vector3f(0.f, 0.f, 50.f))) + return fail("Walkability query failed"); -float segmentDistanceSquared( - const Eigen::Vector3f& point, - const Eigen::Vector3f& start, - const Eigen::Vector3f& end) -{ - const Eigen::Vector2f p(point.x(), point.z()); - const Eigen::Vector2f a(start.x(), start.z()); - const Eigen::Vector2f segment(end.x() - start.x(), end.z() - start.z()); - const float lengthSquared = segment.squaredNorm(); - const float t = lengthSquared > 1e-8f - ? (std::max)(0.f, (std::min)(1.f, (p - a).dot(segment) / lengthSquared)) - : 0.f; - return (p - (a + segment * t)).squaredNorm(); -} + const auto clamped = pathFinder.clampToNavMesh(FRG::Vec2fp::fromFloat(150.f, 150.f)); + if (static_cast(clamped.x) > 100.01f || + static_cast(clamped.y) > 100.01f) + return fail("Clamping returned a point outside the navmesh"); -void runBenchmark(FRG::PathFinder& pathFinder, int queries) -{ - std::vector obstacles; - for (int i = 0; i < queries; ++i) { - const float x = 4.5f + static_cast((i * 7) % 70); - const float z = 4.5f + static_cast((i * 13) % 70); - obstacles.push_back({Eigen::Vector3f(x, 0.f, z), 0.25f}); - } - const auto start = std::chrono::steady_clock::now(); - size_t waypointCount = 0; - for (int i = 0; i < queries; ++i) { - const Eigen::Vector3f from( - 1.5f, 0.f, 1.5f + static_cast(i % 30)); - const Eigen::Vector3f to( - 78.5f, 0.f, 78.5f - static_cast(i % 30)); - waypointCount += - pathFinder.findPathToNearest(from, to, obstacles).size(); - } - const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start); - std::cout << "PathFinder benchmark " << queries << " characters/queries: " - << elapsed.count() << " ms, " << waypointCount - << " waypoints" << std::endl; -} - -} // namespace - -int main(int argc, char** argv) -{ - const bool benchmark = argc > 1 && std::string(argv[1]) == "--benchmark"; - const auto openGrid = writeGrid("frg_pathfinder_open.txt", 80, 80, false); - FRG::PathFinder pathFinder; - pathFinder.build(openGrid.string()); - if (!pathFinder.isReady()) return fail("Open test grid did not load"); - - const FRG::PathFinder::DynamicObstacle first{ - Eigen::Vector3f(8.5f, 0.f, 8.5f), 0.75f}; - const FRG::PathFinder::DynamicObstacle second{ - Eigen::Vector3f(12.5f, 0.f, 10.5f), 0.75f}; - const Eigen::Vector3f start(2.5f, 0.f, 8.5f); - const Eigen::Vector3f end(18.5f, 0.f, 8.5f); - const auto ordered = pathFinder.findPathToNearest( - start, end, {first, second}); - const auto reversed = pathFinder.findPathToNearest( - start, end, {second, first}); - if (ordered.empty() || !samePath(ordered, reversed)) - return fail("Obstacle insertion order changed the route"); - Eigen::Vector3f previous = start; - for (const Eigen::Vector3f& point : ordered) { - if (segmentDistanceSquared( - first.position, previous, point) < 0.85f * 0.85f) - return fail("Smoothed path crossed a dynamic obstacle"); - previous = point; - } - - const auto occupiedDestination = - pathFinder.findPathToNearest(start, first.position, {first}); - if (occupiedDestination.empty()) - return fail("Occupied destination did not produce a nearest route"); - const Eigen::Vector3f occupiedEnd = occupiedDestination.back(); - const Eigen::Vector2f occupiedDelta( - occupiedEnd.x() - first.position.x(), - occupiedEnd.z() - first.position.z()); - if (occupiedDelta.squaredNorm() < 0.85f * 0.85f) - return fail("Occupied destination ended inside the obstacle"); - - const auto corridorGrid = - writeGrid("frg_pathfinder_corridor.txt", 20, 3, true); - FRG::PathFinder corridor; - corridor.build(corridorGrid.string()); - const FRG::PathFinder::DynamicObstacle blocker{ - Eigen::Vector3f(10.5f, 0.f, 1.5f), 0.45f}; - const auto nearest = corridor.findPathToNearest( - Eigen::Vector3f(1.5f, 0.f, 1.5f), - Eigen::Vector3f(18.5f, 0.f, 1.5f), {blocker}); - if (nearest.empty() || nearest.back().x() >= blocker.position.x()) - return fail("Nearest fallback ignored a dynamic blocker"); - - FRG::CharacterState guardedMover; - guardedMover.configureAsPlayer(); - guardedMover.enabled = true; - guardedMover.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f); - guardedMover.requestedWalkTarget = guardedMover.walkTarget; - guardedMover.pathWaypoints = {guardedMover.walkTarget}; - const std::vector movementObstacles{ - {"mover", Eigen::Vector3f::Zero(), 0.45f}, - {"blocker", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f}}; - FRG::CharacterUpdateContext guardedContext; - guardedContext.characterId = "mover"; - guardedContext.movementObstacles = &movementObstacles; - FRG::CharacterUpdateEvents blockedEvents; - for (int i = 0; i < 3; ++i) - blockedEvents = guardedMover.update(100, guardedContext); - if (!guardedMover.position.isZero() || - blockedEvents.forceNavigationReplan || - !guardedMover.pathWaypoints.empty() || - guardedMover.requestedWalkTarget != guardedMover.position || - guardedMover.currentState != FRG::AnimationState::STAND) - return fail("Blocked player action was not cancelled"); - - FRG::CharacterState retryingNpc; - retryingNpc.enabled = true; - retryingNpc.configureNavigationRetryCooldowns( - "retrying_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11}); - FRG::CharacterState otherNpc; - otherNpc.configureNavigationRetryCooldowns( - "other_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11}); - if (otherNpc.navigationRetryCooldownTicks == - retryingNpc.navigationRetryCooldownTicks) - return fail("Stable NPC IDs produced the same retry sequence"); - const int firstCooldown = - retryingNpc.navigationRetryCooldownTicks.front(); - retryingNpc.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f); - retryingNpc.requestedWalkTarget = retryingNpc.walkTarget; - retryingNpc.pathWaypoints = {retryingNpc.walkTarget}; - const std::vector movingBlocker{ - {"b", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f, true, - Eigen::Vector3f(-1.f, 0.f, 0.f)}}; - FRG::CharacterUpdateContext retryContext; - retryContext.characterId = "retrying_npc"; - retryContext.movementObstacles = &movingBlocker; - FRG::CharacterUpdateEvents retryEvents; - for (int i = 0; i < 3; ++i) - retryEvents = retryingNpc.update(100, retryContext); - if (retryEvents.forceNavigationReplan || - !retryingNpc.pathWaypoints.empty() || - retryingNpc.navigationRetryTicks != firstCooldown || - retryingNpc.requestedWalkTarget != Eigen::Vector3f(2.f, 0.f, 0.f)) - return fail("Blocked NPC did not stop with a deterministic cooldown"); - for (int i = 0; i < firstCooldown; ++i) - retryEvents = retryingNpc.update(100, retryContext); - if (!retryEvents.forceNavigationReplan || - retryEvents.blockingCharacterId != "b") - return fail("Blocked NPC did not retry after its cooldown"); - - retryingNpc.movedLastTick = true; - nlohmann::json blockedState; - retryingNpc.save(blockedState); - FRG::CharacterState restoredMover; - restoredMover.load(blockedState); - if (restoredMover.movementBlockedTicks != - retryingNpc.movementBlockedTicks || - restoredMover.navigationRetryTicks != - retryingNpc.navigationRetryTicks || - restoredMover.navigationRetryCooldownTicks != - retryingNpc.navigationRetryCooldownTicks || - restoredMover.navigationRetryCooldownIndex != - retryingNpc.navigationRetryCooldownIndex || - !restoredMover.movedLastTick) - return fail("Blocked movement state did not round trip"); - - FRG::LocationGameState occupancyState; - auto stationary = std::make_shared(); - stationary->enabled = true; - stationary->hp = 10.f; - auto moving = std::make_shared(); - moving->enabled = true; - moving->hp = 10.f; - moving->pathWaypoints = {Eigen::Vector3f(4.f, 0.f, 0.f)}; - moving->movedLastTick = true; - FRG::PlayerStateMap occupancyPlayers{ - {"stationary", stationary}, {"moving", moving}}; - if (occupancyState.collectPathObstacles( - "requester", occupancyPlayers).size() != 1 || - occupancyState.collectPathObstacles( - "requester", occupancyPlayers, "moving").size() != 2 || - !occupancyState.collectPathObstacles( - "stationary", occupancyPlayers).empty()) - return fail("Stationary occupancy policy is incorrect"); - moving->movedLastTick = false; - if (occupancyState.collectPathObstacles( - "requester", occupancyPlayers).size() != 2) - return fail("Physically stalled character was omitted from planning"); - - FRG::LocationGameState approachingState; - auto left = std::make_shared(); - left->configureAsPlayer(); - left->enabled = true; - left->position = Eigen::Vector3f(0.f, 0.f, 0.f); - left->walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f); - left->requestedWalkTarget = left->walkTarget; - left->pathWaypoints = {left->walkTarget}; - auto right = std::make_shared(); - right->configureAsPlayer(); - right->enabled = true; - right->position = Eigen::Vector3f(1.4f, 0.f, 0.f); - right->walkTarget = Eigen::Vector3f(-1.f, 0.f, 0.f); - right->requestedWalkTarget = right->walkTarget; - right->pathWaypoints = {right->walkTarget}; - FRG::PlayerStateMap approachingPlayers{{"a", left}, {"b", right}}; - for (int i = 0; i < 5; ++i) - approachingState.update(100, approachingPlayers, false); - const float separation = (right->position - left->position).norm(); - if (separation + 1e-5f < - left->movementContactRadius + right->movementContactRadius || - !left->pathWaypoints.empty() || !right->pathWaypoints.empty()) - return fail("Colliding players did not stop without overlap"); - - if (benchmark) { - runBenchmark(pathFinder, 100); - runBenchmark(pathFinder, 1000); - } - std::filesystem::remove(openGrid); - std::filesystem::remove(corridorGrid); - return EXIT_SUCCESS; -} + std::cout << "PathFinderTests: OK" << std::endl; + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/src/LocationState.cpp b/src/LocationState.cpp index 85ebc20..3a6b004 100644 --- a/src/LocationState.cpp +++ b/src/LocationState.cpp @@ -122,21 +122,6 @@ void loadSinglePlayerPersistence( } - // Entity ownership stays above this navigation-only layer. - /**td::vector LocationNetworkState::planPathExceptMyself( - const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::string& entityId, - const std::vector& dynamicObstacles) const - { - (void)entityId; - if (activeNavigationIndex < 0 || - activeNavigationIndex >= static_cast(navigationMaps.size())) - return {}; - return navigationMaps[activeNavigationIndex].findPathToNearest( - start, end, dynamicObstacles); - }*/ - void LocationNetworkState::initNavigation( const std::vector& paths, const std::string& zipFile) { diff --git a/src/LocationState.h b/src/LocationState.h index b7e1231..220211e 100644 --- a/src/LocationState.h +++ b/src/LocationState.h @@ -41,11 +41,6 @@ struct LocationNetworkState : public ISaveable { //This value is populated from the JSON configs and should not be changed during the regular gameplay. //It might be changed only in the editor mode during development. std::vector navigationMaps; - /*std::vector planPathExceptMyself( - const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::string& entityId, - const std::vector& dynamicObstacles = {}) const;*/ void initNavigation( const std::vector& paths, const std::string& zipFile = std::string()); diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 53d9c60..9ff7075 100644 --- a/src/navigation/PathFinder.cpp +++ b/src/navigation/PathFinder.cpp @@ -1,986 +1,418 @@ -#include "navigation/PathFinder.h" -#include "external/nlohmann/json.hpp" -#include "utils/Utils.h" +#include "navigation/PathFinder.h" + #include -#include -#include -#include -#include +#include #include #include namespace FRG { -using json = nlohmann::json; -Vec3fp operator-(Vec3fp a, Vec3fp b) -{ - return Vec3fp{ - a.x - b.x, - a.y - b.y, - a.z - b.z - }; -} -Vec3fp operator+(Vec3fp a, Vec3fp b) -{ - return Vec3fp{ - a.x + b.x, - a.y + b.y, - a.z + b.z - }; -} - -namespace { - - - - 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; - } - - 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 PathFinderOld::build(const std::string& configPath, - const std::string& zipPath) -{ - ready = false; - loadedConfigPath = configPath; - loadedZipPath = zipPath; - - const bool isTxt = configPath.size() >= 4 && - configPath.compare(configPath.size() - 4, 4, ".txt") == 0; - if (isTxt) { - if (loadGrid(configPath, zipPath)) { - ready = true; - logger() << "[nav] Loaded precomputed grid: " << gridWidth << "x" << gridDepth - << ", cell=" << cellSize << "\n"; - } else { - std::cerr << "[nav] Failed to load precomputed grid: " << configPath << "\n"; - } - return; - } - - 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; - - logger() << "[nav] Polygon grid built: " << gridWidth << "x" << gridDepth - << ", cell=" << cellSize << ", areas=" << areas.size() << "\n"; -} - -std::vector PathFinderOld::findPath(const Eigen::Vector3f& start, - const Eigen::Vector3f& end) const -{ - //const std::lock_guard lock(*queryMutex); - //return runSearch(start, end, 0, false); - return std::vector{start, end}; -} - -/* -std::vector PathFinderOld::findPath(const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& dynamicObstacles) const -{ - const std::lock_guard lock(*queryMutex); - return runSearch(start, end, buildDynamicOverlay(dynamicObstacles), false); -}*/ -std::uint32_t PathFinderOld::buildDynamicOverlay( - const std::vector& dynamicObstacles) const -{ - const size_t cellCount = walkable.size(); - if (dynamicBlockedGeneration.size() != cellCount) { - dynamicBlockedGeneration.assign(cellCount, 0); - dynamicGeneration = 0; - } - if (++dynamicGeneration == 0) { - std::fill(dynamicBlockedGeneration.begin(), dynamicBlockedGeneration.end(), 0); - dynamicGeneration = 1; - } - for (const DynamicObstacle& obstacle : dynamicObstacles) { - // Include the cell half-diagonal so a segment between adjacent - // unblocked cell centres cannot geometrically cut through the circle. - const float radius = - obstacle.radius + agentRadius + cellSize * 0.70710678f; - if (radius <= 0.f) continue; - const int minCellX = (std::max)( - 0, static_cast(std::floor((obstacle.position.x() - radius - minX) / cellSize))); - const int maxCellX = (std::min)( - gridWidth - 1, static_cast(std::floor((obstacle.position.x() + radius - minX) / cellSize))); - const int minCellZ = (std::max)( - 0, static_cast(std::floor((obstacle.position.z() - radius - minZ) / cellSize))); - const int maxCellZ = (std::min)( - gridDepth - 1, static_cast(std::floor((obstacle.position.z() + radius - minZ) / cellSize))); - const float radiusSq = radius * radius; - for (int z = minCellZ; z <= maxCellZ; ++z) { - for (int x = minCellX; x <= maxCellX; ++x) { - const Cell cell{x, z}; - const Eigen::Vector3f center = cellCenter(cell); - const float dx = center.x() - obstacle.position.x(); - const float dz = center.z() - obstacle.position.z(); - if (dx * dx + dz * dz <= radiusSq) - dynamicBlockedGeneration[static_cast(indexOf(cell))] = - dynamicGeneration; - } - } - } - return dynamicGeneration; -} - -bool PathFinderOld::isCellWalkableOverlay( - const Cell& cell, std::uint32_t generation) const -{ - if (!isCellWalkable(cell)) return false; - return generation == 0 || - dynamicBlockedGeneration[static_cast(indexOf(cell))] != generation; -} - -bool PathFinderOld::findNearestWalkableCellOverlay( - const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const -{ - Cell origin; - if (!worldToCell(point, origin)) { - origin.x = (std::max)(0, (std::min)( - gridWidth - 1, static_cast(std::floor((point.x() - minX) / cellSize)))); - origin.z = (std::max)(0, (std::min)( - gridDepth - 1, static_cast(std::floor((point.z() - minZ) / cellSize)))); - } - if (isCellWalkableOverlay(origin, generation)) { - 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 z = origin.z - radius; z <= origin.z + radius; ++z) { - for (int x = origin.x - radius; x <= origin.x + radius; ++x) { - if (std::abs(x - origin.x) != radius && - std::abs(z - origin.z) != radius) continue; - const Cell candidate{x, z}; - if (isCellWalkableOverlay(candidate, generation)) { - out = candidate; - return true; - } - } - } - } - return false; -} - -bool PathFinderOld::hasLineOfSightOverlay( - const Cell& from, const Cell& to, std::uint32_t generation) const -{ - int x = from.x; - int z = from.z; - const int dx = std::abs(to.x - from.x); - const int dz = std::abs(to.z - from.z); - const int sx = from.x < to.x ? 1 : -1; - const int sz = from.z < to.z ? 1 : -1; - int error = dx - dz; - while (true) { - if (!isCellWalkableOverlay({x, z}, generation)) return false; - if (x == to.x && z == to.z) return true; - const int twiceError = error * 2; - const int oldX = x; - const int oldZ = z; - if (twiceError > -dz) { - error -= dz; - x += sx; - } - if (twiceError < dx) { - error += dx; - z += sz; - } - if (x != oldX && z != oldZ && - (!isCellWalkableOverlay({x, oldZ}, generation) || - !isCellWalkableOverlay({oldX, z}, generation))) { - return false; - } - } -} - -std::vector PathFinderOld::smoothCellsOverlay( - const std::vector& cells, std::uint32_t generation) const -{ - if (cells.size() <= 2) return cells; - std::vector result; - result.push_back(cells.front()); - size_t anchor = 0; - while (anchor + 1 < cells.size()) { - size_t next = cells.size() - 1; - while (next > anchor + 1 && - !hasLineOfSightOverlay(cells[anchor], cells[next], generation)) { - --next; - } - result.push_back(cells[next]); - anchor = next; - } - return result; -} - -std::vector PathFinderOld::runSearch( - const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - std::uint32_t obstacleGeneration, - bool nearestReachable) const -{ - if (!ready || walkable.empty()) return {}; - Cell startCell; - Cell endCell; - if (!findNearestWalkableCellOverlay(start, startCell, obstacleGeneration) || - !findNearestWalkableCellOverlay(end, endCell, obstacleGeneration)) return {}; - if (startCell.x == endCell.x && startCell.z == endCell.z) - return {cellCenter(endCell)}; - - const size_t cellCount = walkable.size(); - if (searchCost.size() != cellCount) { - searchCost.resize(cellCount); - searchCameFrom.resize(cellCount); - searchVisitedGeneration.assign(cellCount, 0); - searchGeneration = 0; - } - if (++searchGeneration == 0) { - std::fill(searchVisitedGeneration.begin(), searchVisitedGeneration.end(), 0); - searchGeneration = 1; - } - const auto heuristic = [](const Cell& a, const Cell& b) { - const int dx = std::abs(a.x - b.x); - const int dz = std::abs(a.z - b.z); - return 14 * (std::min)(dx, dz) + 10 * (std::max(dx, dz) - (std::min)(dx, dz)); - }; - struct QueueNode { - int index; - int priority; - int cost; - bool operator<(const QueueNode& other) const { - if (priority != other.priority) return priority > other.priority; - if (cost != other.cost) return cost > other.cost; - return index > other.index; - } - }; - std::priority_queue open; - const int startIndex = indexOf(startCell); - const int endIndex = indexOf(endCell); - searchVisitedGeneration[static_cast(startIndex)] = searchGeneration; - searchCost[static_cast(startIndex)] = 0; - searchCameFrom[static_cast(startIndex)] = -1; - open.push({startIndex, heuristic(startCell, endCell), 0}); - int bestIndex = startIndex; - int bestDistanceSq = - (startCell.x - endCell.x) * (startCell.x - endCell.x) + - (startCell.z - endCell.z) * (startCell.z - endCell.z); - 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(); - const size_t currentOffset = static_cast(current.index); - if (searchVisitedGeneration[currentOffset] != searchGeneration || - searchCost[currentOffset] != current.cost) continue; - if (current.index == endIndex) { - bestIndex = endIndex; - break; - } - const Cell currentCell{ current.index % gridWidth, current.index / gridWidth }; - const int dx = currentCell.x - endCell.x; - const int dz = currentCell.z - endCell.z; - const int distanceSq = dx * dx + dz * dz; - if (distanceSq < bestDistanceSq || - (distanceSq == bestDistanceSq && current.index < bestIndex)) { - bestDistanceSq = distanceSq; - bestIndex = current.index; - } - for (const auto& offset : offsets) { - Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] }; - if (!isCellWalkableOverlay(next, obstacleGeneration)) continue; - const bool diagonal = offset[0] != 0 && offset[1] != 0; - if (diagonal) { - Cell h{ currentCell.x + offset[0], currentCell.z }; - Cell v{ currentCell.x, currentCell.z + offset[1] }; - if (!isCellWalkableOverlay(h, obstacleGeneration) || - !isCellWalkableOverlay(v, obstacleGeneration)) continue; - } - const int nextIndex = indexOf(next); - const size_t nextOffset = static_cast(nextIndex); - const int newCost = current.cost + (diagonal ? 14 : 10); - if (searchVisitedGeneration[nextOffset] == searchGeneration && - newCost >= searchCost[nextOffset]) continue; - searchVisitedGeneration[nextOffset] = searchGeneration; - searchCost[nextOffset] = newCost; - searchCameFrom[nextOffset] = current.index; - open.push({nextIndex, newCost + heuristic(next, endCell), newCost}); - } - } - if (!nearestReachable && bestIndex != endIndex) return {}; - if (bestIndex == startIndex) return {}; - std::vector cells; - for (int cur = bestIndex; cur != -1; - cur = searchCameFrom[static_cast(cur)]) { - cells.push_back({ cur % gridWidth, cur / gridWidth }); - if (cur == startIndex) break; - } - std::reverse(cells.begin(), cells.end()); - cells = smoothCellsOverlay(cells, obstacleGeneration); - 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 (bestIndex == endIndex && !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; -} - -std::vector PathFinderOld::findPathToNearest( - const Eigen::Vector3f& start, const Eigen::Vector3f& end) const -{ - //const std::lock_guard lock(*queryMutex); - // return runSearch(start, end, 0, true); - return std::vector{start, end}; -} - -/* -std::vector PathFinderOld::findPathToNearest( - const Eigen::Vector3f& start, const Eigen::Vector3f& end, - const std::vector& dynamicObstacles) const -{ - const std::lock_guard lock(*queryMutex); - return runSearch( - start, end, buildDynamicOverlay(dynamicObstacles), true); -}*/ - -bool PathFinderOld::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 PathFinderOld::isWalkable(const Eigen::Vector3f& point) const -{ - Cell cell; - return worldToCell(point, cell) && isCellWalkable(cell, walkable); -} - -void PathFinderOld::addObstaclePolygon(const ObstaclePolygon& polygon) -{ - obstaclePolygons.push_back(polygon); - if (ready) { - rebuildWalkableGrid(); - } -} - -bool PathFinderOld::saveConfig(const std::string& path) const -{ - json root; - root["cellSize"] = cellSize; - root["agentRadius"] = agentRadius; - root["floorY"] = floorY; - root["objectPadding"] = objectPadding; - root["boundaryPadding"] = boundaryPadding; - - json areasArr = json::array(); - for (const auto& area : areas) { - json areaObj; - areaObj["name"] = area.name; - areaObj["available"] = area.available; - json poly = json::array(); - for (const auto& pt : area.polygon) { - poly.push_back(json::array({ pt.x(), pt.y() })); - } - areaObj["polygon"] = poly; - areasArr.push_back(areaObj); - } - root["areas"] = areasArr; - - json obstArr = json::array(); - for (const auto& obs : obstaclePolygons) { - json obsObj; - obsObj["name"] = obs.name; - json poly = json::array(); - for (const auto& pt : obs.polygon) { - poly.push_back(json::array({ pt.x(), pt.y() })); - } - obsObj["polygon"] = poly; - obstArr.push_back(obsObj); - } - root["obstacles"] = obstArr; - - /* - std::ofstream f(path); - if (!f.is_open()) { - return false; - } - f << root.dump(2); - return f.good();*/ - - return saveJsonToFile(root, path); -} - -bool PathFinderOld::saveGrid(const std::string& path) const -{ -#if defined(_WIN32) - if (!ready) return false; - std::ofstream f(path); - if (!f.is_open()) return false; - - f << "cellSize " << cellSize << "\n" - << "agentRadius " << agentRadius << "\n" - << "floorY " << floorY << "\n" - << "objectPadding " << objectPadding << "\n" - << "boundaryPadding " << boundaryPadding << "\n" - << "minX " << minX << "\n" - << "minZ " << minZ << "\n" - << "gridWidth " << gridWidth << "\n" - << "gridDepth " << gridDepth << "\n"; - - for (int z = 0; z < gridDepth; ++z) { - for (int x = 0; x < gridWidth; ++x) { - f << (walkable[static_cast(z * gridWidth + x)] ? '1' : '0'); - } - f << '\n'; - } - return f.good(); -#else - return false; -#endif -} - -bool PathFinderOld::loadGrid(const std::string& path, const std::string& zipPath) -{ - std::string content; - try { - if (!zipPath.empty()) { - const auto bytes = readFileFromZIP(path, zipPath); - content.assign(bytes.begin(), bytes.end()); - } else { - content = readTextFile(path); - } - } catch (const std::exception& e) { - std::cerr << "[nav] loadGrid error: " << e.what() << "\n"; - return false; - } - if (content.empty()) return false; - - std::istringstream ss(content); - std::string line; - - auto tryHeader = [&](const std::string& ln) -> bool { - std::istringstream ls(ln); - std::string key; - if (!(ls >> key)) return false; - if (key == "cellSize") { ls >> cellSize; return true; } - else if (key == "agentRadius") { ls >> agentRadius; return true; } - else if (key == "floorY") { ls >> floorY; return true; } - else if (key == "objectPadding") { ls >> objectPadding; return true; } - else if (key == "boundaryPadding") { ls >> boundaryPadding; return true; } - else if (key == "minX") { ls >> minX; return true; } - else if (key == "minZ") { ls >> minZ; return true; } - else if (key == "gridWidth") { ls >> gridWidth; return true; } - else if (key == "gridDepth") { ls >> gridDepth; return true; } - return false; - }; - - std::vector rows; - bool headerDone = false; - while (std::getline(ss, line)) { - if (!headerDone && tryHeader(line)) continue; - headerDone = true; - if (!line.empty()) rows.push_back(line); - } - - if (gridWidth <= 0 || gridDepth <= 0) return false; - - walkable.assign(static_cast(gridWidth * gridDepth), 0); - for (int z = 0; z < static_cast(rows.size()) && z < gridDepth; ++z) { - const std::string& row = rows[z]; - for (int x = 0; x < static_cast(row.size()) && x < gridWidth; ++x) { - walkable[static_cast(z * gridWidth + x)] = (row[x] == '1') ? 1 : 0; - } - } - return true; -} - -void PathFinderOld::loadConfig(const std::string& configPath, const std::string& zipPath) -{ - cellSize = 0.4f; - agentRadius = 0.45f; - floorY = 0.0f; - objectPadding = 0.25f; - boundaryPadding = 0.0f; - areas.clear(); - obstaclePolygons.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); - boundaryPadding = root.value("boundaryPadding", boundaryPadding); - - 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); - } - - 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"; - } - - 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 PathFinderOld::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 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; - maxX += padding; - maxZ += padding; - - gridWidth = static_cast(std::ceil((maxX - minX) / cellSize)); - gridDepth = static_cast(std::ceil((maxZ - minZ) / cellSize)); -} - -void PathFinderOld::rebuildWalkableGrid() -{ - walkable.assign(static_cast(gridWidth * gridDepth), 0); - markAvailableAreasWalkable(); - markObstaclePolygonsBlocked(); -} - -void PathFinderOld::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; - } - } - } - } -} - -void PathFinderOld::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; - } - } - } - } -} - -bool PathFinderOld::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 PathFinderOld::cellCenter(const Cell& cell) const -{ - return { - minX + (static_cast(cell.x) + 0.5f) * cellSize, - floorY, - minZ + (static_cast(cell.z) + 0.5f) * cellSize - }; -} - -bool PathFinderOld::isInsideGrid(const Cell& cell) const -{ - return cell.x >= 0 && cell.z >= 0 && cell.x < gridWidth && cell.z < gridDepth; -} - -bool PathFinderOld::isCellWalkable(const Cell& cell) const -{ - return isCellWalkable(cell, walkable); -} - -bool PathFinderOld::isCellWalkable(const Cell& cell, const std::vector& walkableGrid) const -{ - const int idx = indexOf(cell); - return isInsideGrid(cell) && - idx >= 0 && - idx < static_cast(walkableGrid.size()) && - walkableGrid[static_cast(idx)] != 0; -} - -int PathFinderOld::indexOf(const Cell& cell) const -{ - return cell.z * gridWidth + cell.x; -} - -bool PathFinderOld::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const -{ - return findNearestWalkableCell(point, out, walkable); -} - -bool PathFinderOld::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector& walkableGrid) 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, walkableGrid)) { - 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, walkableGrid)) { - out = candidate; - return true; - } - } - } - } - - return false; -} - -bool PathFinderOld::hasLineOfSight(const Cell& from, const Cell& to) const -{ - return hasLineOfSight(from, to, walkable); -} - -bool PathFinderOld::hasLineOfSight(const Cell& from, const Cell& to, const std::vector& walkableGrid) 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, walkableGrid)) { - return false; - } - } - - return true; -} - -std::vector PathFinderOld::smoothCells(const std::vector& cells) const -{ - return smoothCells(cells, walkable); -} - -std::vector PathFinderOld::smoothCells(const std::vector& cells, const std::vector& walkableGrid) 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], walkableGrid)) { - --next; - } - - result.push_back(cells[next]); - anchor = next; - } - - return result; -} - -bool PathFinderOld::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; -} + void PathFinder::build(const std::string& configPath, const std::string& zipPath) { + // Здесь может быть парсинг configPath в area и obstaclePolygons. + // После заполнения геометрии строим NavMesh: + area.name = "main"; + area.polygon = { + Vec2fp::fromFloat(-100.f, -100.f), + Vec2fp::fromFloat(-100.f, 100.f), + Vec2fp::fromFloat(100.f, 100.f), + Vec2fp::fromFloat(100.f, -100.f) + }; + + area.available = true; + + obstaclePolygons.push_back({ + "p1",{ Vec2fp::fromFloat(-10.f, -10.f), + Vec2fp::fromFloat(-10.f, 10.f), + Vec2fp::fromFloat(10.f, 10.f), + Vec2fp::fromFloat(10.f, -10.f) } + }); + + //ready = true; + + generateNavMesh(); + } + + void PathFinder::generateNavMesh() { + navMesh.clear(); + if (area.polygon.size() < 3) return; + + using namespace Clipper2Lib; + + // Вспомогательная лямбда: безопасно переводит fp в координаты Clipper2 (int64_t) + auto toClipperCoord = [](fp val) -> int64_t { + return (static_cast(val.raw_value()) * CLIPPER_SCALE) / (1LL << FP_FRACTION_BITS); + }; + + // 1. Переводим контур локации (NavigationArea) в формат Clipper2 + Paths64 subject; + Path64 outerPath; + for (const auto& pt : area.polygon) { + outerPath.push_back(Point64(toClipperCoord(pt.x), toClipperCoord(pt.y))); + } + subject.push_back(outerPath); + + // 2. Собираем все препятствия + Paths64 clips; + for (const auto& obs : obstaclePolygons) { + if (obs.polygon.size() < 3) continue; + Path64 clipPath; + for (const auto& pt : obs.polygon) { + clipPath.push_back(Point64(toClipperCoord(pt.x), toClipperCoord(pt.y))); + } + clips.push_back(clipPath); + } + + // 3. Булево объединение пересекающихся препятствий и вычитание их из локации + // Clipper автоматически объединяет пересекающиеся полигоны внутри Difference + PolyTree64 solTree; + Clipper64 clipper; + clipper.AddSubject(subject); + clipper.AddClip(clips); + //clipper.Execute(ClipType::Difference, FillRule::NonZero, solTree); + clipper.Execute(ClipType::Difference, FillRule::EvenOdd, solTree); + + // 4. Подготовка данных для Constrained Delaunay Triangulation (CDT) + std::vector> cdtVertices; + std::vector cdtEdges; + + auto extractPolygons = [&](const PolyPath64* node, auto& selfRef) -> void { + if (!node->Polygon().empty()) { + const Path64& path = node->Polygon(); + std::size_t startIdx = cdtVertices.size(); + std::size_t count = path.size(); + + for (std::size_t i = 0; i < count; ++i) { + double x = static_cast(path[i].x) / CLIPPER_SCALE; + double y = static_cast(path[i].y) / CLIPPER_SCALE; + cdtVertices.push_back({ x, y }); + cdtEdges.push_back(CDT::Edge(startIdx + i, startIdx + (i + 1) % count)); + } + } + for (std::size_t i = 0; i < node->Count(); ++i) { + selfRef(node->Child(i), selfRef); + } + }; + + for (std::size_t i = 0; i < solTree.Count(); ++i) { + extractPolygons(solTree.Child(i), extractPolygons); + } + + if (cdtVertices.empty()) return; + + CDT::RemoveDuplicatesAndRemapEdges(cdtVertices, cdtEdges); + + // 5. Выполняем триангуляцию и удаляем треугольники внутри дыр (препятствий) + CDT::Triangulation cdt; + cdt.insertVertices(cdtVertices); + cdt.insertEdges(cdtEdges); + cdt.eraseOuterTrianglesAndHoles(); + + // 6. Конвертируем результат в детерминированный граф NavTriangle (Q32.32) + const auto& triangles = cdt.triangles; + const auto& vertices = cdt.vertices; + navMesh.resize(triangles.size()); + + for (std::size_t i = 0; i < triangles.size(); ++i) { + NavTriangle& navTri = navMesh[i]; + navTri.id = static_cast(i); + + for (int j = 0; j < 3; ++j) { + const auto& v = vertices[triangles[i].vertices[j]]; + navTri.vertices[j] = Vec2fp(fp(v.x), fp(v.y)); + + // Соседи в CDT хранятся напротив вершин, перепривязываем к ребрам (j, j+1) + auto neighborIdx = triangles[i].neighbors[j]; + navTri.neighbors[j] = (neighborIdx != CDT::noNeighbor) ? static_cast(neighborIdx) : -1; + } + + navTri.center = (navTri.vertices[0] + navTri.vertices[1] + navTri.vertices[2]) / fp(3); + } + + ready = !navMesh.empty(); + } + + std::vector PathFinder::findPathToNearest(const Vec3fp& start3D, const Vec3fp& end3D) const { + if (!ready || !area.available) return { start3D }; + + Vec2fp startPt(start3D.x, start3D.z); + Vec2fp targetPt(end3D.x, end3D.z); + + // 1. Находим треугольники для старта и финиша. + // Если точка попала в препятствие (вне NavMesh), ищем ближайшую точку на ребрах NavMesh + int startTri = findTriangleContaining(startPt); + if (startTri == -1) { + startPt = findClosestPointOnNavMesh(startPt, startTri); + } + + int endTri = findTriangleContaining(targetPt); + if (endTri == -1) { + targetPt = findClosestPointOnNavMesh(targetPt, endTri); + } + + if (startTri == -1 || endTri == -1) return { start3D }; + + // Если старт и конец в одном треугольнике — идем по прямой + if (startTri == endTri) { + return { + start3D, + Vec3fp(targetPt.x, floorY, targetPt.y) + }; + } + + // 2. Поиск коридора из треугольников через A* + std::vector triangleCorridor = runAStar(startTri, endTri, startPt, targetPt); + if (triangleCorridor.empty()) return { start3D }; + + // 3. Сглаживание пути через Funnel Algorithm (Натянутая нить) + std::vector smoothPath2D = stringPullFunnel(startPt, targetPt, triangleCorridor); + + // 4. Формируем выходной массив 3D точек + std::vector result; + result.reserve(smoothPath2D.size()); + for (const auto& pt : smoothPath2D) { + result.emplace_back(pt.x, floorY, pt.y); + } + return result; + } + + std::vector PathFinder::findPathToNearest(const Eigen::Vector3f& start, const Eigen::Vector3f& end) const { + auto pathFp = findPathToNearest(Vec3fp::fromEigen(start), Vec3fp::fromEigen(end)); + std::vector out; + out.reserve(pathFp.size()); + for (const auto& p : pathFp) out.push_back(p.toEigen()); + return out; + } + + bool PathFinder::isWalkable(const Vec3fp& point) const { + return findTriangleContaining(Vec2fp(point.x, point.z)) != -1; + } + + bool PathFinder::isWalkable(const Eigen::Vector3f& point) const { + return isWalkable(Vec3fp::fromEigen(point)); + } + + bool PathFinder::setAreaAvailable(const std::string& areaName, bool available) { + if (area.name == areaName) { + area.available = available; + return true; + } + return false; + } + + bool PathFinder::isReady() const { return ready; } + + const PathFinder::NavigationArea& PathFinder::getArea() const { return area; } + + void PathFinder::setArea(const NavigationArea& a) { area = a; } + + fp PathFinder::getFloorY() const { return floorY; } + + void PathFinder::setFloorY(fp y) { floorY = y; } + + void PathFinder::addObstaclePolygon(const ObstaclePolygon& polygon) { + obstaclePolygons.push_back(polygon); + } + + const std::vector& PathFinder::getObstaclePolygons() const { return obstaclePolygons; } + + bool PathFinder::saveConfig(const std::string& path) const { return true; } + + bool PathFinder::saveGrid(const std::string& path) const { return true; } + + Vec2fp PathFinder::clampToNavMesh(const Vec2fp& pt, int& outTriId) const { + return findClosestPointOnNavMesh(pt, outTriId); + } + + Vec2fp PathFinder::clampToNavMesh(const Vec2fp& pt) const { + int dummyTri = -1; + return findClosestPointOnNavMesh(pt, dummyTri); + } + + bool PathFinder::isPointInTriangle(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b, const Vec2fp& c) { + fp d1 = (b - a).cross(p - a); + fp d2 = (c - b).cross(p - b); + fp d3 = (a - c).cross(p - c); + + bool hasNeg = (d1 < fp(0)) || (d2 < fp(0)) || (d3 < fp(0)); + bool hasPos = (d1 > fp(0)) || (d2 > fp(0)) || (d3 > fp(0)); + return !(hasNeg && hasPos); + } + + int PathFinder::findTriangleContaining(const Vec2fp& pt) const { + for (const auto& tri : navMesh) { + if (isPointInTriangle(pt, tri.vertices[0], tri.vertices[1], tri.vertices[2])) { + return tri.id; + } + } + return -1; + } + + Vec2fp PathFinder::closestPointOnSegment(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b) { + Vec2fp ab = b - a; + fp lenSq = ab.lengthSq(); + if (lenSq == fp(0)) return a; + + fp t = (p - a).dot(ab) / lenSq; + t = std::max(fp(0), std::min(fp(1), t)); + return a + ab * t; + } + + Vec2fp PathFinder::findClosestPointOnNavMesh(const Vec2fp& pt, int& outTriId) const { + fp minDistSq = std::numeric_limits::max(); + Vec2fp bestPt = pt; + outTriId = -1; + + for (const auto& tri : navMesh) { + for (int i = 0; i < 3; ++i) { + // В CDT neighbors[i] — это сосед через ребро (vertices[i], vertices[(i + 1) % 3]) + if (tri.neighbors[i] == -1) { + const Vec2fp& a = tri.vertices[i]; + const Vec2fp& b = tri.vertices[(i + 1) % 3]; + Vec2fp proj = closestPointOnSegment(pt, a, b); + fp distSq = (pt - proj).lengthSq(); + + if (distSq < minDistSq) { + minDistSq = distSq; + bestPt = proj; + outTriId = tri.id; + } + } + } + } + return bestPt; + } + + std::vector PathFinder::runAStar(int startTri, int endTri, const Vec2fp& startPt, const Vec2fp& endPt) const { + struct NodeRecord { + int triId; + fp fScore; + // В случае равенства fScore сортируем по triId для 100% детерминизма + bool operator>(const NodeRecord& o) const { + if (fScore != o.fScore) return fScore > o.fScore; + return triId > o.triId; + } + }; + + std::priority_queue, std::greater> openSet; + std::vector gScore(navMesh.size(), std::numeric_limits::max()); + std::vector cameFrom(navMesh.size(), -1); + std::vector closed(navMesh.size(), false); + + gScore[startTri] = fp(0); + openSet.push({ startTri, (navMesh[startTri].center - endPt).length() }); + + while (!openSet.empty()) { + int current = openSet.top().triId; + openSet.pop(); + + if (current == endTri) { + std::vector path; + for (int curr = endTri; curr != -1; curr = cameFrom[curr]) { + path.push_back(curr); + } + std::reverse(path.begin(), path.end()); + return path; + } + + if (closed[current]) continue; + closed[current] = true; + + const NavTriangle& currTri = navMesh[current]; + for (int i = 0; i < 3; ++i) { + int neighbor = currTri.neighbors[i]; + if (neighbor == -1 || closed[neighbor]) continue; + + fp stepCost = (currTri.center - navMesh[neighbor].center).length(); + fp tentativeG = gScore[current] + stepCost; + + if (tentativeG < gScore[neighbor]) { + cameFrom[neighbor] = current; + gScore[neighbor] = tentativeG; + fp h = (navMesh[neighbor].center - endPt).length(); + openSet.push({ neighbor, tentativeG + h }); + } + } + } + return {}; + } + + PathFinder::Portal PathFinder::getPortal(int fromTriId, int toTriId) const { + const NavTriangle& t1 = navMesh[fromTriId]; + for (int i = 0; i < 3; ++i) { + if (t1.neighbors[i] == toTriId) { + // При обходе треугольника CCW и движении изнутри наружу через ребро i -> (i + 1) % 3: + // вершина i находится СЛЕВА, а вершина (i + 1) % 3 — СПРАВА + return Portal{ t1.vertices[i], t1.vertices[(i + 1) % 3] }; + } + } + return Portal{ t1.center, t1.center }; + } + + std::vector PathFinder::stringPullFunnel(const Vec2fp& startPt, const Vec2fp& endPt, const std::vector& corridor) const { + std::vector portals; + portals.reserve(corridor.size() + 2); + portals.push_back({ startPt, startPt }); + + for (std::size_t i = 0; i + 1 < corridor.size(); ++i) { + portals.push_back(getPortal(corridor[i], corridor[i + 1])); + } + portals.push_back({ endPt, endPt }); + + std::vector pts; + pts.push_back(startPt); + + Vec2fp portalApex = startPt; + Vec2fp portalLeft = startPt; + Vec2fp portalRight = startPt; + int apexIndex = 0, leftIndex = 0, rightIndex = 0; + + for (int i = 1; i < static_cast(portals.size()); ++i) { + const Vec2fp& left = portals[i].left; + const Vec2fp& right = portals[i].right; + + // Обновляем правую границу воронки + if ((portalRight - portalApex).cross(right - portalApex) <= fp(0)) { + if (portalApex == portalRight || (portalLeft - portalApex).cross(right - portalApex) > fp(0)) { + portalRight = right; + rightIndex = i; + } + else { + pts.push_back(portalLeft); + portalApex = portalLeft; + apexIndex = leftIndex; + portalLeft = portalApex; + portalRight = portalApex; + leftIndex = apexIndex; + rightIndex = apexIndex; + i = apexIndex; + continue; + } + } + + // Обновляем левую границу воронки + if ((portalLeft - portalApex).cross(left - portalApex) >= fp(0)) { + if (portalApex == portalLeft || (portalRight - portalApex).cross(left - portalApex) < fp(0)) { + portalLeft = left; + leftIndex = i; + } + else { + pts.push_back(portalRight); + portalApex = portalRight; + apexIndex = rightIndex; + portalLeft = portalApex; + portalRight = portalApex; + leftIndex = apexIndex; + rightIndex = apexIndex; + i = apexIndex; + continue; + } + } + } + + if (pts.back() != endPt) { + pts.push_back(endPt); + } + return pts; + } } // namespace FRG diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index a16b5f7..d38451f 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -1,708 +1,109 @@ -#pragma once +#pragma once + +#include "navigation/fpmutils.h" + +#include +#include -#include #include -#include +#include #include -#include -#include #include #include -#include #include -#ifdef min -#undef min -#endif -#ifdef max -#undef max -#endif -#include "../../thirdparty/fpm-1.1.0/include/fpm/fixed.hpp" -#include "../../thirdparty/fpm-1.1.0/include/fpm/math.hpp" - -// Сторонние библиотеки для геометрии (Header-only / легко подключаемые): -// 1. Clipper2: https://github.com/PolyClipping/Clipper2/releases/tag/Clipper2_2.0.1 -// 2. CDT (Constrained Delaunay Triangulation): https://github.com/artem-ogre/CDT - -#include "../../thirdparty/Clipper2-Clipper2_2.0.1/CPP/build/Release/include/clipper2/clipper.h" -#include "../../thirdparty/CDT-2.0.2/CDT/build/release/include/CDT.h" - namespace FRG { + class PathFinder { + public: + struct NavigationArea { + std::string name; + bool available = true; + std::vector polygon; + }; - //using fp = fpm::fixed; - inline constexpr unsigned int FP_FRACTION_BITS = 12; - using fp = fpm::fixed; + struct ObstaclePolygon { + std::string name; + std::vector polygon; + }; - struct Vec2fp { - fp x{ 0 }; - fp y{ 0 }; + struct NavTriangle { + int id = -1; + Vec2fp vertices[3]; + Vec2fp center; + int neighbors[3] = { -1, -1, -1 }; // Индексы соседних треугольников по ребрам (0-1, 1-2, 2-0) + }; - Vec2fp() = default; - Vec2fp(fp _x, fp _y) : x(_x), y(_y) {} + struct Portal { + Vec2fp left; + Vec2fp right; + }; - // Конвертация из float только на этапе загрузки/ввода - static Vec2fp fromFloat(float fx, float fy) { - return Vec2fp(fp(fx), fp(fy)); - } + // Масштаб для Clipper2 (так как Clipper работает с целыми int64_t, + // сохраняем точность в 10000 единиц на 1 метр) + static constexpr int64_t CLIPPER_SCALE = 10000; - Vec2fp operator+(const Vec2fp& o) const { return { x + o.x, y + o.y }; } - Vec2fp operator-(const Vec2fp& o) const { return { x - o.x, y - o.y }; } - Vec2fp operator*(fp s) const { return { x * s, y * s }; } - Vec2fp operator/(fp s) const { return { x / s, y / s }; } - bool operator==(const Vec2fp& o) const { return x == o.x && y == o.y; } - bool operator!=(const Vec2fp& o) const { return !(*this == o); } - fp dot(const Vec2fp& o) const { return x * o.x + y * o.y; } + void build(const std::string& configPath = "", const std::string& zipPath = ""); - // 2D Векторное произведение (Z-компонента): определяет, с какой стороны лежит точка - fp cross(const Vec2fp& o) const { return x * o.y - y * o.x; } + // Построение NavMesh: Объединение полигонов (Clipper2) + Триангуляция (CDT) + void generateNavMesh(); - fp lengthSq() const { return x * x + y * y; } - fp length() const { return fpm::sqrt(lengthSq()); } + // Основной метод поиска пути (плоскость XZ, Y — высота) + std::vector findPathToNearest(const Vec3fp& start3D, const Vec3fp& end3D) const; - Eigen::Vector2f toEigen() const { - return Eigen::Vector2f(static_cast(x), static_cast(y)); - } + // Перегрузка для совместимости со старым Eigen-интерфейсом + std::vector findPathToNearest(const Eigen::Vector3f& start, const Eigen::Vector3f& end) const; - static Vec2fp fromEigen(const Eigen::Vector2f& v) { - return Vec2fp(fp(v.x()), fp(v.y())); - } - }; + bool isWalkable(const Vec3fp& point) const; - struct Vec3fp { - fp x{ 0 }, y{ 0 }, z{ 0 }; - Vec3fp() = default; - Vec3fp(fp _x, fp _y, fp _z) : x(_x), y(_y), z(_z) {} + bool isWalkable(const Eigen::Vector3f& point) const; - static Vec3fp fromEigen(const Eigen::Vector3f& v) { - return { fp(v.x()), fp(v.y()), fp(v.z()) }; - } - Eigen::Vector3f toEigen() const { - return Eigen::Vector3f(static_cast(x), static_cast(y), static_cast(z)); - } + bool setAreaAvailable(const std::string& areaName, bool available); - fp normSquared() const - { - return x * x + y * y + z * z; - } - Vec3fp operator/(fp value) const - { - if (value == static_cast(0.0)) - { - throw std::runtime_error("Vec3fp error: division by zero"); - return Vec3fp(); - } - Vec3fp r; - r.x = x / value; - r.y = y / value; - r.z = z / value; - return r; - } - Vec3fp operator*(const fp value) const - { - Vec3fp r; - r.x = x * value; - r.y = y * value; - r.z = z * value; - return r; - } - }; + bool isReady() const; + const NavigationArea& getArea() const; + void setArea(const NavigationArea& a); - Vec3fp operator-(Vec3fp a, Vec3fp b); - Vec3fp operator+(Vec3fp a, Vec3fp b); + fp getFloorY() const; + void setFloorY(fp y); - inline std::vector toEigenVector(const std::vector& src) { - std::vector dst; - dst.reserve(src.size()); - for (const auto& pt : src) { - dst.emplace_back(static_cast(pt.x), static_cast(pt.y)); - } - return dst; - } + void addObstaclePolygon(const ObstaclePolygon& polygon); + const std::vector& getObstaclePolygons() const; - inline std::vector fromEigenVector(const std::vector& src) { - std::vector dst; - dst.reserve(src.size()); - for (const auto& pt : src) { - dst.emplace_back(FRG::fp(pt.x()), FRG::fp(pt.y())); - } - return dst; - } + bool saveConfig(const std::string& path) const; + bool saveGrid(const std::string& path) const; - class PathFinder { - public: - struct NavigationArea { - std::string name; - bool available = true; - std::vector polygon; - }; + // Внутри public: класса PathFinder + Vec2fp clampToNavMesh(const Vec2fp& pt, int& outTriId) const; - struct ObstaclePolygon { - std::string name; - std::vector polygon; - }; + // Удобная перегрузка без outTriId, если индекс треугольника не нужен: + Vec2fp clampToNavMesh(const Vec2fp& pt) const; - struct NavTriangle { - int id = -1; - Vec2fp vertices[3]; - Vec2fp center; - int neighbors[3] = { -1, -1, -1 }; // Индексы соседних треугольников по ребрам (0-1, 1-2, 2-0) - }; + protected: + NavigationArea area; + std::vector obstaclePolygons; + std::vector navMesh; + fp floorY{ 0 }; + bool ready = false; - struct Portal { - Vec2fp left; - Vec2fp right; - }; + // Проверка нахождения точки внутри треугольника (через знаки векторных произведений) + static bool isPointInTriangle(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b, const Vec2fp& c); - // Масштаб для Clipper2 (так как Clipper работает с целыми int64_t, - // сохраняем точность в 10000 единиц на 1 метр) - static constexpr int64_t CLIPPER_SCALE = 10000; + int findTriangleContaining(const Vec2fp& pt) const; - void build(const std::string& configPath = "", const std::string& zipPath = "") { - // Здесь может быть парсинг configPath в area и obstaclePolygons. - // После заполнения геометрии строим NavMesh: - area.name = "main"; - area.polygon = { - Vec2fp::fromFloat(-100.f, -100.f), - Vec2fp::fromFloat(-100.f, 100.f), - Vec2fp::fromFloat(100.f, 100.f), - Vec2fp::fromFloat(100.f, -100.f) - }; + // Проекция точки на отрезок AB + static Vec2fp closestPointOnSegment(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b); - area.available = true; + Vec2fp findClosestPointOnNavMesh(const Vec2fp& pt, int& outTriId) const; - obstaclePolygons.push_back({ - "p1", {Vec2fp::fromFloat(-10.f, -10.f), - Vec2fp::fromFloat(-10.f, 10.f), - Vec2fp::fromFloat(10.f, 10.f), - Vec2fp::fromFloat(10.f, -10.f)} - }); + // Поиск пути A* по центрам треугольников + std::vector runAStar(int startTri, int endTri, const Vec2fp& startPt, const Vec2fp& endPt) const; - //ready = true; + // Извлечение общих вершин (портала) между двумя соседними треугольниками + Portal getPortal(int fromTriId, int toTriId) const; - generateNavMesh(); - } - - // Построение NavMesh: Объединение полигонов (Clipper2) + Триангуляция (CDT) - void generateNavMesh() { - navMesh.clear(); - if (area.polygon.size() < 3) return; - - using namespace Clipper2Lib; - - // Вспомогательная лямбда: безопасно переводит fp в координаты Clipper2 (int64_t) - auto toClipperCoord = [](fp val) -> int64_t { - return (static_cast(val.raw_value()) * CLIPPER_SCALE) / (1LL << FP_FRACTION_BITS); - }; - - // 1. Переводим контур локации (NavigationArea) в формат Clipper2 - Paths64 subject; - Path64 outerPath; - for (const auto& pt : area.polygon) { - outerPath.push_back(Point64(toClipperCoord(pt.x), toClipperCoord(pt.y))); - } - subject.push_back(outerPath); - - // 2. Собираем все препятствия - Paths64 clips; - for (const auto& obs : obstaclePolygons) { - if (obs.polygon.size() < 3) continue; - Path64 clipPath; - for (const auto& pt : obs.polygon) { - clipPath.push_back(Point64(toClipperCoord(pt.x), toClipperCoord(pt.y))); - } - clips.push_back(clipPath); - } - - // 3. Булево объединение пересекающихся препятствий и вычитание их из локации - // Clipper автоматически объединяет пересекающиеся полигоны внутри Difference - PolyTree64 solTree; - Clipper64 clipper; - clipper.AddSubject(subject); - clipper.AddClip(clips); - //clipper.Execute(ClipType::Difference, FillRule::NonZero, solTree); - clipper.Execute(ClipType::Difference, FillRule::EvenOdd, solTree); - - // 4. Подготовка данных для Constrained Delaunay Triangulation (CDT) - std::vector> cdtVertices; - std::vector cdtEdges; - - auto extractPolygons = [&](const PolyPath64* node, auto& selfRef) -> void { - if (!node->Polygon().empty()) { - const Path64& path = node->Polygon(); - std::size_t startIdx = cdtVertices.size(); - std::size_t count = path.size(); - - for (std::size_t i = 0; i < count; ++i) { - double x = static_cast(path[i].x) / CLIPPER_SCALE; - double y = static_cast(path[i].y) / CLIPPER_SCALE; - cdtVertices.push_back({ x, y }); - cdtEdges.push_back(CDT::Edge(startIdx + i, startIdx + (i + 1) % count)); - } - } - for (std::size_t i = 0; i < node->Count(); ++i) { - selfRef(node->Child(i), selfRef); - } - }; - - for (std::size_t i = 0; i < solTree.Count(); ++i) { - extractPolygons(solTree.Child(i), extractPolygons); - } - - if (cdtVertices.empty()) return; - - CDT::RemoveDuplicatesAndRemapEdges(cdtVertices, cdtEdges); - - // 5. Выполняем триангуляцию и удаляем треугольники внутри дыр (препятствий) - CDT::Triangulation cdt; - cdt.insertVertices(cdtVertices); - cdt.insertEdges(cdtEdges); - cdt.eraseOuterTrianglesAndHoles(); - - // 6. Конвертируем результат в детерминированный граф NavTriangle (Q32.32) - const auto& triangles = cdt.triangles; - const auto& vertices = cdt.vertices; - navMesh.resize(triangles.size()); - - for (std::size_t i = 0; i < triangles.size(); ++i) { - NavTriangle& navTri = navMesh[i]; - navTri.id = static_cast(i); - - for (int j = 0; j < 3; ++j) { - const auto& v = vertices[triangles[i].vertices[j]]; - navTri.vertices[j] = Vec2fp(fp(v.x), fp(v.y)); - - // Соседи в CDT хранятся напротив вершин, перепривязываем к ребрам (j, j+1) - auto neighborIdx = triangles[i].neighbors[j]; - navTri.neighbors[j] = (neighborIdx != CDT::noNeighbor) ? static_cast(neighborIdx) : -1; - } - - navTri.center = (navTri.vertices[0] + navTri.vertices[1] + navTri.vertices[2]) / fp(3); - } - - ready = !navMesh.empty(); - } - - // Основной метод поиска пути (плоскость XZ, Y — высота) - std::vector findPathToNearest(const Vec3fp& start3D, const Vec3fp& end3D) const { - if (!ready || !area.available) return { start3D }; - - Vec2fp startPt(start3D.x, start3D.z); - Vec2fp targetPt(end3D.x, end3D.z); - - // 1. Находим треугольники для старта и финиша. - // Если точка попала в препятствие (вне NavMesh), ищем ближайшую точку на ребрах NavMesh - int startTri = findTriangleContaining(startPt); - if (startTri == -1) { - startPt = findClosestPointOnNavMesh(startPt, startTri); - } - - int endTri = findTriangleContaining(targetPt); - if (endTri == -1) { - targetPt = findClosestPointOnNavMesh(targetPt, endTri); - } - - if (startTri == -1 || endTri == -1) return { start3D }; - - // Если старт и конец в одном треугольнике — идем по прямой - if (startTri == endTri) { - return { - start3D, - Vec3fp(targetPt.x, floorY, targetPt.y) - }; - } - - // 2. Поиск коридора из треугольников через A* - std::vector triangleCorridor = runAStar(startTri, endTri, startPt, targetPt); - if (triangleCorridor.empty()) return { start3D }; - - // 3. Сглаживание пути через Funnel Algorithm (Натянутая нить) - std::vector smoothPath2D = stringPullFunnel(startPt, targetPt, triangleCorridor); - - // 4. Формируем выходной массив 3D точек - std::vector result; - result.reserve(smoothPath2D.size()); - for (const auto& pt : smoothPath2D) { - result.emplace_back(pt.x, floorY, pt.y); - } - return result; - } - - // Перегрузка для совместимости со старым Eigen-интерфейсом - std::vector findPathToNearest(const Eigen::Vector3f& start, const Eigen::Vector3f& end) const { - auto pathFp = findPathToNearest(Vec3fp::fromEigen(start), Vec3fp::fromEigen(end)); - std::vector out; - out.reserve(pathFp.size()); - for (const auto& p : pathFp) out.push_back(p.toEigen()); - return out; - } - - bool isWalkable(const Vec3fp& point) const { - return findTriangleContaining(Vec2fp(point.x, point.z)) != -1; - } - - bool isWalkable(const Eigen::Vector3f& point) const { - return isWalkable(Vec3fp::fromEigen(point)); - } - - bool setAreaAvailable(const std::string& areaName, bool available) { - if (area.name == areaName) { - area.available = available; - return true; - } - return false; - } - - bool isReady() const { return ready; } - const NavigationArea& getArea() const { return area; } - void setArea(const NavigationArea& a) { area = a; } - - fp getFloorY() const { return floorY; } - void setFloorY(fp y) { floorY = y; } - - void addObstaclePolygon(const ObstaclePolygon& polygon) { - obstaclePolygons.push_back(polygon); - } - const std::vector& getObstaclePolygons() const { return obstaclePolygons; } - - bool saveConfig(const std::string& path) const { return true; } - bool saveGrid(const std::string& path) const { return true; } - - // Внутри public: класса PathFinder - Vec2fp clampToNavMesh(const Vec2fp& pt, int& outTriId) const { - return findClosestPointOnNavMesh(pt, outTriId); - } - - // Удобная перегрузка без outTriId, если индекс треугольника не нужен: - Vec2fp clampToNavMesh(const Vec2fp& pt) const { - int dummyTri = -1; - return findClosestPointOnNavMesh(pt, dummyTri); - } - - protected: - NavigationArea area; - std::vector obstaclePolygons; - std::vector navMesh; - fp floorY{ 0 }; - bool ready = false; - - // Проверка нахождения точки внутри треугольника (через знаки векторных произведений) - static bool isPointInTriangle(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b, const Vec2fp& c) { - fp d1 = (b - a).cross(p - a); - fp d2 = (c - b).cross(p - b); - fp d3 = (a - c).cross(p - c); - - bool hasNeg = (d1 < fp(0)) || (d2 < fp(0)) || (d3 < fp(0)); - bool hasPos = (d1 > fp(0)) || (d2 > fp(0)) || (d3 > fp(0)); - return !(hasNeg && hasPos); - } - - int findTriangleContaining(const Vec2fp& pt) const { - for (const auto& tri : navMesh) { - if (isPointInTriangle(pt, tri.vertices[0], tri.vertices[1], tri.vertices[2])) { - return tri.id; - } - } - return -1; - } - - // Проекция точки на отрезок AB - static Vec2fp closestPointOnSegment(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b) { - Vec2fp ab = b - a; - fp lenSq = ab.lengthSq(); - if (lenSq == fp(0)) return a; - - fp t = (p - a).dot(ab) / lenSq; - t = std::max(fp(0), std::min(fp(1), t)); - return a + ab * t; - } - - Vec2fp findClosestPointOnNavMesh(const Vec2fp& pt, int& outTriId) const { - fp minDistSq = std::numeric_limits::max(); - Vec2fp bestPt = pt; - outTriId = -1; - - for (const auto& tri : navMesh) { - for (int i = 0; i < 3; ++i) { - // В CDT neighbors[i] — это сосед через ребро (vertices[i], vertices[(i + 1) % 3]) - if (tri.neighbors[i] == -1) { - const Vec2fp& a = tri.vertices[i]; - const Vec2fp& b = tri.vertices[(i + 1) % 3]; - Vec2fp proj = closestPointOnSegment(pt, a, b); - fp distSq = (pt - proj).lengthSq(); - - if (distSq < minDistSq) { - minDistSq = distSq; - bestPt = proj; - outTriId = tri.id; - } - } - } - } - return bestPt; - } - - // Поиск пути A* по центрам треугольников - std::vector runAStar(int startTri, int endTri, const Vec2fp& startPt, const Vec2fp& endPt) const { - struct NodeRecord { - int triId; - fp fScore; - // В случае равенства fScore сортируем по triId для 100% детерминизма - bool operator>(const NodeRecord& o) const { - if (fScore != o.fScore) return fScore > o.fScore; - return triId > o.triId; - } - }; - - std::priority_queue, std::greater> openSet; - std::vector gScore(navMesh.size(), std::numeric_limits::max()); - std::vector cameFrom(navMesh.size(), -1); - std::vector closed(navMesh.size(), false); - - gScore[startTri] = fp(0); - openSet.push({ startTri, (navMesh[startTri].center - endPt).length() }); - - while (!openSet.empty()) { - int current = openSet.top().triId; - openSet.pop(); - - if (current == endTri) { - std::vector path; - for (int curr = endTri; curr != -1; curr = cameFrom[curr]) { - path.push_back(curr); - } - std::reverse(path.begin(), path.end()); - return path; - } - - if (closed[current]) continue; - closed[current] = true; - - const NavTriangle& currTri = navMesh[current]; - for (int i = 0; i < 3; ++i) { - int neighbor = currTri.neighbors[i]; - if (neighbor == -1 || closed[neighbor]) continue; - - fp stepCost = (currTri.center - navMesh[neighbor].center).length(); - fp tentativeG = gScore[current] + stepCost; - - if (tentativeG < gScore[neighbor]) { - cameFrom[neighbor] = current; - gScore[neighbor] = tentativeG; - fp h = (navMesh[neighbor].center - endPt).length(); - openSet.push({ neighbor, tentativeG + h }); - } - } - } - return {}; - } - - // Извлечение общих вершин (портала) между двумя соседними треугольниками -// Извлечение общих вершин (портала) между двумя соседними треугольниками - Portal getPortal(int fromTriId, int toTriId) const { - const NavTriangle& t1 = navMesh[fromTriId]; - for (int i = 0; i < 3; ++i) { - if (t1.neighbors[i] == toTriId) { - // При обходе треугольника CCW и движении изнутри наружу через ребро i -> (i + 1) % 3: - // вершина i находится СЛЕВА, а вершина (i + 1) % 3 — СПРАВА - return Portal{ t1.vertices[i], t1.vertices[(i + 1) % 3] }; - } - } - return Portal{ t1.center, t1.center }; - } - - // Алгоритм натянутой нити (Simple Stupid Funnel Algorithm) - std::vector stringPullFunnel(const Vec2fp& startPt, const Vec2fp& endPt, const std::vector& corridor) const { - std::vector portals; - portals.reserve(corridor.size() + 2); - portals.push_back({ startPt, startPt }); - - for (std::size_t i = 0; i + 1 < corridor.size(); ++i) { - portals.push_back(getPortal(corridor[i], corridor[i + 1])); - } - portals.push_back({ endPt, endPt }); - - std::vector pts; - pts.push_back(startPt); - - Vec2fp portalApex = startPt; - Vec2fp portalLeft = startPt; - Vec2fp portalRight = startPt; - int apexIndex = 0, leftIndex = 0, rightIndex = 0; - - for (int i = 1; i < static_cast(portals.size()); ++i) { - const Vec2fp& left = portals[i].left; - const Vec2fp& right = portals[i].right; - - // Обновляем правую границу воронки - if ((portalRight - portalApex).cross(right - portalApex) <= fp(0)) { - if (portalApex == portalRight || (portalLeft - portalApex).cross(right - portalApex) > fp(0)) { - portalRight = right; - rightIndex = i; - } - else { - pts.push_back(portalLeft); - portalApex = portalLeft; - apexIndex = leftIndex; - portalLeft = portalApex; - portalRight = portalApex; - leftIndex = apexIndex; - rightIndex = apexIndex; - i = apexIndex; - continue; - } - } - - // Обновляем левую границу воронки - if ((portalLeft - portalApex).cross(left - portalApex) >= fp(0)) { - if (portalApex == portalLeft || (portalRight - portalApex).cross(left - portalApex) < fp(0)) { - portalLeft = left; - leftIndex = i; - } - else { - pts.push_back(portalRight); - portalApex = portalRight; - apexIndex = rightIndex; - portalLeft = portalApex; - portalRight = portalApex; - leftIndex = apexIndex; - rightIndex = apexIndex; - i = apexIndex; - continue; - } - } - } - - if (pts.back() != endPt) { - pts.push_back(endPt); - } - return pts; - } - }; - - -class PathFinderOld { -public: - - struct ObstaclePolygon { - std::string name; - std::vector polygon; - }; - - struct DynamicObstacle { - Eigen::Vector3f position = Eigen::Vector3f::Zero(); - float radius = 0.0f; + // Алгоритм натянутой нити (Simple Stupid Funnel Algorithm) + std::vector stringPullFunnel(const Vec2fp& startPt, const Vec2fp& endPt, const std::vector& corridor) const; }; - struct Cell { - int x = 0; - int z = 0; - }; - - struct NavigationArea { - std::string name; - bool available = true; - std::vector polygon; - }; - - void build(const std::string& configPath, - const std::string& zipPath = ""); - - std::vector findPath(const Eigen::Vector3f& start, - const Eigen::Vector3f& end) const; - - /*std::vector findPath(const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& dynamicObstacles) const; -*/ - // Like findPath, but when the destination is unreachable the character - // walks as close as possible instead of not moving at all. - std::vector findPathToNearest(const Eigen::Vector3f& start, - const Eigen::Vector3f& end) const; - /*std::vector findPathToNearest(const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& dynamicObstacles) const; - */ - bool setAreaAvailable(const std::string& areaName, bool available); - bool isReady() const { return ready; } - bool isWalkable(const Eigen::Vector3f& point) const; - const std::vector& getAreas() const { return areas; } - const std::vector& getObstaclePolygons() const { return obstaclePolygons; } - float getFloorY() const { return floorY; } - - void addObstaclePolygon(const ObstaclePolygon& polygon); - bool saveConfig(const std::string& path) const; - bool saveGrid(const std::string& path) const; - -private: - - float cellSize = 0.4f; - float agentRadius = 0.45f; - float floorY = 0.0f; - float objectPadding = 0.25f; - float boundaryPadding = 0.0f; - - 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 obstaclePolygons; - std::vector walkable; - std::vector areas; - - // Reused by synchronous authoritative path queries. Generation stamps avoid - // copying/clearing the complete map for every dynamic-obstacle search. - mutable std::vector dynamicBlockedGeneration; - mutable std::uint32_t dynamicGeneration = 0; - mutable std::vector searchCost; - mutable std::vector searchCameFrom; - mutable std::vector searchVisitedGeneration; - mutable std::uint32_t searchGeneration = 0; - mutable std::shared_ptr queryMutex = - std::make_shared(); - - void loadConfig(const std::string& configPath, const std::string& zipPath); - bool loadGrid(const std::string& path, const std::string& zipPath); - void resetGridBounds(); - void rebuildWalkableGrid(); - void markAvailableAreasWalkable(); - void markObstaclePolygonsBlocked(); - - 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); - - bool isCellWalkable(const Cell& cell, const std::vector& walkableGrid) const; - bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector& walkableGrid) const; - bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector& walkableGrid) const; - std::vector smoothCells(const std::vector& cells, const std::vector& walkableGrid) const; - - std::uint32_t buildDynamicOverlay( - const std::vector& dynamicObstacles) const; - bool isCellWalkableOverlay(const Cell& cell, std::uint32_t generation) const; - bool findNearestWalkableCellOverlay( - const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const; - bool hasLineOfSightOverlay( - const Cell& from, const Cell& to, std::uint32_t generation) const; - std::vector smoothCellsOverlay( - const std::vector& cells, std::uint32_t generation) const; - std::vector runSearch( - const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - std::uint32_t obstacleGeneration, - bool nearestReachable) const; -}; - } // namespace FRG diff --git a/src/navigation/fpmutils.cpp b/src/navigation/fpmutils.cpp new file mode 100644 index 0000000..03cf8f4 --- /dev/null +++ b/src/navigation/fpmutils.cpp @@ -0,0 +1,134 @@ +#include "navigation/fpmutils.h" + +#include + +namespace FRG { + +Vec2fp Vec2fp::fromFloat(float fx, float fy) +{ + return Vec2fp(fp(fx), fp(fy)); +} + +Vec2fp Vec2fp::operator+(const Vec2fp& other) const +{ + return {x + other.x, y + other.y}; +} + +Vec2fp Vec2fp::operator-(const Vec2fp& other) const +{ + return {x - other.x, y - other.y}; +} + +Vec2fp Vec2fp::operator*(fp scalar) const +{ + return {x * scalar, y * scalar}; +} + +Vec2fp Vec2fp::operator/(fp scalar) const +{ + return {x / scalar, y / scalar}; +} + +bool Vec2fp::operator==(const Vec2fp& other) const +{ + return x == other.x && y == other.y; +} + +bool Vec2fp::operator!=(const Vec2fp& other) const +{ + return !(*this == other); +} + +fp Vec2fp::dot(const Vec2fp& other) const +{ + return x * other.x + y * other.y; +} + +fp Vec2fp::cross(const Vec2fp& other) const +{ + return x * other.y - y * other.x; +} + +fp Vec2fp::lengthSq() const +{ + return x * x + y * y; +} + +fp Vec2fp::length() const +{ + return fpm::sqrt(lengthSq()); +} + +Eigen::Vector2f Vec2fp::toEigen() const +{ + return Eigen::Vector2f(static_cast(x), static_cast(y)); +} + +Vec2fp Vec2fp::fromEigen(const Eigen::Vector2f& value) +{ + return Vec2fp(fp(value.x()), fp(value.y())); +} + +Vec3fp Vec3fp::fromEigen(const Eigen::Vector3f& value) +{ + return {fp(value.x()), fp(value.y()), fp(value.z())}; +} + +Eigen::Vector3f Vec3fp::toEigen() const +{ + return Eigen::Vector3f( + static_cast(x), static_cast(y), static_cast(z)); +} + +fp Vec3fp::normSquared() const +{ + return x * x + y * y + z * z; +} + +Vec3fp Vec3fp::operator/(fp value) const +{ + if (value == static_cast(0.0)) { + throw std::runtime_error("Vec3fp error: division by zero"); + } + return {x / value, y / value, z / value}; +} + +Vec3fp Vec3fp::operator*(fp value) const +{ + return {x * value, y * value, z * value}; +} + +Vec3fp operator-(Vec3fp lhs, Vec3fp rhs) +{ + return {lhs.x - rhs.x, lhs.y - rhs.y, lhs.z - rhs.z}; +} + +Vec3fp operator+(Vec3fp lhs, Vec3fp rhs) +{ + return {lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z}; +} + +std::vector toEigenVector( + const std::vector& source) +{ + std::vector result; + result.reserve(source.size()); + for (const auto& point : source) { + result.emplace_back( + static_cast(point.x), static_cast(point.y)); + } + return result; +} + +std::vector fromEigenVector( + const std::vector& source) +{ + std::vector result; + result.reserve(source.size()); + for (const auto& point : source) { + result.emplace_back(fp(point.x()), fp(point.y())); + } + return result; +} + +} // namespace FRG diff --git a/src/navigation/fpmutils.h b/src/navigation/fpmutils.h new file mode 100644 index 0000000..50468c8 --- /dev/null +++ b/src/navigation/fpmutils.h @@ -0,0 +1,67 @@ +#pragma once + +#include +#include +#include + +#ifdef min +#undef min +#endif +#ifdef max +#undef max +#endif + +#include +#include + +namespace FRG { + +inline constexpr unsigned int FP_FRACTION_BITS = 12; +using fp = fpm::fixed; + +struct Vec2fp { + fp x{0}; + fp y{0}; + + Vec2fp() = default; + Vec2fp(fp _x, fp _y) : x(_x), y(_y) {} + + static Vec2fp fromFloat(float fx, float fy); + + Vec2fp operator+(const Vec2fp& other) const; + Vec2fp operator-(const Vec2fp& other) const; + Vec2fp operator*(fp scalar) const; + Vec2fp operator/(fp scalar) const; + bool operator==(const Vec2fp& other) const; + bool operator!=(const Vec2fp& other) const; + fp dot(const Vec2fp& other) const; + fp cross(const Vec2fp& other) const; + fp lengthSq() const; + fp length() const; + + Eigen::Vector2f toEigen() const; + static Vec2fp fromEigen(const Eigen::Vector2f& value); +}; + +struct Vec3fp { + fp x{0}; + fp y{0}; + fp z{0}; + + Vec3fp() = default; + Vec3fp(fp _x, fp _y, fp _z) : x(_x), y(_y), z(_z) {} + + static Vec3fp fromEigen(const Eigen::Vector3f& value); + Eigen::Vector3f toEigen() const; + fp normSquared() const; + Vec3fp operator/(fp value) const; + Vec3fp operator*(fp value) const; +}; + +Vec3fp operator-(Vec3fp lhs, Vec3fp rhs); +Vec3fp operator+(Vec3fp lhs, Vec3fp rhs); + +std::vector toEigenVector(const std::vector& source); +std::vector fromEigenVector(const std::vector& source); + +} // namespace FRG