Finally restored attacks

src/Campaign.cpp

@@ -28,6 +28,12 @@ namespace FRG {
 		return Eigen::Vector3f(j[0].get<float>(), j[1].get<float>(), j[2].get<float>());
 	}
 
+
+	static Vec3fp readVec3fp(const json& j) {
+		if (!j.is_array() || j.size() != 3) return Vec3fp();
+		return Vec3fp(static_cast<fp>(j[0].get<double>()), static_cast<fp>(j[1].get<double>()), static_cast<fp>(j[2].get<double>()));
+	}
+
 	bool LocationConfig::fromFile(const std::string& path, const std::string& zipFile, LocationConfig& out) {
 		json j;
 		if (!parseJsonFile(path, zipFile, j)) return false;
@@ -43,7 +49,7 @@ namespace FRG {
 		out.teleportsJsonPath          = j.value("teleports", "");
 		out.triggerZonesJsonPath       = j.value("triggerZones", "");
 		out.lightsJsonPath             = j.value("lights", "");
-		if (j.contains("playerPosition")) out.playerPosition = readVec3(j["playerPosition"]);
+		if (j.contains("playerPosition")) out.playerPosition = readVec3fp(j["playerPosition"]);
 		out.lockInteractiveObjectsDuringTutorial = j.value("lockInteractiveObjectsDuringTutorial", false);
 		out.loadingScreenPath          = j.value("loadingScreen", "");
 

src/Campaign.h

@@ -5,6 +5,7 @@
 #include <optional>
 #include <string>
 #include <vector>
+#include "navigation/PathFinder.h"
 
 namespace FRG {
 
@@ -21,7 +22,7 @@ namespace FRG {
 		std::string teleportsJsonPath;
 		std::string triggerZonesJsonPath;
 		std::string lightsJsonPath;
-		Eigen::Vector3f playerPosition = Eigen::Vector3f::Zero();
+		Vec3fp playerPosition;
 
 		// Where the taxi drives in when the player calls it. No taxi when absent.
 		struct Taxi {

src/Character.cpp

@@ -42,19 +42,9 @@ bool Character::isMoving() const {
     return state && state->isMoving();
 }
 
-Eigen::Vector3f Character::getRequestedWalkTarget() const {
-    auto state = stateSnapshot();
-    return state ? state->getRequestedWalkTarget() : Eigen::Vector3f::Zero();
-}
-
-Eigen::Vector3f Character::getCurrentNavigationTarget() const {
-    auto state = stateSnapshot();
-    return state ? state->getCurrentNavigationTarget() : Eigen::Vector3f::Zero();
-}
-
 Eigen::Vector3f Character::renderPosition() const {
     auto state = stateSnapshot();
-    return state ? interpolatedState(*state).position : Eigen::Vector3f::Zero();
+    return state ? interpolatedState(*state).position.toEigen() : Eigen::Vector3f::Zero();
 }
 
 float Character::getHp() const {
@@ -181,8 +171,8 @@ CharacterState Character::interpolatedState(const CharacterState& state) const {
     const float alpha = (std::max)(0.0f, (std::min)(1.0f, elapsedMs / kSimulationTickMs));
 
     CharacterState result = state;
-    result.position = previousPresentationSample_.position +
+    result.position = Vec3fp::fromEigen(previousPresentationSample_.position +
-        alpha * (currentPresentationSample_.position - previousPresentationSample_.position);
+        alpha * (currentPresentationSample_.position - previousPresentationSample_.position));
 
     constexpr float kPi = 3.14159265358979323846f;
     constexpr float kTwoPi = 2.0f * kPi;
@@ -190,7 +180,7 @@ CharacterState Character::interpolatedState(const CharacterState& state) const {
         previousPresentationSample_.facingAngle;
     while (angleDelta > kPi) angleDelta -= kTwoPi;
     while (angleDelta < -kPi) angleDelta += kTwoPi;
-    result.facingAngle = previousPresentationSample_.facingAngle + alpha * angleDelta;
+    result.facingAngle = static_cast<fp>(previousPresentationSample_.facingAngle + alpha * angleDelta);
     if (previousPresentationSample_.currentState == state.currentState &&
         currentPresentationSample_.currentState == state.currentState) {
         float previousFrame = previousPresentationSample_.animationFrame;
@@ -235,8 +225,8 @@ void Character::update(int64_t deltaMs, const CharacterUpdateEvents* events) {
 
     const auto now = std::chrono::steady_clock::now();
     PresentationSample sample;
-    sample.position = state.position;
+    sample.position = state.position.toEigen();
-    sample.facingAngle = state.facingAngle;
+    sample.facingAngle = static_cast<float>(state.facingAngle);
     sample.animationFrame = state.animationFrame;
     sample.currentState = state.currentState;
     sample.enabled = state.enabled;
@@ -299,8 +289,8 @@ void Character::draw(Renderer& renderer) {
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -353,8 +343,8 @@ void Character::drawGpuSkinning(Renderer& renderer, const CharacterState& state)
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -417,8 +407,8 @@ void Character::drawAttachedWeapon(Renderer& renderer, const CharacterState& sta
     Eigen::Matrix4f currentBone = anim.gpuSkinningShaderData.skinningMatrices[boneIdx] * bindBone;
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
     renderer.TranslateMatrix(Eigen::Vector3f(currentBone.block<3, 1>(0, 3)));
@@ -452,8 +442,8 @@ void Character::drawShadowDepthCpu(Renderer& renderer, const CharacterState& sta
     if (it == animations.end()) return;
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -488,8 +478,8 @@ void Character::drawShadowDepthGpuSkinning(Renderer& renderer, const CharacterSt
     renderer.shaderManager.PushShader(shadowSkinningShader);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -561,8 +551,8 @@ void Character::drawCpuWithShadow(Renderer& renderer, const CharacterState& stat
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -623,8 +613,8 @@ void Character::drawGpuSkinningWithShadow(Renderer& renderer, const CharacterSta
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -726,8 +716,8 @@ void Character::drawNightGpuSkinning(Renderer& renderer,
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -798,8 +788,8 @@ void Character::drawNightCpu(Renderer& renderer,
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -900,8 +890,8 @@ void Character::drawNightGpuSkinningWithShadow(Renderer& renderer,
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -985,8 +975,8 @@ void Character::drawNightCpuWithShadow(Renderer& renderer,
         Environment::CONST_Z_NEAR, Environment::CONST_Z_FAR);
 
     renderer.PushMatrix();
-    renderer.TranslateMatrix({ state.position.x(), state.position.y(), state.position.z() });
+    renderer.TranslateMatrix({ static_cast<float>(state.position.x), static_cast<float>(state.position.y), static_cast<float>(state.position.z) });
-    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-state.facingAngle, Eigen::Vector3f::UnitY())).toRotationMatrix());
+    renderer.RotateMatrix(Eigen::Quaternionf(Eigen::AngleAxisf(-static_cast<float>(state.facingAngle), Eigen::Vector3f::UnitY())).toRotationMatrix());
     renderer.ScaleMatrix(state.modelScale);
     renderer.RotateMatrix(state.modelCorrectionRotation.toRotationMatrix());
 
@@ -1027,7 +1017,7 @@ void Character::setupHitSparks(std::shared_ptr<Texture> sparkTexture) {
 }
 
 void Character::presentDamage(float hp, float maxHp,
-                              const Eigen::Vector3f& attackDirection,
+                              const Vec3fp& attackDirection,
                               const std::string& attackerId) {
     if (onHpChanged)
         onHpChanged(hp, maxHp, attackerId);
@@ -1037,9 +1027,9 @@ void Character::presentDamage(float hp, float maxHp,
     if (!stateSnapshot) return;
     const CharacterState& state = *stateSnapshot;
 
-    Eigen::Vector3f emitPos = state.position + Eigen::Vector3f(0.f, 1.f, 0.f);
+    Eigen::Vector3f emitPos = state.position.toEigen() + Eigen::Vector3f(0.f, 1.f, 0.f);
     hitSparkEmitter.resetEmissionPoints({ emitPos });
-    hitSparkEmitter.setEmissionDirection(attackDirection);
+    hitSparkEmitter.setEmissionDirection(attackDirection.toEigen());
 
     static std::mt19937 gen(std::random_device{}());
     std::uniform_int_distribution<int> countDist(10, 15);
@@ -1065,7 +1055,7 @@ static void drawCharacterNameLabel(
     if (!state.enabled || state.hp <= 0.f || displayName.empty()) return;
 
     Eigen::Vector4f worldPos(
-        state.position.x(), state.position.y() + 1.83f, state.position.z(), 1.f);
+        static_cast<float>(state.position.x), static_cast<float>(state.position.y) + 1.83f, static_cast<float>(state.position.z), 1.f);
     Eigen::Vector4f clip = projectionMatrix * cameraViewMatrix * worldPos;
     if (clip.w() <= 0.f) return;
 
@@ -1126,7 +1116,7 @@ void Character::drawHealthBar(Renderer& renderer,
     if (state.hp <= 0.f) return;
     if (state.initialHp <= 0.f) return;
 
-    Eigen::Vector4f worldPos(state.position.x(), state.position.y() + 1.75f, state.position.z(), 1.f);
+    Eigen::Vector4f worldPos(static_cast<float>(state.position.x), static_cast<float>(state.position.y) + 1.75f, static_cast<float>(state.position.z), 1.f);
     Eigen::Vector4f clip = projectionMatrix * cameraViewMatrix * worldPos;
     if (clip.w() <= 0.f) return;
     float ndcX = clip.x() / clip.w();

src/Character.h

@@ -55,7 +55,7 @@ public:
     void update(int64_t deltaMs, const CharacterUpdateEvents* events = nullptr);
 
     // ---- Damage ----
-    void presentDamage(float hp, float maxHp, const Eigen::Vector3f& attackDirection,
+    void presentDamage(float hp, float maxHp, const Vec3fp& attackDirection,
                        const std::string& attackerId);
 
     // ---- Spark setup ----
@@ -93,8 +93,6 @@ public:
 
     // ---- Query helpers (read immutable snapshots) ----
     bool            isMoving() const;
-    Eigen::Vector3f getRequestedWalkTarget() const;
-    Eigen::Vector3f getCurrentNavigationTarget() const;
     Eigen::Vector3f renderPosition() const;
     float           getHp() const;
 

src/CharacterState.cpp

@@ -9,7 +9,13 @@ namespace {
 // Tick counts are authoritative and therefore independent of render rate.
 constexpr float ATTACK_COOLDOWN_TIME = 1.6f;
 constexpr float HOME_DRIFT_CHECK_INTERVAL = 5.0f;
+fp HOME_DRIFT_CHECK_INTERVAL_FP = static_cast<fp>(HOME_DRIFT_CHECK_INTERVAL);
 constexpr float TARGET_REPLAN_THRESHOLD = 0.25f;
+
+fp TARGET_REPLAN_THRESHOLD_SQUARED = static_cast<fp>(TARGET_REPLAN_THRESHOLD * TARGET_REPLAN_THRESHOLD);
+
+
+
 constexpr float PI = 3.14159265358979323846f;
 constexpr int BLOCKED_REPLAN_TICKS = 3;
 
@@ -25,8 +31,8 @@ std::uint32_t stableIdHash(const std::string& value)
 }
 
 void CharacterState::configureAsPlayer() {
-    walkSpeed = 3.0f;
+    walkSpeed = static_cast<fp>(3.0f);
-    rotationSpeed = 8.0f;
+    rotationSpeed = static_cast < fp>(8.0f);
     modelScale = 1.f;
     modelCorrectionRotation = Eigen::Quaternionf(
         Eigen::AngleAxisf(PI, Eigen::Vector3f::UnitY()) *
@@ -66,12 +72,12 @@ void CharacterState::scheduleNextNavigationRetry()
 }
 
 bool CharacterState::isMoving() const {
-    Eigen::Vector3f toTarget = walkTarget - position;
+    Vec3fp toTarget = walkTarget - position;
-    toTarget.y() = 0.f;
+    toTarget.y = static_cast<fp>(0.f);
-    return !pathWaypoints.empty() || toTarget.norm() > kWalkThreshold;
+    return !pathWaypoints.empty() || toTarget.normSquared() > kWalkThreshold_SQUARED();
 }
 
-Eigen::Vector3f CharacterState::getCurrentNavigationTarget() const {
+Vec3fp CharacterState::getCurrentNavigationTarget() const {
     if (!pathWaypoints.empty() && currentWaypointIndex < pathWaypoints.size()) {
         return pathWaypoints[currentWaypointIndex];
     }
@@ -79,31 +85,34 @@ Eigen::Vector3f CharacterState::getCurrentNavigationTarget() const {
 }
 
 void CharacterState::stopInPlace() {
-    walkTarget          = Eigen::Vector3f(position.x(), 0.f, position.z());
+    walkTarget          = Vec3fp(position.x, static_cast<fp>(0.f), position.z);
     requestedWalkTarget = walkTarget;
     pathWaypoints.clear();
     currentWaypointIndex = 0;
     onArrivedCallbackName.clear();
     movementBlockedTicks = 0;
     navigationRetryTicks = 0;
-    blockingCharacterId.clear();
+    velocity = Vec2fp(fp(0), fp(0));
+    preferredVelocity = Vec2fp(fp(0), fp(0));
 }
 
 void CharacterState::setTarget(
-    const Eigen::Vector3f& target,
+    const Vec3fp& target,
     std::string callbackName,
     const LocationNetworkState& location,
     const std::string& entityId)
 {
-    const Eigen::Vector3f normalizedTarget(target.x(), 0.f, target.z());
+
+
+    const Vec3fp normalizedTarget(target.x, static_cast<fp>(0.f), target.z);
     const bool sameRequestedTarget =
-        (normalizedTarget - requestedWalkTarget).norm() <= TARGET_REPLAN_THRESHOLD;
+        (normalizedTarget - requestedWalkTarget).normSquared() <= TARGET_REPLAN_THRESHOLD_SQUARED;
     const bool alreadyMovingToTarget =
         !pathWaypoints.empty() ||
-        (walkTarget - normalizedTarget).norm() <= TARGET_REPLAN_THRESHOLD;
+        (walkTarget - normalizedTarget).normSquared() <= TARGET_REPLAN_THRESHOLD_SQUARED;
     const bool stoppedAfterFailedPath =
         pathWaypoints.empty() &&
-        (walkTarget - position).norm() <= TARGET_REPLAN_THRESHOLD;
+        (walkTarget - position).normSquared() <= TARGET_REPLAN_THRESHOLD_SQUARED;
     if (callbackName.empty() && sameRequestedTarget &&
         (alreadyMovingToTarget || stoppedAfterFailedPath)) {
         return;
@@ -113,18 +122,30 @@ 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 (Eigen::Vector3f& waypoint : pathWaypoints) waypoint.y() = 0.f;
+        for (Vec3fp& waypoint : pathWaypoints)
+        {
+            waypoint.y = static_cast<fp>(0.f);
+        }
         walkTarget = pathWaypoints.back();
         return;
     }
 
-    walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z());
+    walkTarget = Vec3fp(position.x, static_cast<fp>(0.f), position.z);
     if (!isPlayer) scheduleNextNavigationRetry();
 }
 
@@ -132,22 +153,31 @@ void CharacterState::forceReplan(
     const LocationNetworkState& location,
     const std::string& entityId)
 {
-    const Eigen::Vector3f normalizedTarget(
+    const Vec3fp normalizedTarget(
-        requestedWalkTarget.x(), 0.f, requestedWalkTarget.z());
+        requestedWalkTarget.x, static_cast<fp>(0.f), requestedWalkTarget.z);
-    
-    pathWaypoints = location.navigationMaps[location.activeNavigationIndex].findPathToNearest(this->position, normalizedTarget);
 
-    currentWaypointIndex = 0;
+    if (location.activeNavigationIndex < 0 ||
-    movementBlockedTicks = 0;
+        location.activeNavigationIndex >= static_cast<int>(location.navigationMaps.size())) {
-    navigationRetryTicks = 0;
+        walkTarget = Vec3fp(position.x, static_cast<fp>(0.f), position.z);
-    if (!pathWaypoints.empty()) {
-        for (Eigen::Vector3f& waypoint : pathWaypoints) waypoint.y() = 0.f;
-        walkTarget = pathWaypoints.back();
-        blockingCharacterId.clear();
         return;
     }
 
-    walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z());
+    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) {
+            waypoint.y = static_cast<fp>(0.f);
+        }
+        walkTarget = pathWaypoints.back();
+        return;
+    }
+
+    walkTarget = Vec3fp(position.x, static_cast<fp>(0.f), position.z);
     if (!isPlayer) scheduleNextNavigationRetry();
 }
 
@@ -209,145 +239,134 @@ CharacterUpdateEvents CharacterState::update(
         attack = 0;
         attackTargetId.clear();
     } else {
-        const float distance = (attackTarget->position - position).norm();
+        const fp distanceSquared = (attackTarget->position - position).normSquared();
-        if (distance >= visibleRange) {
+        if (distanceSquared >= visibleRange* visibleRange) {
-            if ((attackTarget->position - requestedWalkTarget).norm() >
+            if ((attackTarget->position - requestedWalkTarget).normSquared() >
-                TARGET_REPLAN_THRESHOLD) {
+                TARGET_REPLAN_THRESHOLD_SQUARED) {
                 events.navigationRequested = true;
                 events.navigationTarget = attackTarget->position;
             }
             battle_state = 0;
-        } else if (distance >= 1.25f) {
+        } else if (distanceSquared >= static_cast<fp>(1.25f*1.25f)) {
-            if ((attackTarget->position - requestedWalkTarget).norm() > TARGET_REPLAN_THRESHOLD) {
+            if ((attackTarget->position - requestedWalkTarget).normSquared() > TARGET_REPLAN_THRESHOLD_SQUARED) {
                 events.navigationRequested = true;
                 events.navigationTarget = attackTarget->position;
             }
             battle_state = 0;
         } else {
             battle_state = 1;
+            velocity = Vec2fp(fp(0), fp(0));
         }
     }
 
-    const Eigen::Vector3f activeTarget = getCurrentNavigationTarget();
+    const Vec3fp activeTarget = getCurrentNavigationTarget();
-    const Eigen::Vector3f lookTarget =
-        battle_state == 1 && attackTarget ? attackTarget->position : activeTarget;
 
-    if (currentState == AnimationState::WALK || currentState == AnimationState::STAND) {
+    preferredVelocity = Vec2fp(fp(0), fp(0));
-        Eigen::Vector3f toTarget = activeTarget - position;
+
-        toTarget.y() = 0.f;
+    if (hp > 0.f && battle_state == 0 && !pathWaypoints.empty() &&
-        const float distance = toTarget.norm();
+        (currentState == AnimationState::WALK || currentState == AnimationState::STAND))
-        if (distance > kWalkThreshold) {
+    {
-            const Eigen::Vector3f direction = toTarget / distance;
+        // Если мы уже достигли промежуточного waypoint'а, сразу переключаемся на следующий в этом же тике
-            const float moveAmount = walkSpeed * static_cast<float>(deltaMs) / 1000.f;
+        while (!pathWaypoints.empty() && currentWaypointIndex + 1 < pathWaypoints.size()) {
-            Eigen::Vector3f candidate =
+            Vec3fp diff = pathWaypoints[currentWaypointIndex] - position;
-                moveAmount >= distance ? activeTarget : position + direction * moveAmount;
+            diff.y = fp(0);
-            candidate.y() = 0.f;
+            if (diff.normSquared() <= kWalkThreshold_SQUARED()) {
-            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).squaredNorm() > 1e-8f;
-                position = candidate;
-                movementBlockedTicks = 0;
-                blockingCharacterId.clear();
-            }
-            targetFacingAngle = std::atan2(direction.x(), -direction.z());
-            if (!blocker) 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();
+            else {
-                currentWaypointIndex = 0;
+                break;
-                if (completedPath &&
+            }
-                    (requestedWalkTarget - position).norm() >
+        }
-                        TARGET_REPLAN_THRESHOLD &&
+
-                    !isPlayer)
+        const Vec3fp currentTarget = getCurrentNavigationTarget();
-                    scheduleNextNavigationRetry();
+        Vec3fp toTarget = currentTarget - position;
-                if (completedPath && !onArrivedCallbackName.empty()) {
+        toTarget.y = fp(0);
-                    events.arrivedCallbackName = onArrivedCallbackName;
+        const fp distanceSquared = toTarget.normSquared();
-                    onArrivedCallbackName.clear();
+
-                }
+        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;
             }
 
+            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;
+                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) {
-                Eigen::Vector3f toFace = faceTarget->position - position;
+                Vec3fp toFace = faceTarget->position - position;
-                toFace.y() = 0.f;
+                toFace.y = fp(0);
-                const float distanceToFace = toFace.norm();
+                const fp distanceToFace = fpm::sqrt(toFace.normSquared());
-                targetFacingAngle = distanceToFace > 1e-3f
+                targetFacingAngle = distanceToFace > static_cast<fp>(1e-3f)
-                    ? std::atan2(toFace.x() / distanceToFace, -toFace.z() / distanceToFace)
+                    ? 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).norm() > TARGET_REPLAN_THRESHOLD &&
+        (requestedWalkTarget - position).normSquared() > TARGET_REPLAN_THRESHOLD_SQUARED &&
         !events.navigationRequested) {
         if (navigationRetryTicks > 0) --navigationRetryTicks;
         if (navigationRetryTicks == 0) {
             events.navigationRequested = true;
             events.forceNavigationReplan = true;
             events.navigationTarget = requestedWalkTarget;
-            events.blockingCharacterId = blockingCharacterId;
         }
     }
 
@@ -358,9 +377,11 @@ CharacterUpdateEvents CharacterState::update(
             resetAnim = true;
             attackTargetId.clear();
         }
-    } else if (battle_state == 1 && attackTarget) {
+    }
-        targetFacingAngle = std::atan2(
+    else if (battle_state == 1 && attackTarget) {
-            lookTarget.x() - position.x(), -(lookTarget.z() - position.z()));
+        targetFacingAngle = fpm::atan2(
+            attackTarget->position.x - position.x, -(attackTarget->position.z - position.z));
+
         if (currentState == AnimationState::STAND || currentState == AnimationState::WALK) {
             currentState = AnimationState::STAND_TO_ACTION;
             resetAnim = true;
@@ -384,24 +405,25 @@ CharacterUpdateEvents CharacterState::update(
         resetAnim = true;
     }
 
-    float angleDiff = targetFacingAngle - facingAngle;
+    fp angleDiff = targetFacingAngle - facingAngle;
-    while (angleDiff > PI) angleDiff -= 2.f * PI;
+    const fp PIfp = static_cast<fp>(PI);
-    while (angleDiff < -PI) angleDiff += 2.f * PI;
+    while (angleDiff > PIfp) angleDiff -= static_cast<fp>(2.f) * PIfp;
-    const float rotationStep = rotationSpeed * static_cast<float>(deltaMs) / 1000.f;
+    while (angleDiff < -PIfp) angleDiff += static_cast<fp>(2.f) * PIfp;
-    if (std::fabs(angleDiff) <= rotationStep) {
+    const fp rotationStep = rotationSpeed * static_cast<fp>(deltaMs) / static_cast<fp>(1000.f);
+    if (fpm::abs(angleDiff) <= rotationStep) {
         facingAngle = targetFacingAngle;
         faceTargetId.clear();
     } else {
-        facingAngle += angleDiff > 0.f ? rotationStep : -rotationStep;
+        facingAngle += angleDiff > static_cast<fp>(0.f) ? rotationStep : -rotationStep;
     }
 
-    if (!isPlayer && hp > 0.f && battle_state == 0 && maxHomeDrift > 0.f) {
+    if (!isPlayer && hp > 0.f && battle_state == 0 && maxHomeDriftSquared > static_cast<fp>(0.f)) {
-        homeDriftCheckTimer -= static_cast<float>(deltaMs) / 1000.f;
+        homeDriftCheckTimer -= static_cast<fp>(deltaMs) / static_cast<fp>(1000.f);
-        if (homeDriftCheckTimer <= 0.f) {
+        if (homeDriftCheckTimer <= static_cast <fp>(0.f)) {
-            homeDriftCheckTimer = HOME_DRIFT_CHECK_INTERVAL;
+            homeDriftCheckTimer = HOME_DRIFT_CHECK_INTERVAL_FP;
-            Eigen::Vector3f toHome = homePosition - position;
+            Vec3fp toHome = homePosition - position;
-            toHome.y() = 0.f;
+            toHome.y = static_cast<fp>(0.f);
-            if (!isMoving() && toHome.norm() > maxHomeDrift) {
+            if (!isMoving() && toHome.normSquared() > maxHomeDriftSquared) {
                 events.navigationRequested = true;
                 events.navigationTarget = homePosition;
             }
@@ -424,10 +446,18 @@ CharacterUpdateEvents CharacterState::update(
         events.attackImpact = true;
         events.attackTargetId = attackTargetId;
         events.attackDirection = attackTarget->position - position;
-        events.attackDirection.y() = 0.f;
+        events.attackDirection.y = static_cast<fp>(0.f);
-        const float length = events.attackDirection.norm();
+        const fp lengthSquared = events.attackDirection.normSquared(); 
-        if (length > 1e-3f) events.attackDirection /= length;
+        if (lengthSquared > static_cast<fp>(0.001f))
-        else events.attackDirection = Eigen::Vector3f::Zero();
+        {
+            const fp length = fpm::sqrt(lengthSquared); //FP SQRT
+
+            events.attackDirection = events.attackDirection / length;
+        }
+        else
+        {
+            events.attackDirection = Vec3fp{};
+        }
     }
 
     showWeapon =
@@ -463,9 +493,9 @@ CharacterUpdateEvents CharacterState::update(
 
 void CharacterState::save(nlohmann::json& out) const
 {
-    out["position"]           = { position.x(), position.y(), position.z() };
+    out["position"]           = { static_cast<double>(position.x), static_cast<double>(position.y), static_cast<double>(position.z) };
-    out["facingAngle"]        = facingAngle;
+    out["facingAngle"]        = static_cast<double>(facingAngle);
-    out["targetFacingAngle"]  = targetFacingAngle;
+    out["targetFacingAngle"]  = static_cast<double>(targetFacingAngle);
     out["hp"]                 = hp;
     out["enabled"]            = enabled;
     out["battle_state"]       = battle_state;
@@ -474,25 +504,24 @@ void CharacterState::save(nlohmann::json& out) const
     out["showWeapon"]         = showWeapon;
     out["attackTargetId"]     = attackTargetId;
     out["faceTargetId"]       = faceTargetId;
-    out["homePosition"]       = { homePosition.x(), homePosition.y(), homePosition.z() };
+    out["homePosition"]       = { static_cast<double>(homePosition.x), static_cast<double>(homePosition.y), static_cast<double>(homePosition.z) };
-    out["walkTarget"]         = { walkTarget.x(), walkTarget.y(), walkTarget.z() };
+    out["walkTarget"]         = { static_cast<double>(walkTarget.x), static_cast<double>(walkTarget.y), static_cast<double>(walkTarget.z) };
-    out["requestedWalkTarget"] = { requestedWalkTarget.x(), requestedWalkTarget.y(), requestedWalkTarget.z() };
+    out["requestedWalkTarget"] = { static_cast<double>(requestedWalkTarget.x), static_cast<double>(requestedWalkTarget.y), static_cast<double>(requestedWalkTarget.z) };
     nlohmann::json wpArr = nlohmann::json::array();
     for (const auto& wp : pathWaypoints) {
-        wpArr.push_back({ wp.x(), wp.y(), wp.z() });
+        wpArr.push_back({ static_cast<double>(wp.x), static_cast<double>(wp.y), static_cast<double>(wp.z) });
     }
     out["pathWaypoints"]        = std::move(wpArr);
     out["currentWaypointIndex"] = currentWaypointIndex;
-    out["homeDriftCheckTimer"]  = homeDriftCheckTimer;
+    out["homeDriftCheckTimer"]  = static_cast<double>(homeDriftCheckTimer);
     out["movementBlockedTicks"] = movementBlockedTicks;
     out["navigationRetryTicks"] = navigationRetryTicks;
     out["movedLastTick"] = movedLastTick;
     out["navigationRetryCooldownTicks"] = navigationRetryCooldownTicks;
     out["navigationRetryCooldownIndex"] = navigationRetryCooldownIndex;
-    if (!blockingCharacterId.empty())
+
-        out["blockingCharacterId"] = blockingCharacterId;
     out["animationFrame"]       = animationFrame;
-    out["visibleRange"]         = visibleRange;
+    out["visibleRange"]         = static_cast<double>(visibleRange);
     if (!onArrivedCallbackName.empty())
         out["onArrivedCallbackName"] = onArrivedCallbackName;
 }
@@ -505,11 +534,19 @@ void CharacterState::load(const nlohmann::json& in)
         return def;
     };
 
-    if (in.contains("position"))            position           = readVec3(in["position"], position);
+    auto readVec3fp = [](const nlohmann::json& j, Vec3fp def) -> Vec3fp {
-    facingAngle        = in.value("facingAngle",       facingAngle);
+        if (j.is_array() && j.size() == 3)
-    targetFacingAngle  = in.value("targetFacingAngle", targetFacingAngle);
+            return Vec3fp{ static_cast<fp>(j[0].get<double>()), static_cast<fp>(j[1].get<double>()), static_cast<fp>(j[2].get<double>()) };
+        return def;
+        };
+
+    if (in.contains("position")) { 
+        position = readVec3fp(in["position"], position); 
+    }
+    facingAngle        = static_cast<fp>(in.value("facingAngle",       static_cast<double>(facingAngle)));
+    targetFacingAngle  = static_cast<fp>(in.value("targetFacingAngle", static_cast<double>(targetFacingAngle)));
     hp                 = in.value("hp",                hp);
-    visibleRange       = in.value("visibleRange",      visibleRange);
+    visibleRange       = static_cast<fp>(in.value("visibleRange", static_cast<double>(visibleRange)));
     enabled            = in.value("enabled",           enabled);
     battle_state       = in.value("battle_state",      battle_state);
     currentState       = static_cast<AnimationState>(in.value("currentState", static_cast<int>(currentState)));
@@ -517,17 +554,17 @@ void CharacterState::load(const nlohmann::json& in)
     showWeapon         = in.value("showWeapon",        showWeapon);
     attackTargetId = in.value("attackTargetId", attackTargetId);
     faceTargetId   = in.value("faceTargetId",   faceTargetId);
-    if (in.contains("homePosition"))        homePosition       = readVec3(in["homePosition"], homePosition);
+    if (in.contains("homePosition"))        homePosition       = readVec3fp(in["homePosition"], homePosition);
-    if (in.contains("walkTarget"))          walkTarget         = readVec3(in["walkTarget"], walkTarget);
+    if (in.contains("walkTarget"))          walkTarget         = readVec3fp(in["walkTarget"], walkTarget);
-    if (in.contains("requestedWalkTarget")) requestedWalkTarget = readVec3(in["requestedWalkTarget"], requestedWalkTarget);
+    if (in.contains("requestedWalkTarget")) requestedWalkTarget = readVec3fp(in["requestedWalkTarget"], requestedWalkTarget);
     pathWaypoints.clear();
     if (in.contains("pathWaypoints") && in["pathWaypoints"].is_array()) {
         for (const auto& wp : in["pathWaypoints"]) {
-            pathWaypoints.push_back(readVec3(wp, Eigen::Vector3f::Zero()));
+            pathWaypoints.push_back(readVec3fp(wp, Vec3fp()));
         }
     }
     currentWaypointIndex = in.value("currentWaypointIndex", currentWaypointIndex);
-    homeDriftCheckTimer  = in.value("homeDriftCheckTimer",  homeDriftCheckTimer);
+    homeDriftCheckTimer  = static_cast<fp>(in.value("homeDriftCheckTimer", static_cast<double>(homeDriftCheckTimer)));
     movementBlockedTicks = in.value("movementBlockedTicks", movementBlockedTicks);
     navigationRetryTicks = in.value("navigationRetryTicks", navigationRetryTicks);
     movedLastTick = in.value("movedLastTick", movedLastTick);
@@ -537,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

@@ -61,6 +61,9 @@ public:
     // Distance below which the character is considered to have reached its target.
     static constexpr float kWalkThreshold = 0.05f;
 
+    static fp kWalkThreshold_SQUARED() { return static_cast<fp>(kWalkThreshold * kWalkThreshold); };
+
+    
     // --- Identity ---
     std::string npcName;
     bool isPlayer  = false;
@@ -68,15 +71,15 @@ public:
     bool canAttack = false;
 
     // --- Position / orientation ---
-    Eigen::Vector3f position          = Eigen::Vector3f::Zero();
+    Vec3fp position;
-    float           facingAngle       = 0.0f;
+    fp           facingAngle;
-    float           targetFacingAngle = 0.0f;
+    fp           targetFacingAngle;
 
     // --- Health ---
     float hp        = 200.f;
     float initialHp = 200.f;
 
-    float visibleRange = VISIBLE_RANGE;
+    fp visibleRange = static_cast<fp>(VISIBLE_RANGE);
 
     // --- Combat flags ---
     int            battle_state    = 0;
@@ -95,8 +98,8 @@ public:
     std::string faceTargetId;
 
     // --- Home position for drift recovery ---
-    Eigen::Vector3f homePosition = Eigen::Vector3f::Zero();
+    Vec3fp homePosition;
-    float maxHomeDrift = 3.0f;
+    fp maxHomeDriftSquared = static_cast<fp>(3.f*3.f);
 
     // --- Authoritative logical animation clock ---
     // Frame counts are immutable metadata copied from loaded clips during
@@ -106,29 +109,33 @@ public:
     std::array<int, 9> animationFrameCounts = {{1, 1, 1, 1, 1, 1, 1, 1, 1}};
 
     // --- Movement / visual parameters ---
-    float walkSpeed      = 3.0f;
+    fp walkSpeed = static_cast<fp>(3.0f);
-    float rotationSpeed  = 4.0f;
+    fp rotationSpeed = static_cast<fp>(4.0f);
+
     float modelScale     = 0.12f;
     Eigen::Quaternionf modelCorrectionRotation = Eigen::Quaternionf::Identity();
     bool  useGpuSkinning  = true;
-    // Full clearance used while planning around stationary characters.
+
-    float collisionRadius = 0.45f;
+    fp collisionRadius = static_cast<fp>(0.4f); // Радиус капсулы персонажа
-    // Smaller physical contact threshold used by the movement guard.
+
-    float movementContactRadius = 0.32f;
+    // Векторы скорости в плоскости XZ (сериализовать в JSON не обязательно, если считаются каждый тик, 
-    float interactionRadius = 0.0f;
+    // но для строгого 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;
 
     // --- Pathfinding data (serializable parts) ---
-    Eigen::Vector3f walkTarget          = Eigen::Vector3f::Zero();
+    Vec3fp walkTarget;
-    Eigen::Vector3f requestedWalkTarget = Eigen::Vector3f::Zero();
+    Vec3fp requestedWalkTarget;
-    std::vector<Eigen::Vector3f> pathWaypoints;
+    std::vector<Vec3fp> pathWaypoints;
     size_t currentWaypointIndex = 0;
 
     // Internal drift-check timer (serializable so it survives save/load).
-    float homeDriftCheckTimer = 5.0f;
+    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;
 
@@ -145,10 +152,10 @@ public:
                                      const std::string& entityId,
                                      std::vector<int> cooldownTicks = {});
     bool                   isMoving()                   const;
-    const Eigen::Vector3f& getRequestedWalkTarget()     const { return requestedWalkTarget; }
+    const Vec3fp& getRequestedWalkTarget()     const { return requestedWalkTarget; }
-    Eigen::Vector3f        getCurrentNavigationTarget()  const;
+    Vec3fp       getCurrentNavigationTarget()  const;
     void                   stopInPlace();
-    void                   setTarget(const Eigen::Vector3f& target,
+    void                   setTarget(const Vec3fp& target,
                                      std::string callbackName,
                                      const LocationNetworkState& location,
                                      const std::string& entityId);
@@ -173,23 +180,14 @@ using PlayerStateMap =
     std::unordered_map<std::string, std::shared_ptr<CharacterState>>;
 
 struct CharacterObservation {
-    Eigen::Vector3f position = Eigen::Vector3f::Zero();
+    Vec3fp position;
     float hp = 0.f;
 };
 
-struct CharacterMovementObstacle {
-    std::string id;
-    Eigen::Vector3f position = Eigen::Vector3f::Zero();
-    float radius = 0.f;
-    bool moving = false;
-    Eigen::Vector3f movementDirection = Eigen::Vector3f::Zero();
-};
-
 struct CharacterUpdateContext {
     std::string characterId;
     const CharacterObservation* attackTarget = nullptr;
     const CharacterObservation* faceTarget = nullptr;
-    const std::vector<CharacterMovementObstacle>* movementObstacles = nullptr;
 };
 
 struct CharacterUpdateEvents {
@@ -197,11 +195,11 @@ struct CharacterUpdateEvents {
     bool deathAnimationComplete = false;
     bool navigationRequested = false;
     bool forceNavigationReplan = false;
-    Eigen::Vector3f navigationTarget = Eigen::Vector3f::Zero();
+    Vec3fp navigationTarget;
-    std::string blockingCharacterId;
+
     std::string arrivedCallbackName;
     std::string attackTargetId;
-    Eigen::Vector3f attackDirection = Eigen::Vector3f::Zero();
+    Vec3fp attackDirection;
 };
 
 } // namespace FRG

src/Game.cpp

@@ -664,9 +664,9 @@ namespace FRG
 			if (!loc) return;
 			if (loc->player) {
 				server().submit(SetCharacterPositionCommand{
-					destName, loc->player->entityId(), destPos});
+					destName, loc->player->entityId(), Vec3fp::fromEigen(destPos)});
 				server().submit(SetCharacterFacingAngleCommand{
-					destName, loc->player->entityId(), destRotY, true});
+					destName, loc->player->entityId(), static_cast<fp>(destRotY), true});
 				loc->stopCharacter(*loc->player);
 			}
 			loc->localState().cameraAzimuth = destRotY;

src/Location.cpp

@@ -126,7 +126,7 @@ namespace FRG
 	}
 
 	void Location::setCharacterTarget(
-		Character& character, const Eigen::Vector3f& target, std::string callbackName)
+		Character& character, const Vec3fp& target, std::string callbackName)
 	{
 		gameServer_.submit(SetCharacterDestinationCommand{
 			locationId_, character.entityId(), target, std::move(callbackName)});
@@ -1597,7 +1597,7 @@ namespace FRG
 		if (playerState && !pointLights.empty()) {
 			float minDist = FLT_MAX;
 			for (int i = 0; i < static_cast<int>(pointLights.size()); ++i) {
-				const float d = (pointLights[i].position - playerState->position).norm();
+				const float d = (pointLights[i].position - playerState->position.toEigen()).norm();
 				if (d < minDist) { minDist = d; nearestLightIdx = i; }
 			}
 		}
@@ -1611,12 +1611,12 @@ namespace FRG
 			if (!active && playerState) {
 				if (pl.autoLightLimitX > 0.0f && pl.autoLightLimitY > 0.0f) {
 					// Rectangular activation zone — independent of nearest-light heuristic
-					const float dx = std::abs(playerState->position.x() - pl.position.x());
+					const float dx = std::abs(static_cast<float>(playerState->position.x) - pl.position.x());
-					const float dz = std::abs(playerState->position.z() - pl.position.z());
+					const float dz = std::abs(static_cast<float>(playerState->position.z) - pl.position.z());
 					active = (dx <= pl.autoLightLimitX && dz <= pl.autoLightLimitY);
 				} else {
 					// only the nearest autoLight activates, with optional distance cap
-					const float dist = (pl.position - playerState->position).norm();
+					const float dist = (pl.position - playerState->position.toEigen()).norm();
 					active = (i == nearestLightIdx) &&
 					         (pl.autoLightDistance <= 0.0f || dist <= pl.autoLightDistance);
 				}
@@ -1632,7 +1632,7 @@ namespace FRG
 		if (playerState && !activeLightIndices.empty()) {
 			float minDist = FLT_MAX;
 			for (int idx : activeLightIndices) {
-				const float d = (pointLights[idx].position - playerState->position).norm();
+				const float d = (pointLights[idx].position - playerState->position.toEigen()).norm();
 				if (d < minDist) { minDist = d; shadowLight = &pointLights[idx]; }
 			}
 		}
@@ -1850,37 +1850,6 @@ namespace FRG
 		return true;
 	}
 
-	void Location::nudgeCharacterAside(Character* standing, const Eigen::Vector3f& awayFrom)
-	{
-		auto standingState = standing ? standing->stateSnapshot() : nullptr;
-		if (!standingState || standingState->isPlayer) return;
-		auto locationState = stateSnapshot();
-		if (!locationState || locationState->activeNavigationIndex < 0 ||
-			locationState->activeNavigationIndex >=
-				static_cast<int>(locationState->navigationMaps.size())) return;
-		const PathFinder& navigation =
-			locationState->navigationMaps[locationState->activeNavigationIndex];
-		if (navigation.isReady()) return;
-
-		static constexpr float kNudgeDist = 1.2f;
-
-		Eigen::Vector3f dir = standingState->position - awayFrom;
-		dir.y() = 0.f;
-		if (dir.norm() < 1e-3f) dir = Eigen::Vector3f(1.f, 0.f, 0.f);
-		else dir.normalize();
-
-		const float angles[] = { 0.f, static_cast<float>(M_PI * 0.5), static_cast<float>(-M_PI * 0.5), static_cast<float>(M_PI) };
-		for (float angle : angles) {
-			Eigen::Vector3f rotated = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitY()) * dir;
-			Eigen::Vector3f candidate = standingState->position + rotated * kNudgeDist;
-			candidate.y() = 0.f;
-			if (navigation.isWalkable(candidate)) {
-				setCharacterTarget(*standing, candidate);
-				return;
-			}
-		}
-	}
-
 	void Location::update(int64_t delta, const LocationSimulationEvents& events)
 	{
 		synchronizeRemotePlayers();
@@ -1951,7 +1920,7 @@ namespace FRG
 			player->update(delta, eventFor(gameServer_.localPlayerId()));
 			auto playerState = player->stateSnapshot();
 			if (playerState && playerState->enabled)
-				updateTriggerZones(playerState->position);
+				updateTriggerZones(playerState->position.toEigen());
 		}
 		for (auto& [id, remote] : remotePlayers_)
 			if (remote) remote->update(delta, eventFor(id));
@@ -1988,7 +1957,7 @@ namespace FRG
 		auto playerState = player ? player->stateSnapshot() : nullptr;
 		if (auto* tz = getTargetTeleportZone(*locationState);
 			tz && playerState && playerState->enabled) {
-			float dist = (playerState->position - tz->position).norm();
+			float dist = (playerState->position.toEigen() - tz->position).norm();
 			if (dist <= tz->radius) {
 				logger() << "[TELEPORT] Player reached teleport zone '" << tz->id << "'" << std::endl;
 				gameServer_.submit(SetLocationTargetsCommand{
@@ -2005,7 +1974,7 @@ namespace FRG
 			for (auto& tz : teleportZones) {
 				if (!tz.active || !tz.automatic) continue;
 				const float autoR = tz.automaticRadius > 0.0f ? tz.automaticRadius : tz.radius;
-				const float dist = (playerState->position - tz.position).norm();
+				const float dist = (playerState->position.toEigen() - tz.position).norm();
 
 				if (!tz.automaticInitialized) {
 					// If the player starts inside the zone (e.g. just arrived here via teleport),
@@ -2101,7 +2070,7 @@ namespace FRG
 			const Eigen::Vector3f interactionTarget = clickedObject->state.hasInteractionPosition
 				? clickedObject->state.interactionPosition
 				: clickedObject->state.position;
-			setCharacterTarget(*player, interactionTarget);
+			setCharacterTarget(*player, Vec3fp::fromEigen(interactionTarget));
 			pathVisualization.start(*player, interactionTarget);
 			clearCharacterAttackTarget(*player);
 			//logger() << "[CLICK] Player moving to object..." << std::endl;
@@ -2114,7 +2083,7 @@ namespace FRG
 			if (clickedNpc && player) {
 				auto clickedNpcState = clickedNpc->stateSnapshot();
 				if (!clickedNpcState) return;
-				float distance = (playerState->position - clickedNpcState->position).norm();
+				float distance = (playerState->position - clickedNpcState->position).toEigen().norm();
 				int npcIndex = -1;
 				for (size_t i = 0; i < npcs.size(); ++i) {
 					if (npcs[i].get() == clickedNpc) {
@@ -2144,11 +2113,11 @@ namespace FRG
 							setCharacterTarget(*player, clickedNpcState->position);
 							pathVisualization.start(
 								*player,
-								clickedNpcState->position,
+								clickedNpcState->position.toEigen(),
 								PathVisualization::Style::Hostile);
 						}
 
-						if (distance <= clickedNpcState->interactionRadius) {
+						if (distance <= static_cast<float>(clickedNpcState->interactionRadius)) {
 							//logger() << "[CLICK] Hostile NPC " << npcIndex << " in range; firing on_npc_interact" << std::endl;
 							scriptEngine.callNpcInteractCallback(clickedNpc->entityId());
 						}
@@ -2172,7 +2141,7 @@ namespace FRG
 							//logger() << "[CLICK] NPC " << npcIndex << " out of talk range (distance " << distance
 							//	<< " > " << NPC_TALK_DISTANCE << "); walking to NPC..." << std::endl;
 							setCharacterTarget(*player, clickedNpcState->position);
-							pathVisualization.start(*player, clickedNpcState->position);
+							pathVisualization.start(*player, clickedNpcState->position.toEigen());
 							gameServer_.submit(SetLocationTargetsCommand{
 								locationId_, std::string(), clickedNpc->entityId(),
 								std::string()});
@@ -2199,7 +2168,7 @@ namespace FRG
 					gameServer_.submit(SetLocationTargetsCommand{
 						locationId_, std::string(), std::string(),
 						clickedTeleport->id});
-					setCharacterTarget(*player, clickedTeleport->position);
+					setCharacterTarget(*player, Vec3fp::fromEigen(clickedTeleport->position));
 					pathVisualization.start(
 						*player,
 						clickedTeleport->position,
@@ -2207,7 +2176,7 @@ namespace FRG
 					clearCharacterAttackTarget(*player);
 				} else {
 					const Eigen::Vector3f clickedDestination(hit.x(), 0.f, hit.z());
-					setCharacterTarget(*player, clickedDestination);
+					setCharacterTarget(*player, Vec3fp::fromEigen(clickedDestination));
 					pathVisualization.start(*player, clickedDestination);
 					clearCharacterAttackTarget(*player);
 					gameServer_.submit(SetLocationTargetsCommand{

src/Location.h

@@ -64,7 +64,7 @@ namespace FRG
 		std::string teleportsJsonPath;
 		std::string triggerZonesJsonPath;
 		std::string lightsJsonPath;
-		Eigen::Vector3f playerPosition = Eigen::Vector3f::Zero();
+		Vec3fp playerPosition;
 		// Player shared by all locations of a campaign (see Location::createPlayer);
 		// setup() creates a private one when this is null.
 		std::shared_ptr<Character> player;
@@ -174,7 +174,7 @@ namespace FRG
 		void setup(const LocationSetup& params, AudioPlayerAsync* audioPlayer);
 		//void setupNavigation(const std::vector<std::string>& paths);
 		bool switchNavigation(int index);
-		void setCharacterTarget(Character& character, const Eigen::Vector3f& target,
+		void setCharacterTarget(Character& character, const Vec3fp& target,
 			std::string callbackName = "");
 		void clearCharacterAttackTarget(Character& character);
 		void stopCharacter(Character& character);
@@ -259,7 +259,6 @@ namespace FRG
 		void loadTriggerZones(const std::string& jsonPath, const char* zipFile);
 		void loadPointLights(const std::string& jsonPath, const char* zipFile);
 		void updateTriggerZones(const Eigen::Vector3f& playerPos);
-		void nudgeCharacterAside(Character* standing, const Eigen::Vector3f& awayFrom);
 		void captureAuxiliaryState(LocationGameState& out) const;
 		void applyAuxiliaryState(const LocationGameState& in);
 

src/LocationEditor.cpp

@@ -484,9 +484,9 @@ namespace FRG
         data.rotationZ   = 0.0f;
         auto playerState = loc.player ? loc.player->stateSnapshot() : nullptr;
         if (!playerState) return;
-        data.positionX   = playerState->position.x();
+        data.positionX   = static_cast<float>(playerState->position.x);
-        data.positionY = playerState->position.y();//-0.1;
+        data.positionY = static_cast<float>(playerState->position.y);
-        data.positionZ   = playerState->position.z();
+        data.positionZ   = static_cast<float>(playerState->position.z);
         data.scale       = scaleDist(rng);
 
         GameObjectData data2;

src/LocationGameState.cpp

@@ -2,6 +2,7 @@
 #include <algorithm>
 #include <cmath>
 #include <stdexcept>
+#include "OrcaSolver.h"
 
 namespace FRG {
 
@@ -126,40 +127,9 @@ 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 Eigen::Vector3f& target,
+	const Vec3fp& target,
 	std::string callbackName,
 	PlayerStateMap& players,
 	const std::string& forcedObstacleId)
@@ -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 Eigen::Vector3f homeTarget(
-			character->homePosition.x(), 0.f, character->homePosition.z());
-		if ((homeTarget - character->position).norm() >
-			CharacterState::kWalkThreshold) {
-			character->setTarget(
-				homeTarget, {}, *location, id);
-		}
-		return true;
-	}
-	character->forceReplan(
-		*location, id);
 	return true;
 }
 
@@ -234,14 +171,14 @@ std::string LocationGameState::selectNpcTarget(
 			playerIds.push_back(id);
 	std::sort(playerIds.begin(), playerIds.end());
 
-	const float visibleRangeSquared = npc.visibleRange * npc.visibleRange;
+	const fp visibleRangeSquared = npc.visibleRange * npc.visibleRange;
 	std::string selectedTarget;
-	float nearestDistanceSquared = visibleRangeSquared;
+	fp nearestDistanceSquared = visibleRangeSquared;
 	for (const std::string& playerId : playerIds) {
-		const Eigen::Vector3f delta =
+		const Vec3fp delta =
 			players.at(playerId)->position - npc.position;
-		const float distanceSquared =
+		const fp distanceSquared =
-			delta.x() * delta.x() + delta.z() * delta.z();
+			delta.x * delta.x + delta.z * delta.z;
 		if (distanceSquared <= nearestDistanceSquared &&
 			(selectedTarget.empty() ||
 			 distanceSquared < nearestDistanceSquared ||
@@ -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;
 
@@ -423,111 +381,105 @@ LocationSimulationEvents LocationGameState::update(
 		if (selectedTarget.empty()) {
 			npc.battle_state = 0;
 			npc.attack = 0;
-			const Eigen::Vector3f homeTarget(
+			const Vec3fp homeTarget(
-				npc.homePosition.x(), 0.f, npc.homePosition.z());
+				npc.homePosition.x, static_cast<fp>(0.f), npc.homePosition.z);
-			const Eigen::Vector3f toHome = homeTarget - npc.position;
+			const Vec3fp toHome = homeTarget - npc.position;
-			if (toHome.norm() > CharacterState::kWalkThreshold &&
+			if (toHome.normSquared() > CharacterState::kWalkThreshold_SQUARED() &&
-				(npc.requestedWalkTarget - homeTarget).norm() >
+				(npc.requestedWalkTarget - homeTarget).normSquared() >
-					CharacterState::kWalkThreshold) {
+				CharacterState::kWalkThreshold_SQUARED()) {
-				setCharacterDestination(id, homeTarget, {}, players);
+				setCharacterDestination(entry.id, homeTarget, {}, players);
 			}
 		}
 	}
-	/*
-	std::vector<CharacterMovementObstacle> movementObstacles;
-	movementObstacles.reserve(playerIds.size() + npcIds.size());
-	auto addMovementObstacle =
-		[&movementObstacles](const std::string& id,
-							 const CharacterState& character) {
-			Eigen::Vector3f direction =
-				character.getCurrentNavigationTarget() - character.position;
-			direction.y() = 0.f;
-			const float length = direction.norm();
-			if (length > CharacterState::kWalkThreshold) direction /= length;
-			else direction = Eigen::Vector3f::Zero();
-			movementObstacles.push_back({
-				id,
-				character.position,
-				character.movementContactRadius,
-				character.movedLastTick,
-				direction});
-		};
-	for (const std::string& id : playerIds) {
-		const CharacterState& character = *players.at(id);
-		if (character.enabled && character.hp > 0.f)
-			addMovementObstacle(id, character);
-	}
-	for (const std::string& id : npcIds) {
-		const CharacterState& character = *npcs.at(id);
-		if (character.enabled && character.hp > 0.f)
-			addMovementObstacle(id, character);
-	}
-	// One global ID order also makes blocker selection deterministic.
-	std::sort(
-		movementObstacles.begin(), movementObstacles.end(),
-		[](const CharacterMovementObstacle& lhs,
-		   const CharacterMovementObstacle& rhs) { return lhs.id < rhs.id; });*/
 
-	/*
+	// 2. ФАЗА 1: Обновляем логику каждого персонажа (вычисляется preferredVelocity)
-	auto advance = [&](const std::string& id, CharacterState& state) {
+	for (auto& entry : allChars) {
 		CharacterUpdateContext context;
-		context.characterId = id;
+		context.characterId = entry.id;
-		context.movementObstacles = &movementObstacles;
+
-		auto attack = observations.find(state.attackTargetId);
+
+		auto attack = observations.find(entry.state->attackTargetId);
 		if (attack != observations.end()) context.attackTarget = &attack->second;
-		auto face = observations.find(state.faceTargetId);
+		auto face = observations.find(entry.state->faceTargetId);
 		if (face != observations.end()) context.faceTarget = &face->second;
-		CharacterUpdateEvents events = state.update(deltaMs, context);
+
-		auto reservation = std::lower_bound(
+		CharacterUpdateEvents events = entry.state->update(deltaMs, context);
-			movementObstacles.begin(), movementObstacles.end(), id,
+
-			[](const CharacterMovementObstacle& obstacle,
-			   const std::string& value) { return obstacle.id < value; });
-		if (reservation != movementObstacles.end() && reservation->id == id) {
-			reservation->position = state.position;
-			reservation->moving = state.movedLastTick;
-			Eigen::Vector3f direction =
-				state.getCurrentNavigationTarget() - state.position;
-			direction.y() = 0.f;
-			const float length = direction.norm();
-			if (length > CharacterState::kWalkThreshold)
-				reservation->movementDirection = direction / length;
-			else
-				reservation->movementDirection = Eigen::Vector3f::Zero();
-		}
 		if (!events.arrivedCallbackName.empty() || events.attackImpact ||
 			events.deathAnimationComplete || events.navigationRequested) {
-			result.characters.push_back({id, std::move(events)});
+			result.characters.push_back({ entry.id, std::move(events) });
-		}
-	};*/
-	/*
-	for (const std::string& id : playerIds)
-		if (players.at(id)->enabled)
-			advance(id, *players.at(id));
-	for (const std::string& id : npcIds)
-		if (npcs.at(id)->enablesd)
-			advance(id, *npcs.at(id));*/
-
-	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));
 		}
 	}
 
-	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;
@@ -541,44 +493,9 @@ LocationSimulationEvents LocationGameState::update(
 			damage,
 			target->hp,
 			target->initialHp,
-			source.events.attackDirection});
+			source.events.attackDirection });
 	}
 
-	/*
-	auto player = players.find("player1");
-	if (!interactiveObjectsLocked &&
-		!location->targetInteractiveObjectId.empty() &&
-		player != players.end() && player->second && player->second->enabled) {
-		auto target = interactiveObjects.find(location->targetInteractiveObjectId);
-		const auto object =
-			target == interactiveObjects.end() ? nullptr : target->second;
-		if (object && !object->isAnimating) {
-			const Eigen::Vector3f& approachTarget = object->hasInteractionPosition
-				? object->interactionPosition : object->position;
-			if ((player->second->position - approachTarget).norm() <= object->approachRadius) {
-				result.interactiveObjects.push_back(
-					{location->targetInteractiveObjectId, {}, true,
-					 object->activateFunctionName});
-				location->targetInteractiveObjectId.clear();
-			}
-		}
-	}
-
-	if (!location->targetInteractNpcId.empty() &&
-		player != players.end() && player->second && player->second->enabled &&
-		npcs.count(location->targetInteractNpcId) != 0 &&
-		npcs.at(location->targetInteractNpcId)) {
-		const auto& npc = npcs.at(location->targetInteractNpcId);
-		const float distance =
-			(player->second->position - npc->position).norm();
-		if (distance <= NPC_TALK_DISTANCE) {
-			player->second->stopInPlace();
-			npc->faceTargetId = "player1";
-			result.npcInteractionId = location->targetInteractNpcId;
-			location->targetInteractNpcId.clear();
-		}
-	}*/
-
 	return result;
 }
 

src/LocationGameState.h

@@ -27,7 +27,7 @@ struct CharacterDamageEvent {
 	float amount = 0.f;
 	float hp = 0.f;
 	float maxHp = 0.f;
-	Eigen::Vector3f direction = Eigen::Vector3f::Zero();
+	Vec3fp direction;
 };
 
 struct InteractiveObjectSimulationEvent {
@@ -64,20 +64,15 @@ 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 Eigen::Vector3f& target,
+		const Vec3fp& target,
 		std::string callbackName,
 		PlayerStateMap& players,
 		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/ScriptEngine.cpp

@@ -13,6 +13,9 @@
 
 namespace FRG {
 
+    constexpr float kPi = 3.14159265358979323846f;
+
+
     namespace {
 
     // Reverse-looks-up the global name a Lua function is registered under, by
@@ -126,9 +129,9 @@ namespace FRG {
                         std::cerr << "[script] npc_walk_to: on_arrived must be a global Lua function\n";
                 }
                 loc->gameServer().submit(SetCharacterHomePositionCommand{
-                    loc->locationId(), npc->entityId(), Eigen::Vector3f(x, 0.f, z)});
+                    loc->locationId(), npc->entityId(), Vec3fp(static_cast<fp>(x), static_cast<fp>(0.f), static_cast<fp>(z))});
                 loc->setCharacterTarget(
-                    *npc, Eigen::Vector3f(x, y, z), std::move(cbName));
+                    *npc, Vec3fp(static_cast<fp>(x), static_cast<fp>(y), static_cast<fp>(z)), std::move(cbName));
             });
 
 
@@ -140,16 +143,16 @@ namespace FRG {
                     return;
                 }
                 
-                const float rad = angle * static_cast<float>(M_PI) / 180.f;
+                const fp rad = static_cast<fp>(angle) * static_cast<fp>(kPi) / static_cast < fp>(180.f);
-				logger() << "[script] npc_rotate_to: " << npcId << " angle " << angle << " degrees (" << rad << " radians)" << std::endl;
+				logger() << "[script] npc_rotate_to: " << npcId << " angle " << angle << " degrees (" << static_cast<float>(rad) << " radians)" << std::endl;
                 loc->gameServer().submit(SetCharacterFacingAngleCommand{
                     loc->locationId(), npc->entityId(), rad, false});
             });
 
         api.set_function("player_rotate_to",
             [loc](float angle) {
-                const float rad = angle * static_cast<float>(M_PI) / 180.f;
+                const fp rad = static_cast<fp>(angle) * static_cast<fp>(kPi) / static_cast <fp>(180.f);
-                logger() << "[script] player_rotate_to: angle " << angle << " degrees (" << rad << " radians)" << std::endl;
+                logger() << "[script] player_rotate_to: angle " << angle << " degrees (" << static_cast<float>(rad) << " radians)" << std::endl;
                 loc->gameServer().submit(SetCharacterFacingAngleCommand{
                     loc->locationId(), loc->player->entityId(), rad, false});
             });
@@ -188,9 +191,9 @@ namespace FRG {
                 }
                 loc->gameServer().submit(SetCharacterHomePositionCommand{
                     loc->locationId(), loc->player->entityId(),
-                    Eigen::Vector3f(x, 0.f, z)});
+                    Vec3fp(static_cast<fp>(x), static_cast<fp>(0.f), static_cast<fp>(z))});
                 loc->setCharacterTarget(
-                    *loc->player, Eigen::Vector3f(x, y, z), std::move(cbName));
+                    *loc->player, Vec3fp(static_cast<fp>(x), static_cast<fp>(y), static_cast<fp>(z)), std::move(cbName));
             });
 
 
@@ -424,12 +427,12 @@ namespace FRG {
         api.set_function("get_player_x",
             [loc]() -> float {
                 auto state = loc->player ? loc->player->stateSnapshot() : nullptr;
-                return state ? state->position.x() : 0.0f;
+                return state ? static_cast<float>(state->position.x) : 0.0f;
             });
         api.set_function("get_player_z",
             [loc]() -> float {
                 auto state = loc->player ? loc->player->stateSnapshot() : nullptr;
-                return state ? state->position.z() : 0.0f;
+                return state ? static_cast<float>(state->position.z) : 0.0f;
             });
 
         api.set_function("refill_current_player_hp",
@@ -593,7 +596,7 @@ namespace FRG {
                     std::cerr << "[script] npc_set_position: unknown NPC id '" << npcId << "'\n";
                     return;
                 }
-                Eigen::Vector3f pos(x, y, z);
+                Vec3fp pos(static_cast<fp>(x), static_cast<fp>(y), static_cast<fp>(z));
                 loc->gameServer().submit(SetCharacterPositionCommand{
                     loc->locationId(), npc->entityId(), pos, true});
                 loc->setCharacterTarget(*npc, pos);
@@ -607,7 +610,8 @@ namespace FRG {
                     std::cerr << "[script] npc_set_rotation: unknown NPC id '" << npcId << "'\n";
                     return;
                 }
-                const float rad = angle * static_cast<float>(M_PI) / 180.f;
+                const fp rad = static_cast<fp>(angle) * static_cast<fp>(kPi) / static_cast <fp>(180.f);
+
                 loc->gameServer().submit(SetCharacterFacingAngleCommand{
                     loc->locationId(), npc->entityId(), rad, true});
             });
@@ -626,7 +630,7 @@ namespace FRG {
 
         api.set_function("player_set_position",
             [loc](float x, float y, float z) {
-                Eigen::Vector3f pos(x, y, z);
+                Vec3fp pos(static_cast<fp>(x), static_cast<fp>(y), static_cast<fp>(z));
                 loc->gameServer().submit(SetCharacterPositionCommand{
                     loc->locationId(), loc->player->entityId(), pos, true});
                 loc->setCharacterTarget(*loc->player, pos);
@@ -634,7 +638,8 @@ namespace FRG {
 
         api.set_function("player_set_rotation",
             [loc](float angle) {
-                const float rad = angle * static_cast<float>(M_PI) / 180.f;
+                const fp rad = static_cast<fp>(angle) * static_cast<fp>(kPi) / static_cast <fp>(180.f);
+
                 loc->gameServer().submit(SetCharacterFacingAngleCommand{
                     loc->locationId(), loc->player->entityId(), rad, true});
             });

src/content/LocationDefinition.cpp

@@ -6,6 +6,7 @@
 #include <cstring>
 #include <stdexcept>
 #include <unordered_set>
+#include <math.h>
 
 namespace FRG {
 namespace {
@@ -172,7 +173,7 @@ CharacterState NpcData::toCharacterState() const
 	CharacterState state;
 	state.npcName = name;
 	state.position = {positionX, positionY, positionZ};
-	state.homePosition = {positionX, 0.f, positionZ};
+	state.homePosition = {positionX, static_cast<fp>(0.f), positionZ};
 	state.stopInPlace();
 	state.facingAngle = facingAngle;
 	state.walkSpeed = walkSpeed;
@@ -183,9 +184,9 @@ CharacterState NpcData::toCharacterState() const
 	state.visibleRange = visibleRange;
 	state.canAttack = canAttack;
 	state.enabled = enabled;
-	state.collisionRadius = navigationAvoidanceRadius;
+	//state.collisionRadius = navigationAvoidanceRadius;
-	state.movementContactRadius =
+	//state.movementContactRadius =
-		(std::min)(movementContactRadius, navigationAvoidanceRadius);
+	//	(std::min)(movementContactRadius, navigationAvoidanceRadius);
 	state.interactionRadius = interactionRadius;
 	state.configureNavigationRetryCooldowns(
 		id, navigationRetryCooldownTicks);
@@ -309,8 +310,8 @@ LocationDefinition LocationContentLoader::load(
 						data.meshTextures[name] = path.get<std::string>();
 				}
 			}
-			data.interactionRadius =
+			data.interactionRadius = static_cast<fp>(
-				item.value("interactionRadius", 0.f);
+				item.value<double>("interactionRadius", 0.0));
 			data.animationIdlePath =
 				item.value("animationIdlePath", "");
 			data.animationWalkPath =
@@ -327,11 +328,11 @@ LocationDefinition LocationContentLoader::load(
 				item.value("animationActionToDeathPath", "");
 			data.animationDeathIdlePath =
 				item.value("animationDeathIdlePath", "");
-			data.positionX = item.value("positionX", 0.f);
+			data.positionX = static_cast<fp>(item.value<double>("positionX", 0.0));
-			data.positionY = item.value("positionY", 0.f);
+			data.positionY = static_cast<fp>(item.value<double>("positionY", 0.0));
-			data.positionZ = item.value("positionZ", 0.f);
+			data.positionZ = static_cast<fp>(item.value<double>("positionZ", 0.0));
-			data.walkSpeed = item.value("walkSpeed", 1.5f);
+			data.walkSpeed = static_cast<fp>(item.value<double>("walkSpeed", 1.5));
-			data.rotationSpeed = item.value("rotationSpeed", 8.f);
+			data.rotationSpeed = static_cast<fp>(item.value<double>("rotationSpeed", 8.0));
 			data.modelScale = item.value("modelScale", 0.01f);
 			const float degrees = kPi / 180.f;
 			data.modelCorrectionRotX =
@@ -341,12 +342,13 @@ LocationDefinition LocationContentLoader::load(
 			data.modelCorrectionRotZ =
 				item.value("modelCorrectionRotZ", 0.f) * degrees;
 			data.facingAngle =
-				item.value("facingAngle", 0.f) * degrees;
+				static_cast<fp>(item.value<double>("facingAngle", 0.0)) * static_cast<fp>(kPi) / static_cast<fp>(180.0);
 			data.hp = item.value("hp", 100.f);
 			data.visibleRange =
-				item.value("visibleRange", VISIBLE_RANGE);
+				static_cast<fp>(item.value<double>("visibleRange", static_cast<double>(VISIBLE_RANGE)));
 			data.canAttack = item.value("canAttack", false);
 			data.enabled = item.value("enabled", true);
+			/*
 			data.navigationAvoidanceRadius = item.value(
 				"navigationAvoidanceRadius",
 				item.value("collisionRadius", 0.45f));
@@ -359,6 +361,7 @@ LocationDefinition LocationContentLoader::load(
 				0.05f, (std::min)(
 					data.movementContactRadius,
 					data.navigationAvoidanceRadius));
+					*/
 			const char* cooldownKey =
 				item.contains("navigationRetryCooldownTicks")
 					? "navigationRetryCooldownTicks"

src/content/LocationDefinition.h

@@ -58,23 +58,23 @@ struct NpcData {
 	std::string animationActionToStandPath;
 	std::string animationActionToDeathPath;
 	std::string animationDeathIdlePath;
-	float positionX = 0.f;
+	fp positionX = static_cast<fp>(0.f);
-	float positionY = 0.f;
+	fp positionY = static_cast<fp>(0.f);
-	float positionZ = 0.f;
+	fp positionZ = static_cast<fp>(0.f);
-	float walkSpeed = 1.5f;
+	fp walkSpeed = static_cast<fp>(1.5f);
-	float rotationSpeed = 8.f;
+	fp rotationSpeed = static_cast<fp>(8.f);
 	float modelScale = 0.01f;
 	float modelCorrectionRotX = 0.f;
 	float modelCorrectionRotY = 0.f;
 	float modelCorrectionRotZ = 0.f;
-	float facingAngle = 0.f;
+	fp facingAngle = static_cast<fp>(0.f);
 	float hp = 100.f;
-	float visibleRange = VISIBLE_RANGE;
+	fp visibleRange = static_cast<fp>(VISIBLE_RANGE);
 	bool canAttack = false;
 	bool enabled = true;
-	float navigationAvoidanceRadius = 0.45f;
+	//fp navigationAvoidanceRadius = static_cast<fp>(0.45f);
-	float movementContactRadius = 0.32f;
+	//fp movementContactRadius = static_cast<fp>(0.32f);
-	float interactionRadius = 0.f;
+	fp interactionRadius = static_cast<fp>(0.f);
 	std::vector<int> navigationRetryCooldownTicks;
 
 	CharacterState toCharacterState() const;

src/game_server/IGameServer.h

@@ -137,19 +137,19 @@ struct SetGlobalFloatCommand { std::string key; float value; };
 struct SetCharacterPositionCommand {
 	std::string locationId;
 	std::string characterId;
-	Eigen::Vector3f position;
+	Vec3fp position;
 	bool updateHomePosition = false;
 };
 struct SetCharacterFacingAngleCommand {
 	std::string locationId;
 	std::string characterId;
-	float angle;
+	fp angle;
 	bool setCurrentAngle = false;
 };
 struct SetCharacterHomePositionCommand {
 	std::string locationId;
 	std::string characterId;
-	Eigen::Vector3f position;
+	Vec3fp position;
 };
 struct SetCharacterTargetCommand {
 	std::string locationId;
@@ -173,7 +173,7 @@ struct ClearCharacterAttackTargetCommand {
 struct SetCharacterDestinationCommand {
 	std::string locationId;
 	std::string characterId;
-	Eigen::Vector3f target;
+	Vec3fp target;
 	std::string callbackName;
 };
 struct StopCharacterCommand {

src/game_server/InternalGameServer.cpp

@@ -70,11 +70,11 @@ void InternalGameServer::updateBattleZoneTestBot(std::int64_t deltaMs)
 	battleZoneTestBotElapsedMs_ %= kBattleZoneTestBotIntervalMs;
 
 	static std::mt19937 randomEngine(std::random_device{}());
-	static std::uniform_real_distribution<float> coordinate(-10.0f, 10.0f);
+	static std::uniform_int_distribution<int> coordinate(-100, 100); //Divide by 10 then.
 	submit(SetCharacterDestinationCommand{
 		state_.currentLocationName,
 		kBattleZonePlayer2Id,
-		Eigen::Vector3f(coordinate(randomEngine), 0.f, coordinate(randomEngine)),
+		Vec3fp(static_cast<fp>(coordinate(randomEngine)) / static_cast<fp>(10), static_cast<fp>(0.f), coordinate(randomEngine) / static_cast<fp>(10)),
 		std::string()});
 }
 
@@ -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/game_server/NetworkProtocol.cpp

@@ -91,6 +91,44 @@ bool readTarget(
 	return true;
 }
 
+bool readTargetFp(
+	const nlohmann::json& value, FRG::Vec3fp& target, std::string& error)
+{
+	if (!value.is_array() || value.size() != 3) {
+		error = "target must contain three coordinates";
+		return false;
+	}
+
+	FRG::fp coords[3];
+	for (std::size_t index = 0; index < 3; ++index) {
+		if (!value[index].is_number()) {
+			error = "target coordinates must be numbers";
+			return false;
+		}
+		const double coordinate = value[index].get<double>();
+		if (!std::isfinite(coordinate) || std::abs(coordinate) > 100000.0) {
+			error = "target coordinate is not finite or is out of range";
+			return false;
+		}
+		// Округляем до сотых в double и конвертируем в Q20.12 (fp)
+		coords[index] = FRG::fp(coordinate);
+	}
+
+	target.x = coords[0];
+	target.y = coords[1];
+	target.z = coords[2];
+	return true;
+}
+
+nlohmann::json targetJsonFp(const FRG::Vec3fp& target)
+{
+	return nlohmann::json::array({
+		static_cast<double>(target.x),
+		static_cast<double>(target.y),
+		static_cast<double>(target.z)
+		});
+}
+
 nlohmann::json targetJson(const Eigen::Vector3f& target)
 {
 	return nlohmann::json::array({target.x(), target.y(), target.z()});
@@ -161,7 +199,7 @@ bool parseInitialStateMessage(
 }
 
 std::uint64_t LockstepTimeline::schedule(
-	const std::string& playerId, const Eigen::Vector3f& target)
+	const std::string& playerId, const Vec3fp& target)
 {
 	const std::uint64_t sequence = nextCommandSequence_++;
 	pending_[currentTick_ + inputDelayTicks_].push_back(
@@ -206,7 +244,7 @@ std::string makeDestinationInputMessage(const DestinationInputMessage& message)
 {
 	return envelope("destination_input", {
 		{"clientSequence", message.clientSequence},
-		{"target", targetJson(message.target)}
+		{"target", targetJsonFp(message.target)}
 	});
 }
 
@@ -219,7 +257,7 @@ bool parseDestinationInputMessage(
 		return false;
 	if (!payload.contains("target"))
 		return error = "target is required", false;
-	return readTarget(payload["target"], message.target, error);
+	return readTargetFp(payload["target"], message.target, error);
 }
 
 std::string makeCharacterActionInputMessage(
@@ -265,7 +303,7 @@ std::string makeTickMessage(const TickMessage& message)
 					{"type", "destination"},
 					{"commandSequence", command.commandSequence},
 					{"playerId", command.playerId},
-					{"target", targetJson(command.target)}
+					{"target", targetJsonFp(command.target)}
 				});
 			} else {
 				if constexpr (std::is_same_v<T, ScheduledCharacterEnabledCommand>) {
@@ -320,7 +358,7 @@ bool parseTickMessage(
 			destination.commandSequence = previousSequence;
 			if (!value.contains("playerId") || !value["playerId"].is_string() ||
 				!value.contains("target") ||
-				!readTarget(value["target"], destination.target, error)) {
+				!readTargetFp(value["target"], destination.target, error)) {
 				if (error.empty()) error = "invalid destination command";
 				return false;
 			}

src/game_server/NetworkProtocol.h

@@ -7,6 +7,7 @@
 #include <string>
 #include <variant>
 #include <vector>
+#include "../navigation/PathFinder.h"
 
 namespace FRG {
 
@@ -25,7 +26,7 @@ struct InitialStateMessage {
 
 struct DestinationInputMessage {
 	std::uint64_t clientSequence = 0;
-	Eigen::Vector3f target = Eigen::Vector3f::Zero();
+	Vec3fp target;
 };
 
 struct CharacterActionInputMessage {
@@ -37,7 +38,7 @@ struct CharacterActionInputMessage {
 struct ScheduledDestinationCommand {
 	std::uint64_t commandSequence = 0;
 	std::string playerId;
-	Eigen::Vector3f target = Eigen::Vector3f::Zero();
+	Vec3fp target;
 };
 
 struct ScheduledCharacterEnabledCommand {
@@ -73,7 +74,7 @@ public:
 
 	std::uint64_t currentTick() const { return currentTick_; }
 	std::uint64_t schedule(
-		const std::string& playerId, const Eigen::Vector3f& target);
+		const std::string& playerId, const Vec3fp& target);
 	std::uint64_t scheduleCharacterEnabled(
 		const std::string& locationId, const std::string& characterId,
 		bool enabled);

src/items/GameObjectLoader.cpp

@@ -13,6 +13,9 @@
 
 namespace FRG {
 
+	constexpr float kPi = 3.14159265358979323846f;
+
+
 	using json = nlohmann::json;
 
 	// -----------------------------------------------------------------------
@@ -341,7 +344,7 @@ namespace FRG {
 							data.meshTextures[it.key()] = it.value().get<std::string>();
 					}
 				}
-				data.interactionRadius       = item.value("interactionRadius", 0.0f);
+				data.interactionRadius       = static_cast<fp>(item.value<double>("interactionRadius", 0.0));
 				data.animationIdlePath       = item.value("animationIdlePath", "");
 				data.animationWalkPath       = item.value("animationWalkPath", "");
 				data.animationActionIdlePath = item.value("animationActionIdlePath", "");
@@ -350,18 +353,18 @@ namespace FRG {
 				data.animationActionToStandPath = item.value("animationActionToStandPath", "");
 				data.animationActionToDeathPath = item.value("animationActionToDeathPath", "");
 				data.animationDeathIdlePath     = item.value("animationDeathIdlePath", "");
-				data.positionX      = item.value("positionX", 0.0f);
+				data.positionX      = static_cast<fp>(item.value<double>("positionX", 0.0));
-				data.positionY      = item.value("positionY", 0.0f);
+				data.positionY      = static_cast<fp>(item.value<double>("positionY", 0.0));
-				data.positionZ      = item.value("positionZ", 0.0f);
+				data.positionZ      = static_cast<fp>(item.value<double>("positionZ", 0.0));
-				data.walkSpeed      = item.value("walkSpeed", 1.5f);
+				data.walkSpeed      = static_cast<fp>(item.value<double>("walkSpeed", 1.5));
-				data.rotationSpeed  = item.value("rotationSpeed", 8.0f);
+				data.rotationSpeed  = static_cast<fp>(item.value<double>("rotationSpeed", 8.0));
 				data.modelScale     = item.value("modelScale", 0.01f);
 				data.modelCorrectionRotX = item.value("modelCorrectionRotX", 0.0f) * static_cast<float>(M_PI) / 180.0f;
 				data.modelCorrectionRotY = item.value("modelCorrectionRotY", 0.0f) * static_cast<float>(M_PI) / 180.0f;
 				data.modelCorrectionRotZ = item.value("modelCorrectionRotZ", 0.0f) * static_cast<float>(M_PI) / 180.0f;
-				data.facingAngle    = item.value("facingAngle", 0.0f) * static_cast<float>(M_PI) / 180.0f;
+				data.facingAngle    = static_cast<fp>(item.value("facingAngle", 0.0f)) * static_cast<fp>(kPi) / static_cast<fp>(180.0f);
 				data.hp             = item.value("hp", 100.0f);
-				data.visibleRange   = item.value("visibleRange", VISIBLE_RANGE);
+				data.visibleRange   = static_cast<fp>(item.value<double>("visibleRange", static_cast<double>(VISIBLE_RANGE)));
 				data.canAttack      = item.value("canAttack", false);
 				data.enabled        = item.value("enabled", true);
 
@@ -401,7 +404,7 @@ namespace FRG {
 					renderer, zipPath.c_str());
 				if (npc) {
 					logger() << "Successfully loaded NPC: " << npcData.name << " at ("
-					          << npcData.positionX << ", " << npcData.positionY << ", " << npcData.positionZ << ")" << std::endl;
+					          << static_cast<float>(npcData.positionX) << ", " << static_cast<float>(npcData.positionY) << ", " << static_cast<float>(npcData.positionZ) << ")" << std::endl;
 					npcs.push_back(std::move(npc));
 				}
 			} catch (const std::exception& e) {

src/navigation/PathFinder.cpp

@@ -12,8 +12,27 @@
 namespace FRG {
 
 using json = nlohmann::json;
+Vec3fp operator-(Vec3fp a, Vec3fp b)
+{
+    return Vec3fp{
+        a.x - b.x,
+        a.y - b.y,
+        a.z - b.z
+    };
+}
+Vec3fp operator+(Vec3fp a, Vec3fp b)
+{
+    return Vec3fp{
+        a.x + b.x,
+        a.y + b.y,
+        a.z + b.z
+    };
+}
 
 namespace {
+
+
+
     std::vector<Eigen::Vector2f> readPolygon(const json& item)
     {
         std::vector<Eigen::Vector2f> polygon;

src/navigation/PathFinder.h

@@ -80,8 +80,37 @@ namespace FRG {
         Eigen::Vector3f toEigen() const {
             return Eigen::Vector3f(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z));
         }
+
+        fp normSquared() const
+        {
+            return x * x + y * y + z * z;
+        }
+        Vec3fp operator/(fp value) const
+        {
+            if (value == static_cast<fp>(0.0))
+            {
+                throw std::runtime_error("Vec3fp error: division by zero");
+                return Vec3fp();
+            }
+            Vec3fp r;
+            r.x = x / value;
+            r.y = y / value;
+            r.z = z / value;
+            return r;
+        }
+        Vec3fp operator*(const fp value) const
+        {
+            Vec3fp r;
+            r.x = x * value;
+            r.y = y * value;
+            r.z = z * value;
+            return r;
+        }
     };
 
+    Vec3fp operator-(Vec3fp a, Vec3fp b);
+    Vec3fp operator+(Vec3fp a, Vec3fp b);
+
     inline std::vector<Eigen::Vector2f> toEigenVector(const std::vector<FRG::Vec2fp>& src) {
         std::vector<Eigen::Vector2f> dst;
         dst.reserve(src.size());
@@ -339,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;

src/render/PathVisualization.cpp

@@ -109,7 +109,7 @@ namespace FRG {
 		const size_t firstWaypoint = (std::min)(
 			state->currentWaypointIndex, state->pathWaypoints.size());
 		for (size_t i = firstWaypoint; i < state->pathWaypoints.size(); ++i) {
-			const Eigen::Vector3f& waypoint = state->pathWaypoints[i];
+			const Eigen::Vector3f& waypoint = state->pathWaypoints[i].toEigen();
 			route.push_back(Eigen::Vector3f(waypoint.x(), 0.0f, waypoint.z()));
 		}
 		const Eigen::Vector3f clickedPosition(
@@ -140,7 +140,7 @@ namespace FRG {
 			constexpr float kPlayerClearanceMs = 20.0f;
 			constexpr float kPlayerMaximumSpeedMultiplier = 1.5f;
 			const float playerSpeed = (std::max)(
-				state->walkSpeed * kPlayerMaximumSpeedMultiplier, 0.01f);
+				static_cast<float>(state->walkSpeed) * kPlayerMaximumSpeedMultiplier, 0.01f);
 			const float playerRouteTimeMs = totalLength / playerSpeed * 1000.0f;
 			const float revealDurationMs = (std::min)(kMaximumRevealMs, playerRouteTimeMs / 3.0f);
 			const float waveSpeed = totalLength / (revealDurationMs / 1000.0f);