diff --git a/src/CharacterState.h b/src/CharacterState.h index ded174c..a703745 100644 --- a/src/CharacterState.h +++ b/src/CharacterState.h @@ -116,7 +116,8 @@ public: Eigen::Quaternionf modelCorrectionRotation = Eigen::Quaternionf::Identity(); bool useGpuSkinning = true; - fp collisionRadius = static_cast(0.4f); // Радиус капсулы персонажа + //fp collisionRadius = static_cast(0.4f); // Радиус капсулы персонажа + fp collisionRadius = static_cast(kCharacterAgentRadius); // Радиус капсулы персонажа // Векторы скорости в плоскости XZ (сериализовать в JSON не обязательно, если считаются каждый тик, // но для строгого lockstep-снапшота velocity лучше сохранять) diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 48c2826..9517840 100644 --- a/src/navigation/PathFinder.cpp +++ b/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. Булево объединение пересекающихся препятствий и вычитание их из локации - // 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); + // 3. Вычитаем препятствия из контура локации, чтобы получить чистую геометрию проходимой зоны + Paths64 walkablePaths; + { + Clipper64 clipper; + clipper.AddSubject(subject); + clipper.AddClip(clips); + clipper.Execute(ClipType::Difference, FillRule::EvenOdd, walkablePaths); + } - // 4. Подготовка данных для Constrained Delaunay Triangulation (CDT) + // 4. Сужаем проходимую область внутрь на радиус персонажа (Minkowski erosion) + const double radiusScaled = static_cast(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> cdtVertices; std::vector cdtEdges; @@ -182,13 +204,13 @@ namespace FRG { CDT::RemoveDuplicatesAndRemapEdges(cdtVertices, cdtEdges); - // 5. Выполняем триангуляцию и удаляем треугольники внутри дыр (препятствий) + // 6. Выполняем триангуляцию и удаляем треугольники внутри дыр (препятствий) CDT::Triangulation 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(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 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(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. Находим треугольники для старта и финиша. - // Если точка попала в препятствие (вне NavMesh), ищем ближайшую точку на ребрах NavMesh + // 1. Находим треугольник старта 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) { - targetPt = findClosestPointOnNavMesh(targetPt, endTri); + if (endTri == -1 || navMesh[endTri].componentId != startComponent) { + 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 triangleCorridor = runAStar(startTri, endTri, startPt, targetPt); if (triangleCorridor.empty()) return { start3D }; - // 3. Сглаживание пути через Funnel Algorithm (Натянутая нить) + // 4. Сглаживание пути через Funnel Algorithm (Натянутая нить) std::vector smoothPath2D = stringPullFunnel(startPt, targetPt, triangleCorridor); - // 4. Формируем выходной массив 3D точек + // 5. Формируем выходной массив 3D точек std::vector 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::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) { diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index d38451f..fcc9af8 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -14,6 +14,8 @@ namespace FRG { + inline constexpr float kCharacterAgentRadius = 0.42f; + class PathFinder { public: struct NavigationArea { @@ -29,9 +31,10 @@ 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) + int neighbors[3] = { -1, -1, -1 };// Индексы соседних треугольников по ребрам (0-1, 1-2, 2-0) }; struct Portal { @@ -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 runAStar(int startTri, int endTri, const Vec2fp& startPt, const Vec2fp& endPt) const;