Fixing bugs

src/CharacterState.h

@@ -116,7 +116,8 @@ public:
     Eigen::Quaternionf modelCorrectionRotation = Eigen::Quaternionf::Identity();
     bool  useGpuSkinning  = true;
 
-    fp collisionRadius = static_cast<fp>(0.4f); // Радиус капсулы персонажа
+    //fp collisionRadius = static_cast<fp>(0.4f); // Радиус капсулы персонажа
+    fp collisionRadius = static_cast<fp>(kCharacterAgentRadius); // Радиус капсулы персонажа
 
     // Векторы скорости в плоскости XZ (сериализовать в JSON не обязательно, если считаются каждый тик, 
     // но для строгого lockstep-снапшота velocity лучше сохранять)

src/navigation/PathFinder.cpp

@@ -114,6 +114,7 @@ namespace FRG {
 	}
 
 	void PathFinder::generateNavMesh() {
+		ready = false;
 		navMesh.clear();
 		if (area.polygon.size() < 3) return;
 
@@ -143,16 +144,37 @@ namespace FRG {
 			clips.push_back(clipPath);
 		}
 
-		// 3. Булево объединение пересекающихся препятствий и вычитание их из локации
+		// 3. Вычитаем препятствия из контура локации, чтобы получить чистую геометрию проходимой зоны
-		// Clipper автоматически объединяет пересекающиеся полигоны внутри Difference
+		Paths64 walkablePaths;
-		PolyTree64 solTree;
+		{
 			Clipper64 clipper;
 			clipper.AddSubject(subject);
 			clipper.AddClip(clips);
-		//clipper.Execute(ClipType::Difference, FillRule::NonZero, solTree);
+			clipper.Execute(ClipType::Difference, FillRule::EvenOdd, walkablePaths);
-		clipper.Execute(ClipType::Difference, FillRule::EvenOdd, solTree);
+		}
 
-		// 4. Подготовка данных для Constrained Delaunay Triangulation (CDT)
+		// 4. Сужаем проходимую область внутрь на радиус персонажа (Minkowski erosion)
+		const double radiusScaled = static_cast<double>(kCharacterAgentRadius) * CLIPPER_SCALE;
+		if (radiusScaled > 0.0) {
+			// JoinType::Miter с miter_limit = 2.0 аккуратно срезает слишком острые пики на углах
+			walkablePaths = InflatePaths(
+				walkablePaths,
+				-radiusScaled,
+				JoinType::Miter,
+				EndType::Polygon,
+				2.0
+			);
+		}
+
+		// Повторно прогоняем через PolyTree64, чтобы получить корректную иерархию внешних контуров и дыр для CDT
+		PolyTree64 solTree;
+		{
+			Clipper64 treeBuilder;
+			treeBuilder.AddSubject(walkablePaths);
+			treeBuilder.Execute(ClipType::Union, FillRule::NonZero, solTree);
+		}
+
+		// 5. Подготовка данных для Constrained Delaunay Triangulation (CDT)
 		std::vector<CDT::V2d<double>> cdtVertices;
 		std::vector<CDT::Edge> cdtEdges;
 
@@ -182,13 +204,13 @@ namespace FRG {
 
 		CDT::RemoveDuplicatesAndRemapEdges(cdtVertices, cdtEdges);
 
-		// 5. Выполняем триангуляцию и удаляем треугольники внутри дыр (препятствий)
+		// 6. Выполняем триангуляцию и удаляем треугольники внутри дыр (препятствий)
 		CDT::Triangulation<double> cdt;
 		cdt.insertVertices(cdtVertices);
 		cdt.insertEdges(cdtEdges);
 		cdt.eraseOuterTrianglesAndHoles();
 
-		// 6. Конвертируем результат в детерминированный граф NavTriangle (Q32.32)
+		// 7. Конвертируем результат в детерминированный граф NavTriangle
 		const auto& triangles = cdt.triangles;
 		const auto& vertices = cdt.vertices;
 		navMesh.resize(triangles.size());
@@ -201,7 +223,6 @@ namespace FRG {
 				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;
 			}
@@ -209,6 +230,30 @@ namespace FRG {
 			navTri.center = (navTri.vertices[0] + navTri.vertices[1] + navTri.vertices[2]) / fp(3);
 		}
 
+		// 8. Размечаем компоненты связности (острова) через BFS
+		int currentComponent = 0;
+		std::queue<int> bfsQueue;
+		for (std::size_t i = 0; i < navMesh.size(); ++i) {
+			if (navMesh[i].componentId != -1) continue;
+
+			navMesh[i].componentId = currentComponent;
+			bfsQueue.push(static_cast<int>(i));
+
+			while (!bfsQueue.empty()) {
+				int curr = bfsQueue.front();
+				bfsQueue.pop();
+
+				for (int j = 0; j < 3; ++j) {
+					int neighbor = navMesh[curr].neighbors[j];
+					if (neighbor != -1 && navMesh[neighbor].componentId == -1) {
+						navMesh[neighbor].componentId = currentComponent;
+						bfsQueue.push(neighbor);
+					}
+				}
+			}
+			++currentComponent;
+		}
+
 		ready = !navMesh.empty();
 	}
 
@@ -218,21 +263,26 @@ namespace FRG {
 		Vec2fp startPt(start3D.x, start3D.z);
 		Vec2fp targetPt(end3D.x, end3D.z);
 
-		// 1. Находим треугольники для старта и финиша.
+		// 1. Находим треугольник старта
-		// Если точка попала в препятствие (вне NavMesh), ищем ближайшую точку на ребрах NavMesh
 		int startTri = findTriangleContaining(startPt);
 		if (startTri == -1) {
-			startPt = findClosestPointOnNavMesh(startPt, startTri);
+			startPt = findClosestPointOnNavMesh(startPt, startTri, -1);
 		}
+		if (startTri == -1) return { start3D };
 
+		const int startComponent = navMesh[startTri].componentId;
+
+		// 2. Находим треугольник финиша.
+		// Если точка попала в препятствие ИЛИ в изолированную зону (другой остров),
+		// ищем ближайшую доступную точку именно на границе острова startComponent!
 		int endTri = findTriangleContaining(targetPt);
-		if (endTri == -1) {
+		if (endTri == -1 || navMesh[endTri].componentId != startComponent) {
-			targetPt = findClosestPointOnNavMesh(targetPt, endTri);
+			targetPt = findClosestPointOnNavMesh(targetPt, endTri, startComponent);
 		}
 
-		if (startTri == -1 || endTri == -1) return { start3D };
+		if (endTri == -1) return { start3D };
 
-		// Если старт и конец в одном треугольнике — идем по прямой
+		// Если старт и конец оказались в одном треугольнике — идем по прямой
 		if (startTri == endTri) {
 			return {
 				start3D,
@@ -240,14 +290,14 @@ namespace FRG {
 			};
 		}
 
-		// 2. Поиск коридора из треугольников через A*
+		// 3. Поиск коридора из треугольников через A*
 		std::vector<int> triangleCorridor = runAStar(startTri, endTri, startPt, targetPt);
 		if (triangleCorridor.empty()) return { start3D };
 
-		// 3. Сглаживание пути через Funnel Algorithm (Натянутая нить)
+		// 4. Сглаживание пути через Funnel Algorithm (Натянутая нить)
 		std::vector<Vec2fp> smoothPath2D = stringPullFunnel(startPt, targetPt, triangleCorridor);
 
-		// 4. Формируем выходной массив 3D точек
+		// 5. Формируем выходной массив 3D точек
 		std::vector<Vec3fp> result;
 		result.reserve(smoothPath2D.size());
 		for (const auto& pt : smoothPath2D) {
@@ -338,12 +388,18 @@ namespace FRG {
 		return a + ab * t;
 	}
 
-	Vec2fp PathFinder::findClosestPointOnNavMesh(const Vec2fp& pt, int& outTriId) const {
+	Vec2fp PathFinder::findClosestPointOnNavMesh(
+		const Vec2fp& pt, int& outTriId, int requiredComponent) const
+	{
 		fp minDistSq = std::numeric_limits<fp>::max();
 		Vec2fp bestPt = pt;
 		outTriId = -1;
 
 		for (const auto& tri : navMesh) {
+			if (requiredComponent != -1 && tri.componentId != requiredComponent) {
+				continue;
+			}
+
 			for (int i = 0; i < 3; ++i) {
 				// В CDT neighbors[i] — это сосед через ребро (vertices[i], vertices[(i + 1) % 3])
 				if (tri.neighbors[i] == -1) {

src/navigation/PathFinder.h

@@ -14,6 +14,8 @@
 
 namespace FRG {
 
+	inline constexpr float kCharacterAgentRadius = 0.42f;
+
 	class PathFinder {
 	public:
 		struct NavigationArea {
@@ -29,6 +31,7 @@ namespace FRG {
 
 		struct NavTriangle {
 			int id = -1;
+			int componentId = -1; // ID острова связности
 			Vec2fp vertices[3];
 			Vec2fp center;
 			int neighbors[3] = { -1, -1, -1 };// Индексы соседних треугольников по ребрам (0-1, 1-2, 2-0)
@@ -94,7 +97,7 @@ namespace FRG {
 		// Проекция точки на отрезок AB
 		static Vec2fp closestPointOnSegment(const Vec2fp& p, const Vec2fp& a, const Vec2fp& b);
 
-		Vec2fp findClosestPointOnNavMesh(const Vec2fp& pt, int& outTriId) const;
+		Vec2fp findClosestPointOnNavMesh(const Vec2fp& pt, int& outTriId, int requiredComponent = -1) const;
 
 		// Поиск пути A* по центрам треугольников
 		std::vector<int> runAStar(int startTri, int endTri, const Vec2fp& startPt, const Vec2fp& endPt) const;