Working on pathfinding, testing the Q16.16 format

proj-windows/CMakeLists.txt

@@ -131,8 +131,14 @@ 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"
 )
@@ -162,6 +168,7 @@ target_link_libraries(ShadowOverBishkekDemo PRIVATE
     sol2_external_lib
     SDL2_mixer_external_lib
     openssl_ssl_external_lib
+	Clipper2.lib
 )
 
 # Линкуем OpenGL (Windows)

src/CharacterState.cpp

@@ -93,8 +93,7 @@ void CharacterState::setTarget(
     const Eigen::Vector3f& target,
     std::string callbackName,
     const LocationNetworkState& location,
-    const std::string& entityId,
+    const std::string& entityId)
-    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
 {
     const Eigen::Vector3f normalizedTarget(target.x(), 0.f, target.z());
     const bool sameRequestedTarget =
@@ -115,10 +114,9 @@ void CharacterState::setTarget(
     movementBlockedTicks = 0;
     navigationRetryTicks = 0;
     blockingCharacterId.clear();
-    pathWaypoints = location.navigationMaps.empty()
+    
-        ? std::vector<Eigen::Vector3f>{}
+    pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, target);
-        : location.planPathExceptMyself(
+
-            position, normalizedTarget, entityId, dynamicObstacles);
     currentWaypointIndex = 0;
     if (!pathWaypoints.empty()) {
         for (Eigen::Vector3f& waypoint : pathWaypoints) waypoint.y() = 0.f;
@@ -132,15 +130,13 @@ void CharacterState::setTarget(
 
 void CharacterState::forceReplan(
     const LocationNetworkState& location,
-    const std::string& entityId,
+    const std::string& entityId)
-    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
 {
     const Eigen::Vector3f normalizedTarget(
         requestedWalkTarget.x(), 0.f, requestedWalkTarget.z());
-    pathWaypoints = location.navigationMaps.empty()
+    
-        ? std::vector<Eigen::Vector3f>{}
+    pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, normalizedTarget);
-        : location.planPathExceptMyself(
+
-            position, normalizedTarget, entityId, dynamicObstacles);
     currentWaypointIndex = 0;
     movementBlockedTicks = 0;
     navigationRetryTicks = 0;

src/CharacterState.h

@@ -151,13 +151,9 @@ public:
     void                   setTarget(const Eigen::Vector3f& target,
                                      std::string callbackName,
                                      const LocationNetworkState& location,
-                                     const std::string& entityId,
+                                     const std::string& entityId);
-                                     const std::vector<PathFinder::DynamicObstacle>&
-                                         dynamicObstacles = {});
     void                   forceReplan(const LocationNetworkState& location,
-                                       const std::string& entityId,
+                                       const std::string& entityId);
-                                       const std::vector<PathFinder::DynamicObstacle>&
-                                           dynamicObstacles = {});
     void                   stopAndRotateToTarget(std::string characterId);
     void                   resetPlayerAfterDeath();
     void                   tryAttackCombo();

src/Location.cpp

@@ -851,8 +851,8 @@ namespace FRG
 						float t0 = (worldMin[axis] - rayOrigin[axis]) * invD;
 						float t1 = (worldMax[axis] - rayOrigin[axis]) * invD;
 						if (invD < 0.0f) std::swap(t0, t1);
-						tMin = max(tMin, t0);
+						tMin = std::max(tMin, t0);
-						tMax = min(tMax, t1);
+						tMax = std::min(tMax, t1);
 						if (tMax < tMin) { miss = true; break; }
 					}
 				}
@@ -1838,14 +1838,13 @@ namespace FRG
 			state->activeNavigationIndex >= static_cast<int>(state->navigationMaps.size())) {
 			return false;
 		}
-		const auto& areas =
+		const auto& area =
-			state->navigationMaps[state->activeNavigationIndex].getAreas();
+			state->navigationMaps[state->activeNavigationIndex].getArea();
-		const bool found = std::any_of(
+		if (area.name != areaName)
-			areas.begin(), areas.end(),
+		{
-			[&](const PathFinder::NavigationArea& area) {
+			return false;
-				return area.name == areaName;
+		}
-			});
+
-		if (!found) return false;
 		gameServer_.submit(SetNavigationAreaAvailableCommand{
 			locationId_, areaName, available});
 		return true;
