Refactoring

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()

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()

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}")

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()

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}")

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
 )
 

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

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)

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)

server/tests/PathFinderTests.cpp

@@ -1,269 +1,35 @@
-#include "CharacterState.h"
-#include "LocationGameState.h"
 #include "navigation/PathFinder.h"
-#include <algorithm>
-#include <chrono>
-#include <cmath>
+
+#include <Eigen/Core>
 #include <cstdlib>
-#include <filesystem>
-#include <fstream>
 #include <iostream>
-#include <memory>
-#include <string>
-#include <vector>
 
 namespace {
-
 int fail(const char* message)
 {
     std::cout << "PathFinderTests: " << message << std::endl;
     return EXIT_FAILURE;
 }
-
-std::filesystem::path writeGrid(
-	const std::string& name, int width, int depth, bool corridor)
-{
-	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;
 }
 
-bool samePath(
-	const std::vector<Eigen::Vector3f>& lhs,
-	const std::vector<Eigen::Vector3f>& rhs)
+int main()
 {
-	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;
-}
-
-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();
-}
-
-void runBenchmark(FRG::PathFinder& pathFinder, int queries)
-{
-	std::vector<FRG::PathFinder::DynamicObstacle> obstacles;
-	for (int i = 0; i < queries; ++i) {
-		const float x = 4.5f + static_cast<float>((i * 7) % 70);
-		const float z = 4.5f + static_cast<float>((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<float>(i % 30));
-		const Eigen::Vector3f to(
-			78.5f, 0.f, 78.5f - static_cast<float>(i % 30));
-		waypointCount +=
-			pathFinder.findPathToNearest(from, to, obstacles).size();
-	}
-	const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
-		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");
+    pathFinder.build();
+    if (!pathFinder.isReady()) return fail("Navmesh was not built");
 
-	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 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");
 
-	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 clamped = pathFinder.clampToNavMesh(FRG::Vec2fp::fromFloat(150.f, 150.f));
+    if (static_cast<float>(clamped.x) > 100.01f ||
+        static_cast<float>(clamped.y) > 100.01f)
+        return fail("Clamping returned a point outside the navmesh");
 
-	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<FRG::CharacterMovementObstacle> 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<FRG::CharacterMovementObstacle> 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<FRG::CharacterState>();
-	stationary->enabled = true;
-	stationary->hp = 10.f;
-	auto moving = std::make_shared<FRG::CharacterState>();
-	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<FRG::CharacterState>();
-	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<FRG::CharacterState>();
-	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);
+    std::cout << "PathFinderTests: OK" << std::endl;
     return EXIT_SUCCESS;
 }

src/LocationState.cpp

@@ -122,21 +122,6 @@ void loadSinglePlayerPersistence(
 	}
 
 
