Finally restored attacks

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<fp>(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<int>(location.navigationMaps.size())) {
+        walkTarget = Vec3fp(position.x, static_cast<fp>(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)
         {
@@ -144,18 +156,24 @@ void CharacterState::forceReplan(
     const Vec3fp normalizedTarget(
         requestedWalkTarget.x, static_cast<fp>(0.f), requestedWalkTarget.z);
 
-    pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, normalizedTarget);
+    if (location.activeNavigationIndex < 0 ||
+        location.activeNavigationIndex >= static_cast<int>(location.navigationMaps.size())) {
+        walkTarget = Vec3fp(position.x, static_cast<fp>(0.f), position.z);
+        return;
+    }
 
-    currentWaypointIndex = 0;
+    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<fp>(0.f);
         }
         walkTarget = pathWaypoints.back();
-        blockingCharacterId.clear();
         return;
     }
 
@@ -237,120 +255,109 @@ 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) {
+    preferredVelocity = Vec2fp(fp(0), fp(0));
-        Vec3fp toTarget = activeTarget - position;
+
-        toTarget.y = static_cast<fp>(0.f);
+    if (hp > 0.f && battle_state == 0 && !pathWaypoints.empty() &&
-        const fp distanceSquared = toTarget.normSquared();
+        (currentState == AnimationState::WALK || currentState == AnimationState::STAND))
-        if (distanceSquared > kWalkThreshold_SQUARED()) {
+    {
-            fp distance = sqrt(distanceSquared); //FP SQRT
+        // Если мы уже достигли промежуточного waypoint'а, сразу переключаемся на следующий в этом же тике
-            const Vec3fp direction = toTarget / distance;
+        while (!pathWaypoints.empty() && currentWaypointIndex + 1 < pathWaypoints.size()) {
-            const fp moveAmount = walkSpeed * static_cast<fp>(deltaMs) / static_cast < fp>(1000.f);
+            Vec3fp diff = pathWaypoints[currentWaypointIndex] - position;
-            Vec3fp candidate =
+            diff.y = fp(0);
-                moveAmount >= distance ? activeTarget : position + direction * moveAmount;
+            if (diff.normSquared() <= kWalkThreshold_SQUARED()) {
-            candidate.y = static_cast<fp>(0.f);
+                ++currentWaypointIndex;
-            /*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;
+            else {
-                    closest.y() = 0.f;
-                    const bool movingCloser =
-                        endDelta.squaredNorm() + 1e-6f < startDelta.squaredNorm();
-                    if (movingCloser &&
-                        closest.squaredNorm() < minimumDistanceSq) {
-                        blocker = &obstacle;
                 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<fp>(deltaMs) / fp(1000);
+
+            fp desiredSpeed = distance / dtSec;
+            if (desiredSpeed > walkSpeed) {
+                desiredSpeed = walkSpeed;
             }
-            if (blocker) {
+
-                blockingCharacterId = blocker->id;
+            preferredVelocity = Vec2fp(
+                (toTarget.x / distance) * desiredSpeed,
+                (toTarget.z / distance) * desiredSpeed
+            );
+
+            // Если стартуем с места — сразу целим поворот на точку, 
+            // а в движении угол скорректирует Фаза 3 по реальному вектору ORCA (newVel)
+            if (velocity.lengthSq() <= static_cast<fp>(0.05f)) {
+                targetFacingAngle = fpm::atan2(toTarget.x, -toTarget.z);
+            }
+        }
+        else {
+            // Дошли до ФИНАЛЬНОЙ точки маршрута
+            velocity = Vec2fp(fp(0), fp(0));
+            if (currentState == AnimationState::WALK) {
                 currentState = AnimationState::STAND;
-                const bool sameDirection =
+                resetAnim = true;
-                    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<fp>(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) {
-                ++currentWaypointIndex;
-            } else {
             pathWaypoints.clear();
             currentWaypointIndex = 0;
+
             if (completedPath &&
-                    (requestedWalkTarget - position).normSquared() >
+                (requestedWalkTarget - position).normSquared() > TARGET_REPLAN_THRESHOLD_SQUARED &&
-                        TARGET_REPLAN_THRESHOLD_SQUARED &&
+                !isPlayer) {
-                    !isPlayer)
                 scheduleNextNavigationRetry();
+            }
             if (completedPath && !onArrivedCallbackName.empty()) {
                 events.arrivedCallbackName = onArrivedCallbackName;
                 onArrivedCallbackName.clear();
             }
-            }
-
             if (faceTarget) {
                 Vec3fp toFace = faceTarget->position - position;
-                toFace.y = static_cast<fp>(0.f);
+                toFace.y = fp(0);
-                const fp distanceToFace = sqrt(toFace.normSquared()); //FP SQRT
+                const fp distanceToFace = fpm::sqrt(toFace.normSquared());
                 targetFacingAngle = distanceToFace > static_cast<fp>(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<fp>(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 &&
@@ -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<fp>(0.f);
-        const fp lengthSquared = fpm::sqrt(events.attackDirection.normSquared()); 
+        const fp lengthSquared = events.attackDirection.normSquared(); 
-        if (lengthSquared > static_cast<fp>(1e-3f * 1e-3f))
+        if (lengthSquared > static_cast<fp>(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<double>(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());
 }

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<fp>(0.45f);
+    fp collisionRadius = static_cast<fp>(0.4f); // Радиус капсулы персонажа
-    // Smaller physical contact threshold used by the movement guard.
+
-    //fp movementContactRadius = static_cast<fp>(0.32f);
+    // Векторы скорости в плоскости XZ (сериализовать в JSON не обязательно, если считаются каждый тик, 
+    // но для строгого lockstep-снапшота velocity лучше сохранять)
+    Vec2fp velocity{ fp(0), fp(0) };          // Реальная скорость с прошлого тика (нужна соседям в ORCA)
+    Vec2fp preferredVelocity{ fp(0), fp(0) }; // Желаемая скорость к waypoint'у на текущем тике
+
     fp interactionRadius = static_cast<fp>(0.0f);
     bool movedLastTick = false;
 
@@ -132,7 +136,6 @@ public:
     fp homeDriftCheckTimer = static_cast<fp>(5.0f);
     int movementBlockedTicks = 0;
     int navigationRetryTicks = 0;
-    std::string blockingCharacterId;
     std::vector<int> 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;

src/LocationGameState.cpp

@@ -2,6 +2,7 @@
 #include <algorithm>
 #include <cmath>
 #include <stdexcept>
+#include "OrcaSolver.h"
 
 namespace FRG {
 
@@ -126,37 +127,6 @@ const CharacterState* LocationGameState::findCharacter(
 	return nullptr;
 }
 
-/*
-std::vector<PathFinder::DynamicObstacle> LocationGameState::collectPathObstacles(
-	const std::string& requesterId,
-	const PlayerStateMap& players,
-	const std::string& forcedObstacleId) const
-{
-	std::vector<std::pair<std::string, const CharacterState*>> 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<PathFinder::DynamicObstacle> 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,
+	PlayerStateMap& players)
-	const std::string& forcedObstacleId)
 {
 	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()) {
+	character->forceReplan(*location, id);
-			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<fp>(0.f), character->homePosition.z);
-		if ((homeTarget - character->position).normSquared() >
-			CharacterState::kWalkThreshold_SQUARED()) {
-			character->setTarget(
-				homeTarget, {}, *location, id);
-		}
-		return true;
-	}
-	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<std::string> 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
+	//Character update:
-	// after every state has advanced, so vector order cannot suppress an attack.
+	const fp dtSec = static_cast<fp>(deltaMs) / fp(1000);
+
+	// 1. Собираем всех персонажей в единый массив и сортируем по ID
+	// (чтобы даже порядок вызовов CharacterState::update был строго детерминирован!)
+	struct Entry {
+		std::string id;
+		CharacterState* state;
+	};
+	std::vector<Entry> 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<std::string, CharacterObservation> 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<std::string> 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<std::string> 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) {
+	for (auto& entry : allChars) {
-		CharacterState& npc = *npcs.at(id);
+		if (entry.state->isPlayer)
+		{
+			continue;
+		}
+
+		CharacterState& npc = *entry.state;
 		if (!npc.enabled || !npc.canAttack || npc.hp <= 0.f)
 			continue;
 
@@ -429,32 +387,99 @@ LocationSimulationEvents LocationGameState::update(
 			if (toHome.normSquared() > CharacterState::kWalkThreshold_SQUARED() &&
 				(npc.requestedWalkTarget - homeTarget).normSquared() >
 				CharacterState::kWalkThreshold_SQUARED()) {
-				setCharacterDestination(id, homeTarget, {}, players);
+				setCharacterDestination(entry.id, homeTarget, {}, players);
 			}
 		}
 	}
 
-	for (const std::string& id : playerIds)
+	// 2. ФАЗА 1: Обновляем логику каждого персонажа (вычисляется preferredVelocity)
-	{
+	for (auto& entry : allChars) {
-		if (players.at(id)->enabled)
-		{
 		CharacterUpdateContext context;
-			context.characterId = id;
+		context.characterId = entry.id;
-			auto& state = *players.at(id);
+
-			CharacterUpdateEvents events = state.update(deltaMs, context);
+
-			//advance(id, *players.at(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)
+	std::vector<OrcaAgent> orcaAgents;
-	{
+	orcaAgents.reserve(allChars.size());
-		if (npcs.at(id)->enabled)
+
-		{
+	for (auto& entry : allChars) {
-			//advance(id, *npcs.at(id));
+		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<int>(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;

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<PathFinder::DynamicObstacle> 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,
+		PlayerStateMap& players);
-		const std::string& forcedObstacleId = {});
 	void startTaxi();
 	void resetTaxi();
 	LocationSimulationEvents update(

src/OrcaSolver.h

@@ -0,0 +1,419 @@
+#pragma once
+
+#include "navigation/PathFinder.h"
+#include <algorithm>
+#include <string>
+#include <vector>
+
+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<OrcaAgent>& 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<OrcaLine> 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<int>(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<OrcaLine>& 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<OrcaAgent>& 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<OrcaLine>& 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<OrcaLine>& 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<int>(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<int>(lines.size());
+        }
+
+        static void linearProgram3(
+            const std::vector<OrcaLine>& lines,
+            int beginLine,
+            fp radius,
+            Vec2fp& result)
+        {
+            fp distance = fp(0);
+
+            for (int i = beginLine; i < static_cast<int>(lines.size()); ++i) {
+                if (lines[i].direction.cross(lines[i].point - result) > distance) {
+                    std::vector<OrcaLine> 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<int>(projLines.size())) {
+                        result = tempResult;
+                    }
+
+                    distance = lines[i].direction.cross(lines[i].point - result);
+                }
+            }
+        }
+    };
+
+} // namespace FRG

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.characterId, state_.players);
-				event.events.blockingCharacterId);
 		else
 			stored->second.setCharacterDestination(
 				event.characterId, event.events.navigationTarget, {},

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<ObstaclePolygon> obstaclePolygons;