@@ -2253,7 +2252,7 @@ namespace FRG
 			const float minInclination = M_PI * 30.f / 180.f;
 			const float maxInclination = M_PI * 0.5f;
 			localState_->cameraInclination =
-				max(minInclination, min(maxInclination, inclination));
+				std::max(minInclination, std::min(maxInclination, inclination));
 		}
 	}
 

src/LocationEditor.cpp

@@ -30,12 +30,12 @@ namespace FRG
             state->activeNavigationIndex >= static_cast<int>(state->navigationMaps.size())) return;
         const PathFinder& navigation =
             state->navigationMaps[state->activeNavigationIndex];
-        const float y = navigation.getFloorY() + 0.02f;
+        const float y = static_cast<float>(navigation.getFloorY()) + 0.02f;
         const Eigen::Vector3f red(1.0f, 0.0f, 0.0f);
         for (const auto& obs : navigation.getObstaclePolygons()) {
             if (obs.polygon.size() < 3) continue;
             VertexRenderStruct mesh;
-            mesh.data = CreatePolygonFloor(obs.polygon, y, red);
+            mesh.data = CreatePolygonFloor(toEigenVector(obs.polygon), y, red);
             mesh.RefreshVBO();
             navigationEditorNavMeshes.push_back(std::move(mesh));
         }
@@ -60,7 +60,7 @@ namespace FRG
         if (!state || state->activeNavigationIndex < 0 ||
             state->activeNavigationIndex >= static_cast<int>(state->navigationMaps.size())) return;
         const float y =
-            state->navigationMaps[state->activeNavigationIndex].getFloorY() + 0.05f;
+            static_cast<float>(state->navigationMaps[state->activeNavigationIndex].getFloorY()) + 0.05f;
         const float s = 0.2f;
 
         for (const auto& pt : navigationEditorPoints) {
@@ -133,7 +133,7 @@ namespace FRG
         PathFinder::ObstaclePolygon poly;
         poly.name = "editor_obstacle_" + std::to_string(++navigationEditorObstacleCounter);
         for (const auto& pt : navigationEditorPoints) {
-            poly.polygon.emplace_back(pt.x(), pt.z());
+            poly.polygon.emplace_back(static_cast<FRG::fp>(pt.x()), static_cast<FRG::fp>(pt.z()));
         }
 
         loc.gameServer_.submit(AddNavigationObstacleCommand{
@@ -146,10 +146,10 @@ namespace FRG
             if (!state || state->activeNavigationIndex < 0 ||
                 state->activeNavigationIndex >= static_cast<int>(state->navigationMaps.size())) return;
             const float y =
-                state->navigationMaps[state->activeNavigationIndex].getFloorY() + 0.02f;
+                static_cast<float>(state->navigationMaps[state->activeNavigationIndex].getFloorY()) + 0.02f;
             const Eigen::Vector3f red(1.0f, 0.0f, 0.0f);
             VertexRenderStruct mesh;
-            mesh.data = CreatePolygonFloor(poly.polygon, y, red);
+            mesh.data = CreatePolygonFloor(toEigenVector(poly.polygon), y, red);
             mesh.RefreshVBO();
             navigationEditorNavMeshes.push_back(std::move(mesh));
         }
@@ -379,9 +379,9 @@ namespace FRG
 
             auto& target = loc.interactiveObjects[selectedInteractiveObjectIndex];
             target.state.boundsMin = Eigen::Vector3f(
-                min(localA.x(), localB.x()), 0.f, min(localA.z(), localB.z()));
+                std::min(localA.x(), localB.x()), 0.f, std::min(localA.z(), localB.z()));
             target.state.boundsMax = Eigen::Vector3f(
-                max(localA.x(), localB.x()), 1.f, max(localA.z(), localB.z()));
+                std::max(localA.x(), localB.x()), 1.f, std::max(localA.z(), localB.z()));
 
             boundsClickCount = 0;
             buildInteractiveObjectBoundsMeshes();

src/LocationGameState.cpp

@@ -126,6 +126,7 @@ const CharacterState* LocationGameState::findCharacter(
 	return nullptr;
 }
 