-	// Entity ownership stays above this navigation-only layer.
-	/**td::vector<Eigen::Vector3f> LocationNetworkState::planPathExceptMyself(
-		const Eigen::Vector3f& start,
-		const Eigen::Vector3f& end,
-		const std::string& entityId,
-		const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles) const
-	{
-		(void)entityId;
-		if (activeNavigationIndex < 0 ||
-			activeNavigationIndex >= static_cast<int>(navigationMaps.size()))
-			return {};
-		return navigationMaps[activeNavigationIndex].findPathToNearest(
-			start, end, dynamicObstacles);
-	}*/
-
 	void LocationNetworkState::initNavigation(
 		const std::vector<std::string>& paths, const std::string& zipFile)
 	{

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<PathFinder> navigationMaps;
-    /*std::vector<Eigen::Vector3f> planPathExceptMyself(
-        const Eigen::Vector3f& start,
-        const Eigen::Vector3f& end,
-        const std::string& entityId,
-        const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles = {}) const;*/
     void initNavigation(
         const std::vector<std::string>& paths,
         const std::string& zipFile = std::string());

src/navigation/PathFinder.h

@@ -1,134 +1,19 @@
-#pragma once
+#pragma once
+
+#include "navigation/fpmutils.h"
+
+#include <clipper2/clipper.h>
+#include <CDT.h>
 
-#include <Eigen/Core>
 #include <algorithm>
-#include <cstdint>
+#include <functional>
 #include <limits>
-#include <memory>
-#include <mutex>
 #include <queue>
 #include <string>
-#include <unordered_map>
 #include <vector>
 
-#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 {
 
-
-    //using fp = fpm::fixed<std::int32_t, std::int64_t, 16>;
-    inline constexpr unsigned int FP_FRACTION_BITS = 12;
-    using fp = fpm::fixed<std::int32_t, std::int64_t, FP_FRACTION_BITS>;
-
-    struct Vec2fp {
-        fp x{ 0 };
-        fp y{ 0 };
-
-        Vec2fp() = default;
-        Vec2fp(fp _x, fp _y) : x(_x), y(_y) {}
-
-        // Конвертация из float только на этапе загрузки/ввода
-        static Vec2fp fromFloat(float fx, float fy) {
-            return Vec2fp(fp(fx), fp(fy));
-        }
-
-        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; }
-
-        // 2D Векторное произведение (Z-компонента): определяет, с какой стороны лежит точка
-        fp cross(const Vec2fp& o) const { return x * o.y - y * o.x; }
-
-        fp lengthSq() const { return x * x + y * y; }
-        fp length() const { return fpm::sqrt(lengthSq()); }
-
-        Eigen::Vector2f toEigen() const {
-            return Eigen::Vector2f(static_cast<float>(x), static_cast<float>(y));
-        }
-
-        static Vec2fp fromEigen(const Eigen::Vector2f& v) {
-            return Vec2fp(fp(v.x()), fp(v.y()));
-        }
-    };
-
-    struct Vec3fp {
-        fp x{ 0 }, y{ 0 }, z{ 0 };
-        Vec3fp() = default;
-        Vec3fp(fp _x, fp _y, fp _z) : x(_x), y(_y), z(_z) {}
-
-        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<float>(x), static_cast<float>(y), static_cast<float>(z));
-        }
-
-        fp normSquared() const
-        {
-            return x * x + y * y + z * z;
-        }
-        Vec3fp operator/(fp value) const
-        {
-            if (value == static_cast<fp>(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;
-        }
-    };
-
-    Vec3fp operator-(Vec3fp a, Vec3fp b);
-    Vec3fp operator+(Vec3fp a, Vec3fp b);
-
-    inline std::vector<Eigen::Vector2f> toEigenVector(const std::vector<FRG::Vec2fp>& src) {
-        std::vector<Eigen::Vector2f> dst;
-        dst.reserve(src.size());
-        for (const auto& pt : src) {
-            dst.emplace_back(static_cast<float>(pt.x), static_cast<float>(pt.y));
-        }
-        return dst;
-    }
-
-    inline std::vector<FRG::Vec2fp> fromEigenVector(const std::vector<Eigen::Vector2f>& src) {
-        std::vector<FRG::Vec2fp> dst;
-        dst.reserve(src.size());
-        for (const auto& pt : src) {
-            dst.emplace_back(FRG::fp(pt.x()), FRG::fp(pt.y()));
-        }
-        return dst;
-    }
-
 	class PathFinder {
 	public:
 		struct NavigationArea {
@@ -158,226 +43,41 @@ namespace FRG {
 		// сохраняем точность в 10000 единиц на 1 метр)
 		static constexpr int64_t CLIPPER_SCALE = 10000;
 
-        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)
-            };
-
-            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 build(const std::string& configPath = "", const std::string& zipPath = "");
 
 		// Построение 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<int64_t>(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<CDT::V2d<double>> cdtVertices;
-            std::vector<CDT::Edge> 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<double>(path[i].x) / CLIPPER_SCALE;
-                        double y = static_cast<double>(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<double> 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<int>(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<int>(neighborIdx) : -1;
-                }
-
-                navTri.center = (navTri.vertices[0] + navTri.vertices[1] + navTri.vertices[2]) / fp(3);
-            }
-
-            ready = !navMesh.empty();
-        }
+		void generateNavMesh();
 
 		// Основной метод поиска пути (плоскость XZ, Y — высота)
-        std::vector<Vec3fp> 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<int> triangleCorridor = runAStar(startTri, endTri, startPt, targetPt);
-            if (triangleCorridor.empty()) return { start3D };
-
-            // 3. Сглаживание пути через Funnel Algorithm (Натянутая нить)
-            std::vector<Vec2fp> smoothPath2D = stringPullFunnel(startPt, targetPt, triangleCorridor);
-
-            // 4. Формируем выходной массив 3D точек
-            std::vector<Vec3fp> result;
-            result.reserve(smoothPath2D.size());
-            for (const auto& pt : smoothPath2D) {
-                result.emplace_back(pt.x, floorY, pt.y);
-            }
-            return result;
-        }
+		std::vector<Vec3fp> findPathToNearest(const Vec3fp& start3D, const Vec3fp& end3D) const;
 
 		// Перегрузка для совместимости со старым Eigen-интерфейсом
