From ea8f64c9404cab26c9474ac1c976d01f54bd995d Mon Sep 17 00:00:00 2001 From: Vladislav Khorev Date: Thu, 22 Oct 2026 23:02:25 +0600 Subject: [PATCH] Restore back old navigation system --- src/Character.cpp | 7 +- src/Character.h | 2 +- src/CharacterState.h | 3 +- src/Environment.cpp | 4 +- src/Location.cpp | 113 +++++++-------------- src/navigation/PathFinder.cpp | 185 +--------------------------------- src/navigation/PathFinder.h | 5 - 7 files changed, 44 insertions(+), 275 deletions(-) diff --git a/src/Character.cpp b/src/Character.cpp index f153c81..dffe04e 100644 --- a/src/Character.cpp +++ b/src/Character.cpp @@ -173,8 +173,8 @@ void Character::resetPlayerAfterDeath() { state.attack = 0; } -bool Character::forceReplan() { - if (!pathPlanner) return false; +void Character::forceReplan() { + if (!pathPlanner) return; const Eigen::Vector3f normalizedTarget(state.requestedWalkTarget.x(), 0.f, state.requestedWalkTarget.z()); state.pathWaypoints = pathPlanner(state.position, normalizedTarget); @@ -185,13 +185,12 @@ bool Character::forceReplan() { waypoint.y() = 0.f; } state.walkTarget = state.pathWaypoints.back(); - return true; + return; } state.walkTarget = Eigen::Vector3f(state.position.x(), 0.f, state.position.z()); state.onArrivedCallback = nullptr; state.onArrivedCallbackName.clear(); - return false; } void Character::setTexture(std::shared_ptr texture) { diff --git a/src/Character.h b/src/Character.h index 0a17d9f..39c0af7 100644 --- a/src/Character.h +++ b/src/Character.h @@ -59,7 +59,7 @@ public: void setTarget(const Eigen::Vector3f& target, std::function onArrived = nullptr, std::string callbackName = ""); void stopAndRotateToTarget(int npcId); - bool forceReplan(); + void forceReplan(); void stopInPlace() { state.stopInPlace(); } // ---- Reset ---- diff --git a/src/CharacterState.h b/src/CharacterState.h index fd7ea34..ffb55d5 100644 --- a/src/CharacterState.h +++ b/src/CharacterState.h @@ -103,8 +103,7 @@ public: float modelScale = 0.12f; Eigen::Quaternionf modelCorrectionRotation = Eigen::Quaternionf::Identity(); bool useGpuSkinning = true; - //float collisionRadius = 0.45f; - float collisionRadius = 0.65f; + float collisionRadius = 0.45f; float interactionRadius = 0.0f; // --- Pathfinding data (serializable parts) --- diff --git a/src/Environment.cpp b/src/Environment.cpp index ede7474..f714cfd 100644 --- a/src/Environment.cpp +++ b/src/Environment.cpp @@ -22,8 +22,8 @@ int Environment::width = CONST_DEFAULT_WIDTH; int Environment::height = CONST_DEFAULT_HEIGHT; float Environment::zoom = DEFAULT_ZOOM; -//bool Environment::enableLogging = false; -bool Environment::enableLogging = true; +bool Environment::enableLogging = false; +//bool Environment::enableLogging = true; SDL_Window* Environment::window = nullptr; diff --git a/src/Location.cpp b/src/Location.cpp index 43a0ac3..de5812f 100644 --- a/src/Location.cpp +++ b/src/Location.cpp @@ -450,16 +450,7 @@ namespace FRG const auto addCharacter = [&](const Character* other) { if (!other || other == self) return; if (other->getHp() <= 0.f || !other->state.enabled) return; - - // Проверяем, находятся ли персонажи в состоянии фактического столкновения - const float dist = (other->state.position - self->state.position).norm(); - const float collisionDist = self->state.collisionRadius + other->state.collisionRadius; - - // Добавляем epsilon (0.1f) для учета погрешностей при входе в зону коллизии - const bool isColliding = dist <= (collisionDist + 0.1f); - - // Игнорируем движущегося персонажа, только если мы с ним НЕ сталкиваемся в данный момент - if (other->isMoving() && !isColliding) return; + if (other->isMoving()) return; if (distancePointToSegmentXZ(other->state.position, start, end) > kDynamicObstacleInfluenceDist) { return; @@ -469,7 +460,7 @@ namespace FRG obs.position = Eigen::Vector3f(other->state.position.x(), navigation->getFloorY(), other->state.position.z()); obs.radius = (std::max)(0.0f, other->state.collisionRadius * 0.6f); dynamicObstacles.push_back(obs); - }; + }; addCharacter(player.get()); for (const auto& npc : npcs) { @@ -477,8 +468,8 @@ namespace FRG } return navigation->findPathToNearest(start, end, dynamicObstacles); - }; }; + }; if (player) { player->setPathPlanner(makePlanner(player.get())); @@ -600,9 +591,6 @@ namespace FRG } Character* Location::raycastNpcs(const Eigen::Vector3f& rayOrigin, const Eigen::Vector3f& rayDir, float maxDistance) { - - //return nullptr; - // Every NPC is treated as a vertical cylinder: radius 1.0m, height 1.85m, // base at npc->position (the model's foot). Intersection = circle hit in the // XZ plane, then clip the entry/exit t against the [yFoot, yFoot+height] slab. @@ -666,6 +654,7 @@ namespace FRG else { //logger() << "[RAYCAST_NPC] No NPC hit" << std::endl; } + return closestNpc; } @@ -1424,83 +1413,53 @@ namespace FRG continue; } - // 1. ПЕРЕСТРОЕНИЕ ПУТИ (выполняется при пересечении обычного радиуса коллизии) + Eigen::Vector2f normal(1.f, 0.f); + if (dist > kMinSeparationEps) { + normal = delta / dist; + } + + const float penetration = (minDist - dist); + const float push = penetration * 0.5f; + + Eigen::Vector3f newA = a->state.position; + Eigen::Vector3f newB = b->state.position; + newA.x() -= normal.x() * push; + newA.z() -= normal.y() * push; + newA.y() = 0.f; + newB.x() += normal.x() * push; + newB.z() += normal.y() * push; + newB.y() = 0.f; + const bool aWasMoving = a->isMoving(); const bool bWasMoving = b->isMoving(); - auto tryReplan = [&](Character* c) { - if (replanCooldownRemainingMs.find(c) != replanCooldownRemainingMs.end()) { - return; // Уже находится в откате - } - if (c->forceReplan()) { - replanCooldownRemainingMs[c] = 500; // таймаут на спам - } - }; + if (navigation && navigation->isReady()) { + const bool aOk = navigation->isWalkable(newA); + const bool bOk = navigation->isWalkable(newB); - if (aWasMoving || bWasMoving) { - if (aWasMoving && bWasMoving) { - tryReplan(a); - tryReplan(b); - } - else if (aWasMoving) { - tryReplan(a); - } - else if (bWasMoving) { - tryReplan(b); - } - } - - // 2. ФИЗИЧЕСКОЕ ВЫТАЛКИВАНИЕ (только если дистанция меньше 0.5 * minDist) - const float hardMinDist = minDist * 0.5f; - if (dist < hardMinDist) { - Eigen::Vector2f normal(1.f, 0.f); - if (dist > kMinSeparationEps) { - normal = delta / dist; - } - - // Вычисляем выталкивание относительно hardMinDist, а не minDist - const float penetration = (hardMinDist - dist); - const float push = penetration * 0.5f; - - Eigen::Vector3f newA = a->state.position; - Eigen::Vector3f newB = b->state.position; - newA.x() -= normal.x() * push; - newA.z() -= normal.y() * push; - newA.y() = 0.f; - newB.x() += normal.x() * push; - newB.z() += normal.y() * push; - newB.y() = 0.f; - - if (navigation && navigation->isReady()) { - const bool aOk = navigation->isWalkable(newA); - const bool bOk = navigation->isWalkable(newB); - - if (aOk && bOk) { - a->state.position = newA; - b->state.position = newB; - } - else if (aOk && !bOk) { - a->state.position = newA; - } - else if (!aOk && bOk) { - b->state.position = newB; - } - } - else { + if (aOk && bOk) { a->state.position = newA; b->state.position = newB; } + else if (aOk && !bOk) { + a->state.position = newA; + } + else if (!aOk && bOk) { + b->state.position = newB; + } + } + else { + a->state.position = newA; + b->state.position = newB; } - // 3. ОСТАНОВКА И КОЛЛБЭКИ if (a->state.isPlayer && !aWasMoving) a->stopInPlace(); if (b->state.isPlayer && !bWasMoving) b->stopInPlace(); if (aWasMoving && !bWasMoving) { nudgeCharacterAside(b, a->state.position); fireBumpCallbacks(a, b); - } - else if (bWasMoving && !aWasMoving) { + } else if (bWasMoving && !aWasMoving) { nudgeCharacterAside(a, b->state.position); fireBumpCallbacks(b, a); } diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 30b5b23..c54f8e7 100644 --- a/src/navigation/PathFinder.cpp +++ b/src/navigation/PathFinder.cpp @@ -124,68 +124,6 @@ void PathFinder::build(const std::string& configPath, << ", cell=" << cellSize << ", areas=" << areas.size() << "\n"; } - -std::vector PathFinder::findPath(const Eigen::Vector3f& start, - const Eigen::Vector3f& end) const -{ - return runAStar(start, end, walkable, false); -} - -std::vector PathFinder::findPath(const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& dynamicObstacles) const -{ - if (!ready || walkable.empty()) { - return {}; - } - - std::vector walkableGrid = walkable; - // --- Логика внесения dynamicObstacles остается здесь --- - if (!dynamicObstacles.empty()) { - for (const DynamicObstacle& obstacle : dynamicObstacles) { - const float radius = obstacle.radius + agentRadius; - if (radius <= 0.0f) continue; - - const float minWorldX = obstacle.position.x() - radius; - const float maxWorldX = obstacle.position.x() + radius; - const float minWorldZ = obstacle.position.z() - radius; - const float maxWorldZ = obstacle.position.z() + radius; - - const int minCellX = (std::max)(0, static_cast(std::floor((minWorldX - minX) / cellSize))); - const int maxCellX = (std::min)(gridWidth - 1, static_cast(std::floor((maxWorldX - minX) / cellSize))); - const int minCellZ = (std::max)(0, static_cast(std::floor((minWorldZ - minZ) / cellSize))); - const int maxCellZ = (std::min)(gridDepth - 1, static_cast(std::floor((maxWorldZ - minZ) / cellSize))); - - const float radiusSq = radius * radius; - for (int z = minCellZ; z <= maxCellZ; ++z) { - for (int x = minCellX; x <= maxCellX; ++x) { - const Cell cell{ x, z }; - const Eigen::Vector3f center = cellCenter(cell); - const float dx = center.x() - obstacle.position.x(); - const float dz = center.z() - obstacle.position.z(); - if (dx * dx + dz * dz <= radiusSq) { - const int idx = indexOf(cell); - if (idx >= 0 && idx < static_cast(walkableGrid.size())) { - walkableGrid[static_cast(idx)] = 0; - } - } - } - } - } - } - - return runAStar(start, end, walkableGrid, false); -} - -std::vector PathFinder::findNearestReachableImpl( - const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& walkableGrid) const -{ - return runAStar(start, end, walkableGrid, true); -} -/* - std::vector PathFinder::findPath(const Eigen::Vector3f& start, const Eigen::Vector3f& end) const { @@ -556,7 +494,7 @@ std::vector PathFinder::findNearestReachableImpl( path.erase(path.begin()); return path; -}*/ +} std::vector PathFinder::findPathToNearest( const Eigen::Vector3f& start, const Eigen::Vector3f& end) const @@ -1137,125 +1075,4 @@ bool PathFinder::pointInPolygon(float x, float z, const std::vector PathFinder::runAStar( - const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& grid, - bool returnNearestOnFail) const -{ - if (!ready || grid.empty()) return {}; - - Cell startCell, endCell; - if (!findNearestWalkableCell(start, startCell, grid) || - !findNearestWalkableCell(end, endCell, grid)) { - return {}; - } - - if (startCell.x == endCell.x && startCell.z == endCell.z) { - return { cellCenter(endCell) }; - } - - struct QueueNode { - int index = 0; - float priority = 0.0f; - bool operator<(const QueueNode& other) const { return priority > other.priority; } - }; - - const int cellCount = gridWidth * gridDepth; - std::vector cost(static_cast(cellCount), (std::numeric_limits::max)()); - std::vector cameFrom(static_cast(cellCount), -1); - std::priority_queue open; - - const int startIndex = indexOf(startCell); - const int endIndex = indexOf(endCell); - - cost[static_cast(startIndex)] = 0.0f; - open.push({ startIndex, 0.0f }); - - int bestIndex = startIndex; - float bestDist = distanceCells(startCell, endCell); - - static const int offsets[8][2] = { - { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 }, - { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 } - }; - - bool reached = false; - - while (!open.empty()) { - const QueueNode current = open.top(); - open.pop(); - - if (current.index == endIndex) { - bestIndex = endIndex; - reached = true; - break; - } - - const Cell currentCell{ current.index % gridWidth, current.index / gridWidth }; - - if (cost[static_cast(current.index)] < current.priority - distanceCells(currentCell, endCell)) { - continue; - } - - if (returnNearestOnFail) { - const float d = distanceCells(currentCell, endCell); - if (d < bestDist) { - bestDist = d; - bestIndex = current.index; - } - } - - for (const auto& offset : offsets) { - Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] }; - if (!isCellWalkable(next, grid)) continue; - - const bool diagonal = offset[0] != 0 && offset[1] != 0; - if (diagonal) { - Cell h{ currentCell.x + offset[0], currentCell.z }; - Cell v{ currentCell.x, currentCell.z + offset[1] }; - if (!isCellWalkable(h, grid) || !isCellWalkable(v, grid)) continue; - } - - const int nextIndex = indexOf(next); - const float stepCost = diagonal ? 1.41421356f : 1.0f; - const float newCost = cost[static_cast(current.index)] + stepCost; - - if (newCost >= cost[static_cast(nextIndex)]) continue; - - cost[static_cast(nextIndex)] = newCost; - cameFrom[static_cast(nextIndex)] = current.index; - open.push({ nextIndex, newCost + distanceCells(next, endCell) }); - } - } - - if (!reached && !returnNearestOnFail) return {}; - if (bestIndex == startIndex) return {}; - - std::vector cells; - for (int cur = bestIndex; cur != -1; cur = cameFrom[static_cast(cur)]) { - cells.push_back({ cur % gridWidth, cur / gridWidth }); - if (cur == startIndex) break; - } - std::reverse(cells.begin(), cells.end()); - cells = smoothCells(cells, grid); - - std::vector path; - path.reserve(cells.size()); - for (const Cell& cell : cells) path.push_back(cellCenter(cell)); - - if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) { - path.erase(path.begin()); - } - - if (reached && !path.empty()) { - Cell requestedEndCell; - if (worldToCell(end, requestedEndCell) && requestedEndCell.x == endCell.x && requestedEndCell.z == endCell.z) { - path.back() = Eigen::Vector3f(end.x(), floorY, end.z()); - } - } - - return path; -} - } // namespace FRG diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index b2d43b5..adb92fc 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -109,11 +109,6 @@ private: std::vector findNearestReachableImpl(const Eigen::Vector3f& start, const Eigen::Vector3f& end, const std::vector& walkableGrid) const; - - std::vector runAStar(const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& grid, - bool returnNearestOnFail) const; }; } // namespace FRG