diff --git a/src/CharacterState.cpp b/src/CharacterState.cpp index 011210d..f5cfec6 100644 --- a/src/CharacterState.cpp +++ b/src/CharacterState.cpp @@ -92,7 +92,8 @@ void CharacterState::stopInPlace() { onArrivedCallbackName.clear(); movementBlockedTicks = 0; navigationRetryTicks = 0; - blockingCharacterId.clear(); + velocity = Vec2fp(fp(0), fp(0)); + preferredVelocity = Vec2fp(fp(0), fp(0)); } void CharacterState::setTarget( @@ -101,6 +102,8 @@ void CharacterState::setTarget( const LocationNetworkState& location, const std::string& entityId) { + + const Vec3fp normalizedTarget(target.x, static_cast(0.f), target.z); const bool sameRequestedTarget = (normalizedTarget - requestedWalkTarget).normSquared() <= TARGET_REPLAN_THRESHOLD_SQUARED; @@ -119,11 +122,20 @@ void CharacterState::setTarget( onArrivedCallbackName = std::move(callbackName); movementBlockedTicks = 0; navigationRetryTicks = 0; - blockingCharacterId.clear(); - pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, target); + // В setTarget и forceReplan: + if (location.activeNavigationIndex < 0 || + location.activeNavigationIndex >= static_cast(location.navigationMaps.size())) { + walkTarget = Vec3fp(position.x, static_cast(0.f), position.z); + return; + } + + pathWaypoints = location.navigationMaps[location.activeNavigationIndex] + .findPathToNearest(this->position, normalizedTarget); + + // Сразу целимся во второй элемент (индекс 1), так как индекс 0 — это точка старта + currentWaypointIndex = (pathWaypoints.size() > 1) ? 1 : 0; - currentWaypointIndex = 0; if (!pathWaypoints.empty()) { for (Vec3fp& waypoint : pathWaypoints) { @@ -143,19 +155,25 @@ void CharacterState::forceReplan( { const Vec3fp normalizedTarget( requestedWalkTarget.x, static_cast(0.f), requestedWalkTarget.z); - - pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, normalizedTarget); - currentWaypointIndex = 0; + if (location.activeNavigationIndex < 0 || + location.activeNavigationIndex >= static_cast(location.navigationMaps.size())) { + walkTarget = Vec3fp(position.x, static_cast(0.f), position.z); + return; + } + + pathWaypoints = location.navigationMaps[location.activeNavigationIndex] + .findPathToNearest(this->position, normalizedTarget); + + currentWaypointIndex = (pathWaypoints.size() > 1) ? 1 : 0; movementBlockedTicks = 0; navigationRetryTicks = 0; + if (!pathWaypoints.empty()) { - for (Vec3fp& waypoint : pathWaypoints) - { + for (Vec3fp& waypoint : pathWaypoints) { waypoint.y = static_cast(0.f); } walkTarget = pathWaypoints.back(); - blockingCharacterId.clear(); return; } @@ -237,121 +255,110 @@ CharacterUpdateEvents CharacterState::update( battle_state = 0; } else { battle_state = 1; + velocity = Vec2fp(fp(0), fp(0)); } } const Vec3fp activeTarget = getCurrentNavigationTarget(); - const Vec3fp lookTarget = - battle_state == 1 && attackTarget ? attackTarget->position : activeTarget; - if (currentState == AnimationState::WALK || currentState == AnimationState::STAND) { - Vec3fp toTarget = activeTarget - position; - toTarget.y = static_cast(0.f); - const fp distanceSquared = toTarget.normSquared(); - if (distanceSquared > kWalkThreshold_SQUARED()) { - fp distance = sqrt(distanceSquared); //FP SQRT - const Vec3fp direction = toTarget / distance; - const fp moveAmount = walkSpeed * static_cast(deltaMs) / static_cast < fp>(1000.f); - Vec3fp candidate = - moveAmount >= distance ? activeTarget : position + direction * moveAmount; - candidate.y = static_cast(0.f); - /*const CharacterMovementObstacle* blocker = nullptr; - if (context.movementObstacles) { - for (const CharacterMovementObstacle& obstacle : - *context.movementObstacles) { - if (obstacle.id == context.characterId) continue; - Eigen::Vector3f startDelta = position - obstacle.position; - Eigen::Vector3f endDelta = candidate - obstacle.position; - startDelta.y() = 0.f; - endDelta.y() = 0.f; - const float minimumDistance = - movementContactRadius + obstacle.radius; - const float minimumDistanceSq = - minimumDistance * minimumDistance; - const Eigen::Vector3f segment = candidate - position; - const float segmentLengthSq = - segment.x() * segment.x() + segment.z() * segment.z(); - float t = 0.f; - if (segmentLengthSq > 1e-8f) { - t = -(startDelta.x() * segment.x() + - startDelta.z() * segment.z()) / segmentLengthSq; - t = (std::max)(0.f, (std::min)(1.f, t)); - } - Eigen::Vector3f closest = startDelta + segment * t; - closest.y() = 0.f; - const bool movingCloser = - endDelta.squaredNorm() + 1e-6f < startDelta.squaredNorm(); - if (movingCloser && - closest.squaredNorm() < minimumDistanceSq) { - blocker = &obstacle; - break; - } - } - } - if (blocker) { - blockingCharacterId = blocker->id; - currentState = AnimationState::STAND; - const bool sameDirection = - blocker->moving && - blocker->movementDirection.dot(direction) > 0.5f; - if (sameDirection) { - movementBlockedTicks = 0; - } else if (++movementBlockedTicks >= BLOCKED_REPLAN_TICKS) { - if (isPlayer) { - stopInPlace(); - } else { - pathWaypoints.clear(); - currentWaypointIndex = 0; - walkTarget = - Eigen::Vector3f(position.x(), 0.f, position.z()); - movementBlockedTicks = 0; - scheduleNextNavigationRetry(); - navigationPausedThisTick = true; - } - } - } else*/{ - movedLastTick = - (candidate - position).normSquared() > static_cast(1e-8f); - position = candidate; - movementBlockedTicks = 0; - blockingCharacterId.clear(); - } - targetFacingAngle = fpm::atan2(direction.x, -direction.z); - currentState = AnimationState::WALK; - } else { - currentState = AnimationState::STAND; - const bool completedPath = !pathWaypoints.empty(); - const bool hasNextWaypoint = - !pathWaypoints.empty() && currentWaypointIndex + 1 < pathWaypoints.size(); - if (hasNextWaypoint) { + preferredVelocity = Vec2fp(fp(0), fp(0)); + + if (hp > 0.f && battle_state == 0 && !pathWaypoints.empty() && + (currentState == AnimationState::WALK || currentState == AnimationState::STAND)) + { + // Если мы уже достигли промежуточного waypoint'а, сразу переключаемся на следующий в этом же тике + while (!pathWaypoints.empty() && currentWaypointIndex + 1 < pathWaypoints.size()) { + Vec3fp diff = pathWaypoints[currentWaypointIndex] - position; + diff.y = fp(0); + if (diff.normSquared() <= kWalkThreshold_SQUARED()) { ++currentWaypointIndex; - } else { - pathWaypoints.clear(); - currentWaypointIndex = 0; - if (completedPath && - (requestedWalkTarget - position).normSquared() > - TARGET_REPLAN_THRESHOLD_SQUARED && - !isPlayer) - scheduleNextNavigationRetry(); - if (completedPath && !onArrivedCallbackName.empty()) { - events.arrivedCallbackName = onArrivedCallbackName; - onArrivedCallbackName.clear(); - } + } + else { + break; + } + } + + const Vec3fp currentTarget = getCurrentNavigationTarget(); + Vec3fp toTarget = currentTarget - position; + toTarget.y = fp(0); + const fp distanceSquared = toTarget.normSquared(); + + if (distanceSquared > kWalkThreshold_SQUARED()) { + const fp distance = fpm::sqrt(distanceSquared); + const fp dtSec = static_cast(deltaMs) / fp(1000); + + fp desiredSpeed = distance / dtSec; + if (desiredSpeed > walkSpeed) { + desiredSpeed = walkSpeed; } + preferredVelocity = Vec2fp( + (toTarget.x / distance) * desiredSpeed, + (toTarget.z / distance) * desiredSpeed + ); + + // Если стартуем с места — сразу целим поворот на точку, + // а в движении угол скорректирует Фаза 3 по реальному вектору ORCA (newVel) + if (velocity.lengthSq() <= static_cast(0.05f)) { + targetFacingAngle = fpm::atan2(toTarget.x, -toTarget.z); + } + } + else { + // Дошли до ФИНАЛЬНОЙ точки маршрута + velocity = Vec2fp(fp(0), fp(0)); + if (currentState == AnimationState::WALK) { + currentState = AnimationState::STAND; + resetAnim = true; + } + const bool completedPath = !pathWaypoints.empty(); + pathWaypoints.clear(); + currentWaypointIndex = 0; + + if (completedPath && + (requestedWalkTarget - position).normSquared() > TARGET_REPLAN_THRESHOLD_SQUARED && + !isPlayer) { + scheduleNextNavigationRetry(); + } + if (completedPath && !onArrivedCallbackName.empty()) { + events.arrivedCallbackName = onArrivedCallbackName; + onArrivedCallbackName.clear(); + } if (faceTarget) { Vec3fp toFace = faceTarget->position - position; - toFace.y = static_cast(0.f); - const fp distanceToFace = sqrt(toFace.normSquared()); //FP SQRT + toFace.y = fp(0); + const fp distanceToFace = fpm::sqrt(toFace.normSquared()); targetFacingAngle = distanceToFace > static_cast(1e-3f) ? fpm::atan2(toFace.x / distanceToFace, -toFace.z / distanceToFace) : facingAngle; - } else if (!faceTargetId.empty()) { + } + else if (!faceTargetId.empty()) { faceTargetId.clear(); } } } + else if (hp > 0.f && battle_state == 0 && pathWaypoints.empty() && + (currentState == AnimationState::WALK || currentState == AnimationState::STAND)) + { + // Маршрута нет — стоим на месте и при необходимости поворачиваемся к faceTarget + velocity = Vec2fp(fp(0), fp(0)); + if (currentState == AnimationState::WALK) { + currentState = AnimationState::STAND; + resetAnim = true; + } + if (faceTarget) { + Vec3fp toFace = faceTarget->position - position; + toFace.y = fp(0); + const fp distanceToFace = fpm::sqrt(toFace.normSquared()); + targetFacingAngle = distanceToFace > static_cast(1e-3f) + ? fpm::atan2(toFace.x / distanceToFace, -toFace.z / distanceToFace) + : facingAngle; + } + else if (!faceTargetId.empty()) { + faceTargetId.clear(); + } + } + if (!isPlayer && !navigationPausedThisTick && pathWaypoints.empty() && (requestedWalkTarget - position).normSquared() > TARGET_REPLAN_THRESHOLD_SQUARED && !events.navigationRequested) { @@ -360,7 +367,6 @@ CharacterUpdateEvents CharacterState::update( events.navigationRequested = true; events.forceNavigationReplan = true; events.navigationTarget = requestedWalkTarget; - events.blockingCharacterId = blockingCharacterId; } } @@ -371,9 +377,11 @@ CharacterUpdateEvents CharacterState::update( resetAnim = true; attackTargetId.clear(); } - } else if (battle_state == 1 && attackTarget) { + } + else if (battle_state == 1 && attackTarget) { targetFacingAngle = fpm::atan2( - lookTarget.x - position.x, -(lookTarget.z - position.z)); + attackTarget->position.x - position.x, -(attackTarget->position.z - position.z)); + if (currentState == AnimationState::STAND || currentState == AnimationState::WALK) { currentState = AnimationState::STAND_TO_ACTION; resetAnim = true; @@ -439,8 +447,8 @@ CharacterUpdateEvents CharacterState::update( events.attackTargetId = attackTargetId; events.attackDirection = attackTarget->position - position; events.attackDirection.y = static_cast(0.f); - const fp lengthSquared = fpm::sqrt(events.attackDirection.normSquared()); - if (lengthSquared > static_cast(1e-3f * 1e-3f)) + const fp lengthSquared = events.attackDirection.normSquared(); + if (lengthSquared > static_cast(0.001f)) { const fp length = fpm::sqrt(lengthSquared); //FP SQRT @@ -511,8 +519,7 @@ void CharacterState::save(nlohmann::json& out) const out["movedLastTick"] = movedLastTick; out["navigationRetryCooldownTicks"] = navigationRetryCooldownTicks; out["navigationRetryCooldownIndex"] = navigationRetryCooldownIndex; - if (!blockingCharacterId.empty()) - out["blockingCharacterId"] = blockingCharacterId; + out["animationFrame"] = animationFrame; out["visibleRange"] = static_cast(visibleRange); if (!onArrivedCallbackName.empty()) @@ -567,8 +574,6 @@ void CharacterState::load(const nlohmann::json& in) "navigationRetryCooldownIndex", navigationRetryCooldownIndex); if (!navigationRetryCooldownTicks.empty()) navigationRetryCooldownIndex %= navigationRetryCooldownTicks.size(); - blockingCharacterId = - in.value("blockingCharacterId", std::string()); animationFrame = in.value("animationFrame", animationFrame); onArrivedCallbackName = in.value("onArrivedCallbackName", std::string()); } diff --git a/src/CharacterState.h b/src/CharacterState.h index 384cd2e..ded174c 100644 --- a/src/CharacterState.h +++ b/src/CharacterState.h @@ -115,10 +115,14 @@ public: float modelScale = 0.12f; Eigen::Quaternionf modelCorrectionRotation = Eigen::Quaternionf::Identity(); bool useGpuSkinning = true; - // Full clearance used while planning around stationary characters. - //fp collisionRadius = static_cast(0.45f); - // Smaller physical contact threshold used by the movement guard. - //fp movementContactRadius = static_cast(0.32f); + + fp collisionRadius = static_cast(0.4f); // Радиус капсулы персонажа + + // Векторы скорости в плоскости XZ (сериализовать в JSON не обязательно, если считаются каждый тик, + // но для строгого lockstep-снапшота velocity лучше сохранять) + Vec2fp velocity{ fp(0), fp(0) }; // Реальная скорость с прошлого тика (нужна соседям в ORCA) + Vec2fp preferredVelocity{ fp(0), fp(0) }; // Желаемая скорость к waypoint'у на текущем тике + fp interactionRadius = static_cast(0.0f); bool movedLastTick = false; @@ -132,7 +136,6 @@ public: fp homeDriftCheckTimer = static_cast(5.0f); int movementBlockedTicks = 0; int navigationRetryTicks = 0; - std::string blockingCharacterId; std::vector navigationRetryCooldownTicks; std::size_t navigationRetryCooldownIndex = 0; @@ -193,7 +196,7 @@ struct CharacterUpdateEvents { bool navigationRequested = false; bool forceNavigationReplan = false; Vec3fp navigationTarget; - std::string blockingCharacterId; + std::string arrivedCallbackName; std::string attackTargetId; Vec3fp attackDirection; diff --git a/src/LocationGameState.cpp b/src/LocationGameState.cpp index 9d4dd38..2f0edde 100644 --- a/src/LocationGameState.cpp +++ b/src/LocationGameState.cpp @@ -2,6 +2,7 @@ #include #include #include +#include "OrcaSolver.h" namespace FRG { @@ -126,37 +127,6 @@ const CharacterState* LocationGameState::findCharacter( return nullptr; } -/* -std::vector LocationGameState::collectPathObstacles( - const std::string& requesterId, - const PlayerStateMap& players, - const std::string& forcedObstacleId) const -{ - std::vector> characters; - characters.reserve(players.size() + npcs.size()); - for (const auto& [id, character] : players) - if (character) characters.emplace_back(id, character.get()); - for (const auto& [id, character] : npcs) - if (character) characters.emplace_back(id, character.get()); - std::sort(characters.begin(), characters.end(), - [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }); - - const CharacterState* requester = findCharacter(requesterId, players); - const std::string attackTargetId = - requester ? requester->attackTargetId : std::string(); - std::vector obstacles; - obstacles.reserve(characters.size()); - for (const auto& [id, character] : characters) { - if (id == requesterId || !character->enabled || character->hp <= 0.f) - continue; - if (id == attackTargetId && id != forcedObstacleId) - continue; - if (character->movedLastTick && id != forcedObstacleId) continue; - obstacles.push_back({character->position, character->collisionRadius}); - } - return obstacles; -}*/ - bool LocationGameState::setCharacterDestination( const std::string& id, const Vec3fp& target, @@ -173,45 +143,12 @@ bool LocationGameState::setCharacterDestination( bool LocationGameState::replanCharacter( const std::string& id, - PlayerStateMap& players, - const std::string& forcedObstacleId) + PlayerStateMap& players) { CharacterState* character = findCharacter(id, players); if (!character || !location) return false; - if (!forcedObstacleId.empty() && !character->isPlayer && - character->canAttack && character->hp > 0.f) { - const std::string selectedTarget = - selectNpcTarget(*character, players, false); - if (selectedTarget != character->attackTargetId) { - character->stopInPlace(); - character->attackTargetId = selectedTarget; - character->battle_state = 0; - character->attack = 0; - } - if (!selectedTarget.empty()) { - const CharacterState* target = findCharacter(selectedTarget, players); - if (target) { - character->setTarget( - target->position, {}, *location, id); - return true; - } - } - - character->attackTargetId.clear(); - character->battle_state = 0; - character->attack = 0; - const Vec3fp homeTarget( - character->homePosition.x, static_cast(0.f), character->homePosition.z); - if ((homeTarget - character->position).normSquared() > - CharacterState::kWalkThreshold_SQUARED()) { - character->setTarget( - homeTarget, {}, *location, id); - } - return true; - } - character->forceReplan( - *location, id); + character->forceReplan(*location, id); return true; } @@ -365,12 +302,19 @@ LocationSimulationEvents LocationGameState::update( std::int64_t deltaMs, PlayerStateMap& players, bool interactiveObjectsLocked) { LocationSimulationEvents result; + + //Taxi cars update: result.taxiArrived = updateNpcCar(deltaMs); + + //Interactive objects update: std::vector objectIds; objectIds.reserve(interactiveObjects.size()); for (const auto& [id, object] : interactiveObjects) + { if (object) objectIds.push_back(id); + } std::sort(objectIds.begin(), objectIds.end()); + for (const std::string& id : objectIds) { InteractiveObjectUpdateEvents events = interactiveObjects.at(id)->update(deltaMs); @@ -379,35 +323,49 @@ LocationSimulationEvents LocationGameState::update( {id, std::move(events.onCompleteName), false, {}}); } - // All characters observe the same tick-start state. Damage is applied only - // after every state has advanced, so vector order cannot suppress an attack. + //Character update: + const fp dtSec = static_cast(deltaMs) / fp(1000); + + // 1. Собираем всех персонажей в единый массив и сортируем по ID + // (чтобы даже порядок вызовов CharacterState::update был строго детерминирован!) + struct Entry { + std::string id; + CharacterState* state; + }; + std::vector allChars; + allChars.reserve(players.size() + npcs.size()); + + for (auto& [id, ptr] : players) { + if (ptr && ptr->enabled) allChars.push_back({ id, ptr.get() }); + } + for (auto& [id, ptr] : npcs) { + if (ptr && ptr->enabled) allChars.push_back({ id, ptr.get() }); + } + std::sort(allChars.begin(), allChars.end(), [](const Entry& a, const Entry& b) { + return a.id < b.id; + }); + std::unordered_map observations; for (const auto& [id, player] : players) if (player && player->enabled) observations.emplace( - id, CharacterObservation{player->position, player->hp}); + id, CharacterObservation{ player->position, player->hp }); for (const auto& [id, npc] : npcs) if (npc && npc->enabled) observations.emplace( - id, CharacterObservation{npc->position, npc->hp}); + id, CharacterObservation{ npc->position, npc->hp }); - std::vector playerIds; - playerIds.reserve(players.size()); - for (const auto& [id, player] : players) - if (player) playerIds.push_back(id); - std::sort(playerIds.begin(), playerIds.end()); - - std::vector npcIds; - npcIds.reserve(npcs.size()); - for (const auto& [id, npc] : npcs) - if (npc) npcIds.push_back(id); - std::sort(npcIds.begin(), npcIds.end()); // Target selection is authoritative and runs before all characters observe // the tick. A valid target is a lock: it is retained until the player dies // or becomes disabled, even when the player leaves visibleRange. - for (const std::string& id : npcIds) { - CharacterState& npc = *npcs.at(id); + for (auto& entry : allChars) { + if (entry.state->isPlayer) + { + continue; + } + + CharacterState& npc = *entry.state; if (!npc.enabled || !npc.canAttack || npc.hp <= 0.f) continue; @@ -428,33 +386,100 @@ LocationSimulationEvents LocationGameState::update( const Vec3fp toHome = homeTarget - npc.position; if (toHome.normSquared() > CharacterState::kWalkThreshold_SQUARED() && (npc.requestedWalkTarget - homeTarget).normSquared() > - CharacterState::kWalkThreshold_SQUARED()) { - setCharacterDestination(id, homeTarget, {}, players); + CharacterState::kWalkThreshold_SQUARED()) { + setCharacterDestination(entry.id, homeTarget, {}, players); } } } - - 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)); + + // 2. ФАЗА 1: Обновляем логику каждого персонажа (вычисляется preferredVelocity) + for (auto& entry : allChars) { + CharacterUpdateContext context; + context.characterId = entry.id; + + + auto attack = observations.find(entry.state->attackTargetId); + if (attack != observations.end()) context.attackTarget = &attack->second; + auto face = observations.find(entry.state->faceTargetId); + if (face != observations.end()) context.faceTarget = &face->second; + + CharacterUpdateEvents events = entry.state->update(deltaMs, context); + + if (!events.arrivedCallbackName.empty() || events.attackImpact || + events.deathAnimationComplete || events.navigationRequested) { + result.characters.push_back({ entry.id, std::move(events) }); } } - for (const std::string& id : npcIds) - { - if (npcs.at(id)->enabled) - { - //advance(id, *npcs.at(id)); - } + std::vector orcaAgents; + orcaAgents.reserve(allChars.size()); + + for (auto& entry : allChars) { + CharacterState* st = entry.state; + if (st->hp <= 0.f) continue; // Мертвые не участвуют в коллизиях + + const bool wantsToMove = st->preferredVelocity.lengthSq() > fp(0.001f); + const bool inCombat = (st->battle_state == 1); + + OrcaAgent agent; + agent.id = entry.id; + agent.state = st; + agent.pos = Vec2fp(st->position.x, st->position.z); + agent.vel = st->velocity; + agent.prefVel = st->preferredVelocity; + agent.radius = st->collisionRadius; + agent.maxSpeed = st->walkSpeed; + agent.isStationary = (!wantsToMove || inCombat); + agent.isGhost = (st->canAttack && !st->isPlayer); + agent.constantSpeed = !agent.isGhost; + + orcaAgents.push_back(agent); } + const PathFinder* activeNavMesh = nullptr; + if (location && location->activeNavigationIndex >= 0 && + location->activeNavigationIndex < static_cast(location->navigationMaps.size())) { + activeNavMesh = &location->navigationMaps[location->activeNavigationIndex]; + } + OrcaSolver::step(orcaAgents, dtSec, activeNavMesh); + + // 4. ФАЗА 3: Записываем новые позиции, скорости и обновляем анимацию/поворот + for (const auto& agent : orcaAgents) { + CharacterState* st = agent.state; + const Vec2fp oldPos(st->position.x, st->position.z); + const Vec2fp displacement = agent.pos - oldPos; + const fp movedDistSq = displacement.lengthSq(); + + st->position.x = agent.pos.x; + st->position.z = agent.pos.y; + st->velocity = agent.newVel; + + st->movedLastTick = (movedDistSq > fp(0.0005f)); + + // Если персонаж хотел идти, проверяем — продвинулся ли он или уперся в глухую пробку + const bool wantedToMove = st->preferredVelocity.lengthSq() > fp(0.01f); + if (wantedToMove) { + if (st->movedLastTick) { + st->movementBlockedTicks = 0; + if (st->currentState == AnimationState::STAND) { + st->currentState = AnimationState::WALK; + st->resetAnim = true; + } + + if (agent.newVel.lengthSq() > fp(0.05f)) { + st->targetFacingAngle = fpm::atan2(agent.newVel.x, -agent.newVel.y); + } + } + else { + st->movementBlockedTicks++; + if (st->currentState == AnimationState::WALK) { + st->currentState = AnimationState::STAND; + st->resetAnim = true; + } + } + } + } for (const CharacterSimulationEvent& source : result.characters) { if (!source.events.attackImpact) continue; @@ -468,7 +493,7 @@ LocationSimulationEvents LocationGameState::update( damage, target->hp, target->initialHp, - source.events.attackDirection}); + source.events.attackDirection }); } return result; diff --git a/src/LocationGameState.h b/src/LocationGameState.h index 0eb372b..7ec9afe 100644 --- a/src/LocationGameState.h +++ b/src/LocationGameState.h @@ -64,10 +64,6 @@ struct LocationGameState { CharacterState* findCharacter(const std::string& id, PlayerStateMap& players); const CharacterState* findCharacter( const std::string& id, const PlayerStateMap& players) const; - /*std::vector collectPathObstacles( - const std::string& requesterId, - const PlayerStateMap& players, - const std::string& forcedObstacleId = {}) const;*/ bool setCharacterDestination( const std::string& id, const Vec3fp& target, @@ -76,8 +72,7 @@ struct LocationGameState { const std::string& forcedObstacleId = {}); bool replanCharacter( const std::string& id, - PlayerStateMap& players, - const std::string& forcedObstacleId = {}); + PlayerStateMap& players); void startTaxi(); void resetTaxi(); LocationSimulationEvents update( diff --git a/src/OrcaSolver.h b/src/OrcaSolver.h new file mode 100644 index 0000000..018de57 --- /dev/null +++ b/src/OrcaSolver.h @@ -0,0 +1,419 @@ +#pragma once + +#include "navigation/PathFinder.h" +#include +#include +#include + +namespace FRG { + + // Линейное ограничение (полуплоскость допустимых скоростей ORCA) + struct OrcaLine { + Vec2fp point; // Точка на граничной прямой + Vec2fp direction; // Единичный вектор направления прямой (допустимая полуплоскость СЛЕВА) + }; + + struct OrcaAgent { + std::string id; + CharacterState* state = nullptr; + Vec2fp pos; + Vec2fp vel; + Vec2fp prefVel; + fp radius; + fp maxSpeed; + bool isStationary = false; + bool isGhost = false; // Враги-духи (парят в воздухе, легко расталкиваются) + bool constantSpeed = true; // Люди (ходят только с полной скоростью либо стоят) + Vec2fp newVel; + }; + + class OrcaSolver { + public: + // Горизонт упреждения 0.6 сек (~1.8 метра при скорости 3 м/с) + static fp timeHorizon() { return fp(0.6f); } + + static void step( + std::vector& agents, + fp dtSec, + const PathFinder* navMesh) + { + if (dtSec <= fp(0) || agents.empty()) return; + + // 1. Обязательная сортировка по ID для 100% детерминизма Lockstep + std::sort(agents.begin(), agents.end(), [](const OrcaAgent& a, const OrcaAgent& b) { + return a.id < b.id; + }); + + const fp invTimeHorizon = fp(1) / timeHorizon(); + const fp invDt = fp(1) / dtSec; + const fp neighborDist = fp(4.0f); + const fp neighborDistSq = neighborDist * neighborDist; + + std::vector orcaLines; + orcaLines.reserve(16); + + // 2. Для каждого агента строим линии ORCA относительно соседей + for (std::size_t i = 0; i < agents.size(); ++i) { + OrcaAgent& agent = agents[i]; + if (agent.maxSpeed <= fp(0) || agent.prefVel.lengthSq() <= fp(0.001f)) { + agent.newVel = Vec2fp(fp(0), fp(0)); + continue; + } + + orcaLines.clear(); + + // ВАЖНО: Если человек стартует с места, используем prefVel как базу для построения VO, + // чтобы стоящие рядом NPC не запирали его в нулевой скорости! + const Vec2fp effectiveAgentVel = (agent.constantSpeed && agent.vel.lengthSq() < fp(0.1f)) + ? agent.prefVel + : agent.vel; + + for (std::size_t j = 0; j < agents.size(); ++j) { + if (i == j) continue; + const OrcaAgent& other = agents[j]; + + // Человек НЕ строит тормозящих линий ORCA об духов — + // духи сами расступаются перед человеком и расталкиваются в resolveHardCollisions! + if (!agent.isGhost && other.isGhost) { + continue; + } + + const Vec2fp relativePos = other.pos - agent.pos; + const fp distSq = relativePos.lengthSq(); + if (distSq > neighborDistSq) continue; + + const Vec2fp relativeVel = effectiveAgentVel - other.vel; + // Для проверки конуса обхода используем чуть меньший радиус (85%), + // чтобы персонажи легче протискивались мимо друг друга + const fp combinedRadius = (agent.radius + other.radius) * fp(0.85f); + const fp combinedRadiusSq = combinedRadius * combinedRadius; + + OrcaLine line; + Vec2fp u; + + if (distSq > combinedRadiusSq) { + const Vec2fp w = relativeVel - relativePos * invTimeHorizon; + const fp wLengthSq = w.lengthSq(); + const fp dotProduct1 = w.dot(relativePos); + + if (dotProduct1 < fp(0) && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq) { + const fp wLength = fpm::sqrt(wLengthSq); + const Vec2fp unitW = wLength > fp(0.001f) ? (w / wLength) : Vec2fp(fp(1), fp(0)); + line.direction = Vec2fp(unitW.y, -unitW.x); + u = unitW * (combinedRadius * invTimeHorizon - wLength); + } + else { + const fp leg = fpm::sqrt(std::max(fp(0), distSq - combinedRadiusSq)); + if (relativePos.cross(w) > fp(0)) { + line.direction = Vec2fp( + relativePos.x * leg - relativePos.y * combinedRadius, + relativePos.x * combinedRadius + relativePos.y * leg + ) / distSq; + } + else { + line.direction = Vec2fp( + -(relativePos.x * leg + relativePos.y * combinedRadius), + -(-relativePos.x * combinedRadius + relativePos.y * leg) + ) / distSq; + } + const fp dotProduct2 = relativeVel.dot(line.direction); + u = line.direction * dotProduct2 - relativeVel; + } + } + else { + const Vec2fp w = relativeVel - relativePos * invDt; + const fp wLength = w.length(); + const Vec2fp unitW = wLength > fp(0.001f) + ? (w / wLength) + : ((i < j) ? Vec2fp(fp(1), fp(0)) : Vec2fp(fp(-1), fp(0))); + + line.direction = Vec2fp(unitW.y, -unitW.x); + u = unitW * (combinedRadius * invDt - wLength); + } + + fp responsibility = fp(0.5f); + if (agent.isGhost && !other.isGhost) { + responsibility = fp(0.95f); // Дух берет 95% уклонения от человека + } + else if (other.isStationary && !agent.isStationary) { + responsibility = fp(1.0f); // Обходящий берет 100% маневра мимо стоящего + } + else if (!other.isStationary && agent.isStationary) { + responsibility = fp(0.0f); + } + + line.point = effectiveAgentVel + u * responsibility; + orcaLines.push_back(line); + } + + // Решаем стандартную задачу ORCA + Vec2fp resultVel = agent.prefVel; + int lineFail = linearProgram2(orcaLines, agent.maxSpeed, agent.prefVel, false, resultVel); + if (lineFail < static_cast(orcaLines.size())) { + linearProgram3(orcaLines, lineFail, agent.maxSpeed, resultVel); + } + + // Если это человек (constantSpeed == true), ищем угол обхода с сохранением полной скорости: + const fp prefSpeedSq = agent.prefVel.lengthSq(); + if (agent.constantSpeed && prefSpeedSq > fp(0.01f)) { + const fp prefSpeed = fpm::sqrt(prefSpeedSq); + resultVel = findConstantSpeedVelocity(orcaLines, agent.prefVel, prefSpeed, resultVel); + } + + agent.newVel = resultVel; + } + + // 3. Применяем скорости и корректируем жесткие наложения + for (auto& agent : agents) { + agent.pos = agent.pos + agent.newVel * dtSec; + } + + resolveHardCollisions(agents); + + // 4. Удерживаем всех внутри NavMesh + if (navMesh && navMesh->isReady()) { + for (auto& agent : agents) { + Vec3fp pt3D(agent.pos.x, navMesh->getFloorY(), agent.pos.y); + if (!navMesh->isWalkable(pt3D)) { + int dummyTri = -1; + agent.pos = navMesh->clampToNavMesh(agent.pos, dummyTri); + } + } + } + } + + private: + // Подбирает вектор скорости строго длины prefSpeed, который огибает препятствие без замедления + static Vec2fp findConstantSpeedVelocity( + const std::vector& orcaLines, + const Vec2fp& prefVel, + fp prefSpeed, + const Vec2fp& orcaVel) + { + if (orcaLines.empty()) return prefVel; + + const Vec2fp forwardDir = prefVel / prefSpeed; + // Перпендикуляр для отступа влево/вправо при лобовом столкновении + const Vec2fp rightDir(forwardDir.y, -forwardDir.x); + + // Перебираем веер углов отклонения от прямого курса (0°, ±15°, ±30°, ±45°, ±60°, ±75°, ±90°) + // Синусы и косинусы заданы константами для скорости и детерминизма + struct AngleOffset { fp cosA; fp sinA; }; + static const AngleOffset offsets[] = { + { fp(1.000f), fp(0.000f) }, + { fp(0.966f), fp(0.259f) }, { fp(0.966f), fp(-0.259f) }, // ±15° + { fp(0.866f), fp(0.500f) }, { fp(0.866f), fp(-0.500f) }, // ±30° + { fp(0.707f), fp(0.707f) }, { fp(0.707f), fp(-0.707f) }, // ±45° + { fp(0.500f), fp(0.866f) }, { fp(0.500f), fp(-0.866f) }, // ±60° + { fp(0.259f), fp(0.966f) }, { fp(0.259f), fp(-0.966f) }, // ±75° + { fp(0.000f), fp(1.000f) }, { fp(0.000f), fp(-1.000f) } // ±90° + }; + + // Определяем, в какую сторону (влево или вправо) нам выгоднее обходить по подсказке от ORCA + const fp sideHint = rightDir.dot(orcaVel); + + for (std::size_t k = 0; k < sizeof(offsets) / sizeof(offsets[0]); ++k) { + // Если k нечетный (пара ±угол), проверяем сначала ту сторону, куда склоняется ORCA + std::size_t idx = k; + if (k >= 1 && (k % 2 == 1) && sideHint < fp(0)) { + idx = k + 1; + } + else if (k >= 1 && (k % 2 == 0) && sideHint < fp(0)) { + idx = k - 1; + } + + const Vec2fp candidateDir = forwardDir * offsets[idx].cosA + rightDir * offsets[idx].sinA; + const Vec2fp candidateVel = candidateDir * prefSpeed; + + bool valid = true; + for (const auto& line : orcaLines) { + // Допускаем погрешность 0.1f м/с + if (line.direction.cross(line.point - candidateVel) > fp(0.1f)) { + valid = false; + break; + } + } + + if (valid) { + return candidateVel; + } + } + + // Если ни один угол до ±90° не свободен (проход полностью заблокирован вплотную) — стоим + return Vec2fp(fp(0), fp(0)); + } + + static void resolveHardCollisions(std::vector& agents) { + const std::size_t n = agents.size(); + for (int iter = 0; iter < 2; ++iter) { + for (std::size_t i = 0; i < n; ++i) { + for (std::size_t j = i + 1; j < n; ++j) { + Vec2fp delta = agents[j].pos - agents[i].pos; + fp distSq = delta.lengthSq(); + fp minDist = agents[i].radius + agents[j].radius; + + if (distSq < minDist * minDist) { + fp dist = fpm::sqrt(distSq); + Vec2fp normal = (dist > fp(0.001f)) + ? (delta / dist) + : Vec2fp(fp(1), fp(0)); + + fp penetration = (minDist - dist) * fp(0.5f); + + fp weightI = fp(0.5f); + fp weightJ = fp(0.5f); + + if (!agents[i].isGhost && agents[j].isGhost) { + weightI = fp(0.0f); weightJ = fp(1.0f); // Человек на 100% выталкивает духа + } + else if (agents[i].isGhost && !agents[j].isGhost) { + weightI = fp(1.0f); weightJ = fp(0.0f); + } + else { + weightI = agents[i].isStationary ? fp(0.1f) : fp(0.5f); + weightJ = agents[j].isStationary ? fp(0.1f) : fp(0.5f); + fp sumW = weightI + weightJ; + weightI /= sumW; + weightJ /= sumW; + } + + agents[i].pos = agents[i].pos - normal * (penetration * weightI); + agents[j].pos = agents[j].pos + normal * (penetration * weightJ); + } + } + } + } + } + + static bool linearProgram1( + const std::vector& lines, + int lineNo, + fp radius, + const Vec2fp& optVelocity, + bool directionOpt, + Vec2fp& result) + { + const fp dotProduct = lines[lineNo].point.dot(lines[lineNo].direction); + const fp discriminant = dotProduct * dotProduct + radius * radius - lines[lineNo].point.lengthSq(); + + if (discriminant < fp(0)) { + return false; + } + + const fp sqrtDiscriminant = fpm::sqrt(discriminant); + fp tLeft = -dotProduct - sqrtDiscriminant; + fp tRight = -dotProduct + sqrtDiscriminant; + + for (int i = 0; i < lineNo; ++i) { + const fp denominator = lines[lineNo].direction.cross(lines[i].direction); + const fp numerator = lines[i].direction.cross(lines[lineNo].point - lines[i].point); + + if (fpm::abs(denominator) <= fp(0.001f)) { + if (numerator < fp(0)) return false; + continue; + } + + const fp t = numerator / denominator; + if (denominator >= fp(0)) { + tRight = std::min(tRight, t); + } + else { + tLeft = std::max(tLeft, t); + } + + if (tLeft > tRight) return false; + } + + if (directionOpt) { + if (optVelocity.dot(lines[lineNo].direction) > fp(0)) { + result = lines[lineNo].point + lines[lineNo].direction * tRight; + } + else { + result = lines[lineNo].point + lines[lineNo].direction * tLeft; + } + } + else { + const fp t = lines[lineNo].direction.dot(optVelocity - lines[lineNo].point); + const fp tClamped = std::max(tLeft, std::min(tRight, t)); + result = lines[lineNo].point + lines[lineNo].direction * tClamped; + } + + return true; + } + + static int linearProgram2( + const std::vector& lines, + fp radius, + const Vec2fp& optVelocity, + bool directionOpt, + Vec2fp& result) + { + if (directionOpt) { + result = optVelocity * radius; + } + else if (optVelocity.lengthSq() > radius * radius) { + result = (optVelocity / optVelocity.length()) * radius; + } + else { + result = optVelocity; + } + + for (int i = 0; i < static_cast(lines.size()); ++i) { + if (lines[i].direction.cross(lines[i].point - result) > fp(0)) { + const Vec2fp tempResult = result; + if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { + result = tempResult; + return i; + } + } + } + return static_cast(lines.size()); + } + + static void linearProgram3( + const std::vector& lines, + int beginLine, + fp radius, + Vec2fp& result) + { + fp distance = fp(0); + + for (int i = beginLine; i < static_cast(lines.size()); ++i) { + if (lines[i].direction.cross(lines[i].point - result) > distance) { + std::vector projLines; + projLines.reserve(i); + + for (int j = 0; j < i; ++j) { + OrcaLine line; + fp determinant = lines[i].direction.cross(lines[j].direction); + + if (fpm::abs(determinant) <= fp(0.001f)) { + if (lines[i].direction.dot(lines[j].direction) > fp(0)) continue; + line.point = (lines[i].point + lines[j].point) * fp(0.5f); + } + else { + line.point = lines[i].point + lines[i].direction * + (lines[j].direction.cross(lines[i].point - lines[j].point) / determinant); + } + + Vec2fp dirDiff = lines[j].direction - lines[i].direction; + fp dirLen = dirDiff.length(); + if (dirLen <= fp(0.001f)) continue; + + line.direction = dirDiff / dirLen; + projLines.push_back(line); + } + + const Vec2fp tempResult = result; + const Vec2fp optDir(-lines[i].direction.y, lines[i].direction.x); + if (linearProgram2(projLines, radius, optDir, true, result) < static_cast(projLines.size())) { + result = tempResult; + } + + distance = lines[i].direction.cross(lines[i].point - result); + } + } + } + }; + +} // namespace FRG \ No newline at end of file diff --git a/src/game_server/InternalGameServer.cpp b/src/game_server/InternalGameServer.cpp index 2780cfd..018939a 100644 --- a/src/game_server/InternalGameServer.cpp +++ b/src/game_server/InternalGameServer.cpp @@ -520,8 +520,7 @@ void InternalGameServer::update(std::int64_t deltaMs) if (!event.events.navigationRequested) continue; if (event.events.forceNavigationReplan) stored->second.replanCharacter( - event.characterId, state_.players, - event.events.blockingCharacterId); + event.characterId, state_.players); else stored->second.setCharacterDestination( event.characterId, event.events.navigationTarget, {}, diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index cc6ec6e..a16b5f7 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -368,6 +368,17 @@ namespace FRG { bool saveConfig(const std::string& path) const { return true; } bool saveGrid(const std::string& path) const { return true; } + // Внутри public: класса PathFinder + Vec2fp clampToNavMesh(const Vec2fp& pt, int& outTriId) const { + return findClosestPointOnNavMesh(pt, outTriId); + } + + // Удобная перегрузка без outTriId, если индекс треугольника не нужен: + Vec2fp clampToNavMesh(const Vec2fp& pt) const { + int dummyTri = -1; + return findClosestPointOnNavMesh(pt, dummyTri); + } + protected: NavigationArea area; std::vector obstaclePolygons;