+/*
 std::vector<PathFinder::DynamicObstacle> LocationGameState::collectPathObstacles(
 	const std::string& requesterId,
 	const PlayerStateMap& players,
@@ -154,7 +155,7 @@ std::vector<PathFinder::DynamicObstacle> LocationGameState::collectPathObstacles
 		obstacles.push_back({character->position, character->collisionRadius});
 	}
 	return obstacles;
-}
+}*/
 
 bool LocationGameState::setCharacterDestination(
 	const std::string& id,
@@ -166,8 +167,7 @@ bool LocationGameState::setCharacterDestination(
 	CharacterState* character = findCharacter(id, players);
 	if (!character || !location) return false;
 	character->setTarget(
-		target, std::move(callbackName), *location, id,
+		target, std::move(callbackName), *location, id);
-		collectPathObstacles(id, players, forcedObstacleId));
 	return true;
 }
 
@@ -193,8 +193,7 @@ bool LocationGameState::replanCharacter(
 			const CharacterState* target = findCharacter(selectedTarget, players);
 			if (target) {
 				character->setTarget(
-					target->position, {}, *location, id,
+					target->position, {}, *location, id);
-					collectPathObstacles(id, players, forcedObstacleId));
 				return true;
 			}
 		}
@@ -207,13 +206,12 @@ bool LocationGameState::replanCharacter(
 		if ((homeTarget - character->position).norm() >
 			CharacterState::kWalkThreshold) {
 			character->setTarget(
-				homeTarget, {}, *location, id,
+				homeTarget, {}, *location, id);
-				collectPathObstacles(id, players, forcedObstacleId));
 		}
 		return true;
 	}
 	character->forceReplan(
-		*location, id, collectPathObstacles(id, players, forcedObstacleId));
+		*location, id);
 	return true;
 }
 
@@ -435,7 +433,7 @@ LocationSimulationEvents LocationGameState::update(
 			}
 		}
 	}
-
+	/*
 	std::vector<CharacterMovementObstacle> movementObstacles;
 	movementObstacles.reserve(playerIds.size() + npcIds.size());
 	auto addMovementObstacle =
@@ -468,8 +466,9 @@ LocationSimulationEvents LocationGameState::update(
 	std::sort(
 		movementObstacles.begin(), movementObstacles.end(),
 		[](const CharacterMovementObstacle& lhs,
-		   const CharacterMovementObstacle& rhs) { return lhs.id < rhs.id; });
+		   const CharacterMovementObstacle& rhs) { return lhs.id < rhs.id; });*/
 
+	/*
 	auto advance = [&](const std::string& id, CharacterState& state) {
 		CharacterUpdateContext context;
 		context.characterId = id;
@@ -499,14 +498,36 @@ LocationSimulationEvents LocationGameState::update(
 			events.deathAnimationComplete || events.navigationRequested) {
 			result.characters.push_back({id, std::move(events)});
 		}
-	};
+	};*/
-
+	/*
 	for (const std::string& id : playerIds)
 		if (players.at(id)->enabled)
 			advance(id, *players.at(id));
 	for (const std::string& id : npcIds)
+		if (npcs.at(id)->enablesd)
+			advance(id, *npcs.at(id));*/
+
+	for (const std::string& id : playerIds)
+	{
+		if (players.at(id)->enabled)
+		{
+			CharacterUpdateContext context;
+			context.characterId = id;
+			auto& state = *players.at(id);
+			CharacterUpdateEvents events = state.update(deltaMs, context);
+			//advance(id, *players.at(id));
+		}
+	}
+
+	for (const std::string& id : npcIds)
+	{
 		if (npcs.at(id)->enabled)
-			advance(id, *npcs.at(id));
+		{
+			//advance(id, *npcs.at(id));
+		}
+	}
+
+
 
 	for (const CharacterSimulationEvent& source : result.characters) {
 		if (!source.events.attackImpact) continue;
@@ -523,6 +544,7 @@ LocationSimulationEvents LocationGameState::update(
 			source.events.attackDirection});
 	}
 
+	/*
 	auto player = players.find("player1");
 	if (!interactiveObjectsLocked &&
 		!location->targetInteractiveObjectId.empty() &&
@@ -555,7 +577,7 @@ LocationSimulationEvents LocationGameState::update(
 			result.npcInteractionId = location->targetInteractNpcId;
 			location->targetInteractNpcId.clear();
 		}
-	}
+	}*/
 
 	return result;
 }

src/LocationGameState.h