-        std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start, const Eigen::Vector3f& end) const {
-            auto pathFp = findPathToNearest(Vec3fp::fromEigen(start), Vec3fp::fromEigen(end));
-            std::vector<Eigen::Vector3f> out;
-            out.reserve(pathFp.size());
-            for (const auto& p : pathFp) out.push_back(p.toEigen());
-            return out;
-        }
+		std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start, const Eigen::Vector3f& end) const;
 
-        bool isWalkable(const Vec3fp& point) const {
-            return findTriangleContaining(Vec2fp(point.x, point.z)) != -1;
-        }
+		bool isWalkable(const Vec3fp& point) const;
 
-        bool isWalkable(const Eigen::Vector3f& point) const {
-            return isWalkable(Vec3fp::fromEigen(point));
-        }
+		bool isWalkable(const Eigen::Vector3f& point) const;
 
-        bool setAreaAvailable(const std::string& areaName, bool available) {
-            if (area.name == areaName) {
-                area.available = available;
-                return true;
-            }
-            return false;
-        }
+		bool setAreaAvailable(const std::string& areaName, bool available);
 
-        bool isReady() const { return ready; }
-        const NavigationArea& getArea() const { return area; }
-        void setArea(const NavigationArea& a) { area = a; }
+		bool isReady() const;
+		const NavigationArea& getArea() const;
+		void setArea(const NavigationArea& a);
 
-        fp getFloorY() const { return floorY; }
-        void setFloorY(fp y) { floorY = y; }
+		fp getFloorY() const;
+		void setFloorY(fp y);
 
-        void addObstaclePolygon(const ObstaclePolygon& polygon) {
-            obstaclePolygons.push_back(polygon);
-        }
-        const std::vector<ObstaclePolygon>& getObstaclePolygons() const { return obstaclePolygons; }
+		void addObstaclePolygon(const ObstaclePolygon& polygon);
+		const std::vector<ObstaclePolygon>& getObstaclePolygons() const;
 
-        bool saveConfig(const std::string& path) const { return true; }
-        bool saveGrid(const std::string& path) const { return true; }
+		bool saveConfig(const std::string& path) const;
+		bool saveGrid(const std::string& path) const;
 
 		// Внутри public: класса PathFinder
-        Vec2fp clampToNavMesh(const Vec2fp& pt, int& outTriId) const {
-            return findClosestPointOnNavMesh(pt, outTriId);
-        }
+		Vec2fp clampToNavMesh(const Vec2fp& pt, int& outTriId) const;
 
 		// Удобная перегрузка без outTriId, если индекс треугольника не нужен:
-        Vec2fp clampToNavMesh(const Vec2fp& pt) const {
-            int dummyTri = -1;
-            return findClosestPointOnNavMesh(pt, dummyTri);
-        }
+		Vec2fp clampToNavMesh(const Vec2fp& pt) const;
 
 	protected:
 		NavigationArea area;
@@ -387,322 +87,23 @@ namespace FRG {
 		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);
+		static bool isPointInTriangle(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b, const Vec2fp& 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;
-        }
+		int findTriangleContaining(const Vec2fp& pt) const;
 
 		// Проекция точки на отрезок 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;
+		static Vec2fp closestPointOnSegment(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b);
 
-            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<fp>::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;
-        }
+		Vec2fp findClosestPointOnNavMesh(const Vec2fp& pt, int& outTriId) const;
 
 		// Поиск пути A* по центрам треугольников
-        std::vector<int> 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<NodeRecord, std::vector<NodeRecord>, std::greater<NodeRecord>> openSet;
-            std::vector<fp> gScore(navMesh.size(), std::numeric_limits<fp>::max());
-            std::vector<int> cameFrom(navMesh.size(), -1);
-            std::vector<bool> 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<int> 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 {};
-        }
+		std::vector<int> runAStar(int startTri, int endTri, const Vec2fp& startPt, const Vec2fp& endPt) const;
 
 		// Извлечение общих вершин (портала) между двумя соседними треугольниками
-// Извлечение общих вершин (портала) между двумя соседними треугольниками
-        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 };
-        }
+		Portal getPortal(int fromTriId, int toTriId) const;
 
 		// Алгоритм натянутой нити (Simple Stupid Funnel Algorithm)
-        std::vector<Vec2fp> stringPullFunnel(const Vec2fp& startPt, const Vec2fp& endPt, const std::vector<int>& corridor) const {
-            std::vector<Portal> 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<Vec2fp> 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<int>(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;
-        }
+		std::vector<Vec2fp> stringPullFunnel(const Vec2fp& startPt, const Vec2fp& endPt, const std::vector<int>& corridor) const;
 	};
 