@@ -64,10 +64,10 @@ struct LocationGameState {
 	CharacterState* findCharacter(const std::string& id, PlayerStateMap& players);
 	const CharacterState* findCharacter(
 		const std::string& id, const PlayerStateMap& players) const;
-	std::vector<PathFinder::DynamicObstacle> collectPathObstacles(
+	/*std::vector<PathFinder::DynamicObstacle> collectPathObstacles(
 		const std::string& requesterId,
 		const PlayerStateMap& players,
-		const std::string& forcedObstacleId = {}) const;
+		const std::string& forcedObstacleId = {}) const;*/
 	bool setCharacterDestination(
 		const std::string& id,
 		const Eigen::Vector3f& target,

src/LocationState.cpp

@@ -123,7 +123,7 @@ void loadSinglePlayerPersistence(
 
 
 	// Entity ownership stays above this navigation-only layer.
-	std::vector<Eigen::Vector3f> LocationNetworkState::planPathExceptMyself(
+	/**td::vector<Eigen::Vector3f> LocationNetworkState::planPathExceptMyself(
 		const Eigen::Vector3f& start,
 		const Eigen::Vector3f& end,
 		const std::string& entityId,
@@ -135,7 +135,7 @@ void loadSinglePlayerPersistence(
 			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,11 @@ 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(
+    /*std::vector<Eigen::Vector3f> planPathExceptMyself(
         const Eigen::Vector3f& start,
         const Eigen::Vector3f& end,
         const std::string& entityId,
-        const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles = {}) const;
+        const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles = {}) const;*/
     void initNavigation(
         const std::vector<std::string>& paths,
         const std::string& zipFile = std::string());

src/MenuManager.cpp

@@ -17,10 +17,6 @@
 
 namespace FRG {
 
-#ifdef __EMSCRIPTEN__
-	//extern Game* Game::s_instance;
-#endif
-
 	// Localization
 	static const std::string EMPTY_LANGUAGE_RU = u8"(пусто)";
 	static const std::string EMPTY_LANGUAGE_EN = "(empty)";
@@ -81,7 +77,7 @@ namespace FRG {
 		pushWord(currentWord);
 		if (!currentLine.empty()) flushLine();
 
-		return max(1, lines);
+		return std::max(1, lines);
 	}
 
 	static std::string trimChatPreview(const std::string& msg, int maxChars = 22) {
@@ -2257,9 +2253,9 @@ namespace FRG {
 
 	float MenuManager::ToastEntry::currentAlpha() const {
 		switch (state) {
-		case State::FadeIn:  return min(timer / TOAST_FADE_MS,  1.0f);
+		case State::FadeIn:  return std::min(timer / TOAST_FADE_MS,  1.0f);
 		case State::Visible: return 1.0f;
-		case State::FadeOut: return max(1.0f - timer / TOAST_FADE_MS, 0.0f);
+		case State::FadeOut: return std::max(1.0f - timer / TOAST_FADE_MS, 0.0f);
 		}
 		return 0.0f;
 	}

src/game_server/IGameServer.h

@@ -7,6 +7,7 @@
 #include <string>
 #include <variant>
 #include <vector>
+#include "../navigation/PathFinder.h"
 
 namespace FRG {
 
@@ -110,7 +111,7 @@ struct SetNavigationAreaAvailableCommand {
 struct AddNavigationObstacleCommand {
 	std::string locationId;
 	std::string name;
-	std::vector<Eigen::Vector2f> polygon;
+	std::vector<Vec2fp> polygon;
 };
 struct AddInventoryItemCommand { std::string itemId; };
 struct RemoveInventoryItemCommand { std::string itemId; };

src/navigation/PathFinder.cpp

@@ -79,7 +79,7 @@ namespace {
     }
 }
 
-void PathFinder::build(const std::string& configPath,
+void PathFinderOld::build(const std::string& configPath,
                        const std::string& zipPath)
 {
     ready = false;
@@ -115,22 +115,23 @@ void PathFinder::build(const std::string& configPath,
               << ", cell=" << cellSize << ", areas=" << areas.size() << "\n";
 }
 
-std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
+std::vector<Eigen::Vector3f> PathFinderOld::findPath(const Eigen::Vector3f& start,
                                                   const Eigen::Vector3f& end) const
 {
-    const std::lock_guard<std::mutex> lock(*queryMutex);
+    //const std::lock_guard<std::mutex> lock(*queryMutex);
-    return runSearch(start, end, 0, false);
+    //return runSearch(start, end, 0, false);
+    return std::vector<Eigen::Vector3f>{start, end};
 }
 
-std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
+/*
+std::vector<Eigen::Vector3f> PathFinderOld::findPath(const Eigen::Vector3f& start,
                                                   const Eigen::Vector3f& end,
                                                   const std::vector<DynamicObstacle>& dynamicObstacles) const
 {
     const std::lock_guard<std::mutex> lock(*queryMutex);
     return runSearch(start, end, buildDynamicOverlay(dynamicObstacles), false);
-}
+}*/
-
+std::uint32_t PathFinderOld::buildDynamicOverlay(
-std::uint32_t PathFinder::buildDynamicOverlay(
     const std::vector<DynamicObstacle>& dynamicObstacles) const
 {
     const size_t cellCount = walkable.size();
@@ -172,7 +173,7 @@ std::uint32_t PathFinder::buildDynamicOverlay(
     return dynamicGeneration;
 }
 
-bool PathFinder::isCellWalkableOverlay(
+bool PathFinderOld::isCellWalkableOverlay(
     const Cell& cell, std::uint32_t generation) const
 {
     if (!isCellWalkable(cell)) return false;
@@ -180,7 +181,7 @@ bool PathFinder::isCellWalkableOverlay(
         dynamicBlockedGeneration[static_cast<size_t>(indexOf(cell))] != generation;
 }
 
-bool PathFinder::findNearestWalkableCellOverlay(
+bool PathFinderOld::findNearestWalkableCellOverlay(
     const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const
 {
     Cell origin;
@@ -212,7 +213,7 @@ bool PathFinder::findNearestWalkableCellOverlay(
     return false;
 }
 
-bool PathFinder::hasLineOfSightOverlay(
+bool PathFinderOld::hasLineOfSightOverlay(
     const Cell& from, const Cell& to, std::uint32_t generation) const
 {
     int x = from.x;
@@ -244,7 +245,7 @@ bool PathFinder::hasLineOfSightOverlay(
     }
 }
 
-std::vector<PathFinder::Cell> PathFinder::smoothCellsOverlay(
+std::vector<PathFinderOld::Cell> PathFinderOld::smoothCellsOverlay(
     const std::vector<Cell>& cells, std::uint32_t generation) const
 {
     if (cells.size() <= 2) return cells;
@@ -263,7 +264,7 @@ std::vector<PathFinder::Cell> PathFinder::smoothCellsOverlay(
     return result;
 }
 
-std::vector<Eigen::Vector3f> PathFinder::runSearch(
+std::vector<Eigen::Vector3f> PathFinderOld::runSearch(
     const Eigen::Vector3f& start,
     const Eigen::Vector3f& end,
     std::uint32_t obstacleGeneration,
@@ -385,23 +386,25 @@ std::vector<Eigen::Vector3f> PathFinder::runSearch(
     return path;
 }
 
-std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
+std::vector<Eigen::Vector3f> PathFinderOld::findPathToNearest(
     const Eigen::Vector3f& start, const Eigen::Vector3f& end) const
 {
-    const std::lock_guard<std::mutex> lock(*queryMutex);
+    //const std::lock_guard<std::mutex> lock(*queryMutex);
-    return runSearch(start, end, 0, true);
+   // return runSearch(start, end, 0, true);
+    return std::vector<Eigen::Vector3f>{start, end};
 }
 
-std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
+/*
+std::vector<Eigen::Vector3f> PathFinderOld::findPathToNearest(
     const Eigen::Vector3f& start, const Eigen::Vector3f& end,
     const std::vector<DynamicObstacle>& dynamicObstacles) const
 {
     const std::lock_guard<std::mutex> lock(*queryMutex);
     return runSearch(
         start, end, buildDynamicOverlay(dynamicObstacles), true);
-}
+}*/
 
-bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
+bool PathFinderOld::setAreaAvailable(const std::string& areaName, bool available)
 {
     bool found = false;
     bool changed = false;
@@ -420,13 +423,13 @@ bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)
     return found;
 }
 
-bool PathFinder::isWalkable(const Eigen::Vector3f& point) const
+bool PathFinderOld::isWalkable(const Eigen::Vector3f& point) const
 {
     Cell cell;
     return worldToCell(point, cell) && isCellWalkable(cell, walkable);
 }
 
-void PathFinder::addObstaclePolygon(const ObstaclePolygon& polygon)
+void PathFinderOld::addObstaclePolygon(const ObstaclePolygon& polygon)
 {
     obstaclePolygons.push_back(polygon);
     if (ready) {
@@ -434,7 +437,7 @@ void PathFinder::addObstaclePolygon(const ObstaclePolygon& polygon)
     }
 }
 
-bool PathFinder::saveConfig(const std::string& path) const
+bool PathFinderOld::saveConfig(const std::string& path) const
 {
     json root;
     root["cellSize"]        = cellSize;
@@ -481,7 +484,7 @@ bool PathFinder::saveConfig(const std::string& path) const
 	return saveJsonToFile(root, path);
 }
 
-bool PathFinder::saveGrid(const std::string& path) const
+bool PathFinderOld::saveGrid(const std::string& path) const
 {
 #if defined(_WIN32)
     if (!ready) return false;
@@ -510,7 +513,7 @@ bool PathFinder::saveGrid(const std::string& path) const
 #endif
 }
 
-bool PathFinder::loadGrid(const std::string& path, const std::string& zipPath)
+bool PathFinderOld::loadGrid(const std::string& path, const std::string& zipPath)
 {
     std::string content;
     try {
@@ -565,7 +568,7 @@ bool PathFinder::loadGrid(const std::string& path, const std::string& zipPath)
     return true;
 }
 
-void PathFinder::loadConfig(const std::string& configPath, const std::string& zipPath)
+void PathFinderOld::loadConfig(const std::string& configPath, const std::string& zipPath)
 {
     cellSize = 0.4f;
     agentRadius = 0.45f;
@@ -640,7 +643,7 @@ void PathFinder::loadConfig(const std::string& configPath, const std::string& zi
     boundaryPadding = (std::max)(boundaryPadding, 0.0f);
 }
 
-void PathFinder::resetGridBounds()
+void PathFinderOld::resetGridBounds()
 {
     if (areas.empty()) {
         gridWidth = 0;
@@ -682,14 +685,14 @@ void PathFinder::resetGridBounds()
     gridDepth = static_cast<int>(std::ceil((maxZ - minZ) / cellSize));
 }
 
-void PathFinder::rebuildWalkableGrid()
+void PathFinderOld::rebuildWalkableGrid()
 {
     walkable.assign(static_cast<size_t>(gridWidth * gridDepth), 0);
     markAvailableAreasWalkable();
     markObstaclePolygonsBlocked();
 }
 
-void PathFinder::markAvailableAreasWalkable()
+void PathFinderOld::markAvailableAreasWalkable()
 {
     const auto insideAvailableArea = [this](const Eigen::Vector2f& point) {
         for (const NavigationArea& area : areas) {
@@ -763,7 +766,7 @@ void PathFinder::markAvailableAreasWalkable()
     }
 }
 
-void PathFinder::markObstaclePolygonsBlocked()
+void PathFinderOld::markObstaclePolygonsBlocked()
 {
     if (obstaclePolygons.empty()) {
         return;
@@ -800,7 +803,7 @@ void PathFinder::markObstaclePolygonsBlocked()
     }
 }
 
-bool PathFinder::worldToCell(const Eigen::Vector3f& point, Cell& out) const
+bool PathFinderOld::worldToCell(const Eigen::Vector3f& point, Cell& out) const
 {
     if (gridWidth <= 0 || gridDepth <= 0) {
         out = {};
@@ -812,7 +815,7 @@ bool PathFinder::worldToCell(const Eigen::Vector3f& point, Cell& out) const
     return isInsideGrid(out);
 }
 
-Eigen::Vector3f PathFinder::cellCenter(const Cell& cell) const
+Eigen::Vector3f PathFinderOld::cellCenter(const Cell& cell) const
 {
     return {
         minX + (static_cast<float>(cell.x) + 0.5f) * cellSize,
@@ -821,17 +824,17 @@ Eigen::Vector3f PathFinder::cellCenter(const Cell& cell) const
     };
 }
 
-bool PathFinder::isInsideGrid(const Cell& cell) const
+bool PathFinderOld::isInsideGrid(const Cell& cell) const
 {
     return cell.x >= 0 && cell.z >= 0 && cell.x < gridWidth && cell.z < gridDepth;
 }
 
-bool PathFinder::isCellWalkable(const Cell& cell) const
+bool PathFinderOld::isCellWalkable(const Cell& cell) const
 {
     return isCellWalkable(cell, walkable);
 }
 
-bool PathFinder::isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const
+bool PathFinderOld::isCellWalkable(const Cell& cell, const std::vector<unsigned char>& walkableGrid) const
 {
     const int idx = indexOf(cell);
     return isInsideGrid(cell) &&
@@ -840,17 +843,17 @@ bool PathFinder::isCellWalkable(const Cell& cell, const std::vector<unsigned cha
            walkableGrid[static_cast<size_t>(idx)] != 0;
 }
 
-int PathFinder::indexOf(const Cell& cell) const
+int PathFinderOld::indexOf(const Cell& cell) const
 {
     return cell.z * gridWidth + cell.x;
 }
 
-bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const
+bool PathFinderOld::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out) const
 {
     return findNearestWalkableCell(point, out, walkable);
 }
 
-bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const
+bool PathFinderOld::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out, const std::vector<unsigned char>& walkableGrid) const
 {
     Cell origin;
     if (!worldToCell(point, origin)) {
@@ -885,12 +888,12 @@ bool PathFinder::findNearestWalkableCell(const Eigen::Vector3f& point, Cell& out
     return false;
 }
 
-bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to) const
+bool PathFinderOld::hasLineOfSight(const Cell& from, const Cell& to) const
 {
     return hasLineOfSight(from, to, walkable);
 }
 
-bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const
+bool PathFinderOld::hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const
 {
     const Eigen::Vector3f a = cellCenter(from);
     const Eigen::Vector3f b = cellCenter(to);
@@ -909,12 +912,12 @@ bool PathFinder::hasLineOfSight(const Cell& from, const Cell& to, const std::vec
     return true;
 }
 
-std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells) const
+std::vector<PathFinderOld::Cell> PathFinderOld::smoothCells(const std::vector<Cell>& cells) const
 {
     return smoothCells(cells, walkable);
 }
 
-std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const
+std::vector<PathFinderOld::Cell> PathFinderOld::smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const
 {
     if (cells.size() <= 2) {
         return cells;
@@ -937,7 +940,7 @@ std::vector<PathFinder::Cell> PathFinder::smoothCells(const std::vector<Cell>& c
     return result;
 }
 
-bool PathFinder::pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon)
+bool PathFinderOld::pointInPolygon(float x, float z, const std::vector<Eigen::Vector2f>& polygon)
 {
     bool inside = false;
     size_t previous = polygon.size() - 1;

src/navigation/PathFinder.h

@@ -1,15 +1,547 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <algorithm>
 #include <cstdint>
+#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 {
 
-class PathFinder {
+
+    //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));
+        }
+    };
+
+    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 {
+            std::string name;
+            bool available = true;
+            std::vector<Vec2fp> polygon;
+        };
+
+        struct ObstaclePolygon {
+            std::string name;
+            std::vector<Vec2fp> polygon;
+        };
+
+        struct NavTriangle {
+            int id = -1;
+            Vec2fp vertices[3];
+            Vec2fp center;
+            int neighbors[3] = { -1, -1, -1 }; // Индексы соседних треугольников по ребрам (0-1, 1-2, 2-0)
+        };
+
+        struct Portal {
+            Vec2fp left;
+            Vec2fp right;
+        };
+
+        // Масштаб для Clipper2 (так как Clipper работает с целыми int64_t, 
+        // сохраняем точность в 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();
+        }
+
+        // Построение 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();
+        }
+
+        // Основной метод поиска пути (плоскость 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;
+        }
+
+        // Перегрузка для совместимости со старым 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;
+        }
+
+        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<ObstaclePolygon>& getObstaclePolygons() const { return obstaclePolygons; }
+
+        bool saveConfig(const std::string& path) const { return true; }
+        bool saveGrid(const std::string& path) const { return true; }
+
+    protected:
+        NavigationArea area;
+        std::vector<ObstaclePolygon> obstaclePolygons;
+        std::vector<NavTriangle> 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<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;
+        }
+
+        // Поиск пути 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 {};
+        }
+
+        // Извлечение общих вершин (портала) между двумя соседними треугольниками
+// Извлечение общих вершин (портала) между двумя соседними треугольниками
+        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<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;
+        }
+    };
+
+
+class PathFinderOld {
 public:
 
     struct ObstaclePolygon {
@@ -39,18 +571,18 @@ public:
     std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
                                           const Eigen::Vector3f& end) const;
 
-	std::vector<Eigen::Vector3f> findPath(const Eigen::Vector3f& start,
+	/*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,
+	/*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;