-
-class PathFinderOld {
-public:
-
-    struct ObstaclePolygon {
-        std::string name;
-        std::vector<Eigen::Vector2f> polygon;
-    };
-
-	struct DynamicObstacle {
-		Eigen::Vector3f position = Eigen::Vector3f::Zero();
-		float radius = 0.0f;
-	};
-
-    struct Cell {
-        int x = 0;
-        int z = 0;
-    };
-
-    struct NavigationArea {
-        std::string name;
-        bool available = true;
-        std::vector<Eigen::Vector2f> polygon;
-    };
-
-    void build(const std::string& configPath,
-               const std::string& zipPath = "");
-
-    std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
-                                          const Eigen::Vector3f& end) const;
-
-	/*std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
-	                                      const Eigen::Vector3f& end,
-	                                      const std::vector<DynamicObstacle>& 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<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
-	                                               const Eigen::Vector3f& end) const;
-	/*std::vector<Eigen::Vector3f> findPathToNearest(const Eigen::Vector3f& start,
-	                                               const Eigen::Vector3f& end,
-	                                               const std::vector<DynamicObstacle>& 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<NavigationArea>& getAreas() const { return areas; }
-    const std::vector<ObstaclePolygon>& 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<ObstaclePolygon> obstaclePolygons;
-    std::vector<unsigned char> walkable;
-    std::vector<NavigationArea> areas;
-
-    // Reused by synchronous authoritative path queries. Generation stamps avoid
-    // copying/clearing the complete map for every dynamic-obstacle search.
-    mutable std::vector<std::uint32_t> dynamicBlockedGeneration;
-    mutable std::uint32_t dynamicGeneration = 0;
-    mutable std::vector<int> searchCost;
-    mutable std::vector<int> searchCameFrom;
-    mutable std::vector<std::uint32_t> searchVisitedGeneration;
-    mutable std::uint32_t searchGeneration = 0;
-    mutable std::shared_ptr<std::mutex> queryMutex =
-        std::make_shared<std::mutex>();
-
-    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<Cell> smoothCells(const std::vector<Cell>& cells) const;
-
-    static bool pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon);
-
-	bool isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const;
-	bool findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const;
-	bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const;
-	std::vector<Cell> smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const;
-
-    std::uint32_t buildDynamicOverlay(
-        const std::vector<DynamicObstacle>& 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<Cell> smoothCellsOverlay(
-        const std::vector<Cell>& cells, std::uint32_t generation) const;
-    std::vector<Eigen::Vector3f> runSearch(
-        const Eigen::Vector3f& start,
-        const Eigen::Vector3f& end,
-        std::uint32_t obstacleGeneration,
-        bool nearestReachable) const;
-};
-
 } // namespace FRG

src/navigation/fpmutils.cpp

@@ -0,0 +1,134 @@
+#include "navigation/fpmutils.h"
+
+#include <stdexcept>
+
+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<float>(x), static_cast<float>(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<float>(x), static_cast<float>(y), static_cast<float>(z));
+}
+
+fp Vec3fp::normSquared() const
+{
+    return x * x + y * y + z * z;
+}
+
+Vec3fp Vec3fp::operator/(fp value) const
+{
+    if (value == static_cast<fp>(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<Eigen::Vector2f> toEigenVector(
+    const std::vector<Vec2fp>& source)
+{
+    std::vector<Eigen::Vector2f> result;
+    result.reserve(source.size());
+    for (const auto& point : source) {
+        result.emplace_back(
+            static_cast<float>(point.x), static_cast<float>(point.y));
+    }
+    return result;
+}
+
+std::vector<Vec2fp> fromEigenVector(
+    const std::vector<Eigen::Vector2f>& source)
+{
+    std::vector<Vec2fp> result;
+    result.reserve(source.size());
+    for (const auto& point : source) {
+        result.emplace_back(fp(point.x()), fp(point.y()));
+    }
+    return result;
+}
+
+} // namespace FRG

src/navigation/fpmutils.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <cstdint>
+#include <vector>
+
+#ifdef min
+#undef min
+#endif
+#ifdef max
+#undef max
+#endif
+
+#include <fpm/fixed.hpp>
+#include <fpm/math.hpp>
+
+namespace FRG {
+
+inline constexpr unsigned int FP_FRACTION_BITS = 12;
+using fp = fpm::fixed<std::int32_t, std::int64_t, FP_FRACTION_BITS>;
+
+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<Eigen::Vector2f> toEigenVector(const std::vector<Vec2fp>& source);
+std::vector<Vec2fp> fromEigenVector(const std::vector<Eigen::Vector2f>& source);
+
+} // namespace FRG