Working about pathfinding

src/CharacterState.cpp

@@ -6,10 +6,22 @@
 namespace FRG {
 
 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;
 constexpr float TARGET_REPLAN_THRESHOLD = 0.25f;
 constexpr float PI = 3.14159265358979323846f;
+constexpr int BLOCKED_REPLAN_TICKS = 3;
+
+std::uint32_t stableIdHash(const std::string& value)
+{
+    std::uint32_t hash = 2166136261u;
+    for (const unsigned char byte : value) {
+        hash ^= byte;
+        hash *= 16777619u;
+    }
+    return hash;
+}
 }
 
 void CharacterState::configureAsPlayer() {
@@ -23,6 +35,36 @@ void CharacterState::configureAsPlayer() {
     isPlayer = true;
 }
 
+void CharacterState::configureNavigationRetryCooldowns(
+    const std::string& entityId, std::vector<int> cooldownTicks)
+{
+    if (cooldownTicks.empty()) {
+        cooldownTicks = {2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
+    }
+    for (int& ticks : cooldownTicks)
+        ticks = (std::max)(1, (std::min)(600, ticks));
+
+    std::uint32_t state = stableIdHash(entityId);
+    for (std::size_t i = cooldownTicks.size(); i > 1; --i) {
+        state = state * 1664525u + 1013904223u;
+        const std::size_t other = state % i;
+        std::swap(cooldownTicks[i - 1], cooldownTicks[other]);
+    }
+    navigationRetryCooldownTicks = std::move(cooldownTicks);
+    navigationRetryCooldownIndex = 0;
+}
+
+void CharacterState::scheduleNextNavigationRetry()
+{
+    if (navigationRetryCooldownTicks.empty())
+        configureNavigationRetryCooldowns(npcName);
+    navigationRetryTicks = navigationRetryCooldownTicks[
+        navigationRetryCooldownIndex % navigationRetryCooldownTicks.size()];
+    navigationRetryCooldownIndex =
+        (navigationRetryCooldownIndex + 1) %
+        navigationRetryCooldownTicks.size();
+}
+
 bool CharacterState::isMoving() const {
     Eigen::Vector3f toTarget = walkTarget - position;
     toTarget.y() = 0.f;
@@ -42,13 +84,17 @@ void CharacterState::stopInPlace() {
     pathWaypoints.clear();
     currentWaypointIndex = 0;
     onArrivedCallbackName.clear();
+    movementBlockedTicks = 0;
+    navigationRetryTicks = 0;
+    blockingCharacterId.clear();
 }
 
 void CharacterState::setTarget(
     const Eigen::Vector3f& target,
     std::string callbackName,
     const LocationNetworkState& location,
-    const std::string& entityId)
+    const std::string& entityId,
+    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
 {
     const Eigen::Vector3f normalizedTarget(target.x(), 0.f, target.z());
     const bool sameRequestedTarget =
@@ -66,9 +112,13 @@ void CharacterState::setTarget(
 
     requestedWalkTarget = normalizedTarget;
     onArrivedCallbackName = std::move(callbackName);
+    movementBlockedTicks = 0;
+    navigationRetryTicks = 0;
+    blockingCharacterId.clear();
     pathWaypoints = location.navigationMaps.empty()
         ? std::vector<Eigen::Vector3f>{}
-        : location.planPathExceptMyself(position, normalizedTarget, entityId);
+        : location.planPathExceptMyself(
+            position, normalizedTarget, entityId, dynamicObstacles);
     currentWaypointIndex = 0;
     if (!pathWaypoints.empty()) {
         for (Eigen::Vector3f& waypoint : pathWaypoints) waypoint.y() = 0.f;
@@ -77,26 +127,32 @@ void CharacterState::setTarget(
     }
 
     walkTarget = Eigen::Vector3f(position.x(), 0.f, position.z());
-    onArrivedCallbackName.clear();
+    if (!isPlayer) scheduleNextNavigationRetry();
 }
 
 void CharacterState::forceReplan(
-    const LocationNetworkState& location, const std::string& entityId)
+    const LocationNetworkState& location,
+    const std::string& entityId,
+    const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles)
 {
     const Eigen::Vector3f normalizedTarget(
         requestedWalkTarget.x(), 0.f, requestedWalkTarget.z());
     pathWaypoints = location.navigationMaps.empty()
         ? std::vector<Eigen::Vector3f>{}
-        : location.planPathExceptMyself(position, normalizedTarget, entityId);
+        : location.planPathExceptMyself(
+            position, normalizedTarget, entityId, dynamicObstacles);
     currentWaypointIndex = 0;
+    movementBlockedTicks = 0;
+    navigationRetryTicks = 0;
     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());
-    onArrivedCallbackName.clear();
+    if (!isPlayer) scheduleNextNavigationRetry();
 }
 
 void CharacterState::stopAndRotateToTarget(std::string characterId)
@@ -146,7 +202,9 @@ CharacterUpdateEvents CharacterState::update(
     std::int64_t deltaMs, const CharacterUpdateContext& context)
 {
     CharacterUpdateEvents events;
+    movedLastTick = false;
     if (!enabled) return events;
+    bool navigationPausedThisTick = false;
 
     const CharacterObservation* attackTarget = context.attackTarget;
     const CharacterObservation* faceTarget = context.faceTarget;
@@ -185,12 +243,75 @@ CharacterUpdateEvents CharacterState::update(
         if (distance > kWalkThreshold) {
             const Eigen::Vector3f direction = toTarget / distance;
             const float moveAmount = walkSpeed * static_cast<float>(deltaMs) / 1000.f;
-            position = moveAmount >= distance ? activeTarget : position + direction * moveAmount;
-            position.y() = 0.f;
+            Eigen::Vector3f candidate =
+                moveAmount >= distance ? activeTarget : position + direction * moveAmount;
+            candidate.y() = 0.f;
+            const CharacterMovementObstacle* blocker = nullptr;
+            if (context.movementObstacles) {
+                for (const CharacterMovementObstacle& obstacle :
+                     *context.movementObstacles) {
+                    if (obstacle.id == context.characterId) continue;
+                    Eigen::Vector3f startDelta = position - obstacle.position;
+                    Eigen::Vector3f endDelta = candidate - obstacle.position;
+                    startDelta.y() = 0.f;
+                    endDelta.y() = 0.f;
+                    const float minimumDistance =
+                        movementContactRadius + obstacle.radius;
+                    const float minimumDistanceSq =
+                        minimumDistance * minimumDistance;
+                    const Eigen::Vector3f segment = candidate - position;
+                    const float segmentLengthSq =
+                        segment.x() * segment.x() + segment.z() * segment.z();
+                    float t = 0.f;
+                    if (segmentLengthSq > 1e-8f) {
+                        t = -(startDelta.x() * segment.x() +
+                              startDelta.z() * segment.z()) / segmentLengthSq;
+                        t = (std::max)(0.f, (std::min)(1.f, t));
+                    }
+                    Eigen::Vector3f closest = startDelta + segment * t;
+                    closest.y() = 0.f;
+                    const bool movingCloser =
+                        endDelta.squaredNorm() + 1e-6f < startDelta.squaredNorm();
+                    if (movingCloser &&
+                        closest.squaredNorm() < minimumDistanceSq) {
+                        blocker = &obstacle;
+                        break;
+                    }
+                }
+            }
+            if (blocker) {
+                blockingCharacterId = blocker->id;
+                currentState = AnimationState::STAND;
+                const bool sameDirection =
+                    blocker->moving &&
+                    blocker->movementDirection.dot(direction) > 0.5f;
+                if (sameDirection) {
+                    movementBlockedTicks = 0;
+                } else if (++movementBlockedTicks >= BLOCKED_REPLAN_TICKS) {
+                    if (isPlayer) {
+                        stopInPlace();
+                    } else {
+                        pathWaypoints.clear();
+                        currentWaypointIndex = 0;
+                        walkTarget =
+                            Eigen::Vector3f(position.x(), 0.f, position.z());
+                        movementBlockedTicks = 0;
+                        scheduleNextNavigationRetry();
+                        navigationPausedThisTick = true;
+                    }
+                }
+            } else {
+                movedLastTick =
+                    (candidate - position).squaredNorm() > 1e-8f;
+                position = candidate;
+                movementBlockedTicks = 0;
+                blockingCharacterId.clear();
+            }
             targetFacingAngle = std::atan2(direction.x(), -direction.z());
-            currentState = AnimationState::WALK;
+            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) {
@@ -198,7 +319,12 @@ CharacterUpdateEvents CharacterState::update(
             } else {
                 pathWaypoints.clear();
                 currentWaypointIndex = 0;
-                if (!onArrivedCallbackName.empty()) {
+                if (completedPath &&
+                    (requestedWalkTarget - position).norm() >
+                        TARGET_REPLAN_THRESHOLD &&
+                    !isPlayer)
+                    scheduleNextNavigationRetry();
+                if (completedPath && !onArrivedCallbackName.empty()) {
                     events.arrivedCallbackName = onArrivedCallbackName;
                     onArrivedCallbackName.clear();
                 }
@@ -217,6 +343,18 @@ CharacterUpdateEvents CharacterState::update(
         }
     }
 
+    if (!isPlayer && !navigationPausedThisTick && pathWaypoints.empty() &&
+        (requestedWalkTarget - position).norm() > TARGET_REPLAN_THRESHOLD &&
+        !events.navigationRequested) {
+        if (navigationRetryTicks > 0) --navigationRetryTicks;
+        if (navigationRetryTicks == 0) {
+            events.navigationRequested = true;
+            events.forceNavigationReplan = true;
+            events.navigationTarget = requestedWalkTarget;
+            events.blockingCharacterId = blockingCharacterId;
+        }
+    }
+
     if (hp <= 0.f) {
         if (currentState != AnimationState::ACTION_TO_DEATH &&
             currentState != AnimationState::DEATH_IDLE) {
@@ -350,6 +488,13 @@ void CharacterState::save(nlohmann::json& out) const
     out["pathWaypoints"]        = std::move(wpArr);
     out["currentWaypointIndex"] = currentWaypointIndex;
     out["homeDriftCheckTimer"]  = 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;
     if (!onArrivedCallbackName.empty())
@@ -387,6 +532,17 @@ void CharacterState::load(const nlohmann::json& in)
     }
     currentWaypointIndex = in.value("currentWaypointIndex", currentWaypointIndex);
     homeDriftCheckTimer  = in.value("homeDriftCheckTimer",  homeDriftCheckTimer);
+    movementBlockedTicks = in.value("movementBlockedTicks", movementBlockedTicks);
+    navigationRetryTicks = in.value("navigationRetryTicks", navigationRetryTicks);
+    movedLastTick = in.value("movedLastTick", movedLastTick);
+    navigationRetryCooldownTicks = in.value(
+        "navigationRetryCooldownTicks", navigationRetryCooldownTicks);
+    navigationRetryCooldownIndex = in.value(
+        "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

@@ -9,6 +9,7 @@
 #include <Eigen/Geometry>
 #include "external/nlohmann/json.hpp"
 #include "ISaveable.h"
+#include "navigation/PathFinder.h"
 
 namespace FRG {
 
@@ -110,8 +111,12 @@ public:
     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;
+    // Smaller physical contact threshold used by the movement guard.
+    float movementContactRadius = 0.32f;
     float interactionRadius = 0.0f;
+    bool movedLastTick = false;
 
     // --- Pathfinding data (serializable parts) ---
     Eigen::Vector3f walkTarget          = Eigen::Vector3f::Zero();
@@ -121,6 +126,11 @@ public:
 
     // Internal drift-check timer (serializable so it survives save/load).
     float homeDriftCheckTimer = 5.0f;
+    int movementBlockedTicks = 0;
+    int navigationRetryTicks = 0;
+    std::string blockingCharacterId;
+    std::vector<int> navigationRetryCooldownTicks;
+    std::size_t navigationRetryCooldownIndex = 0;
 
     // Global Lua function called when the character reaches the final waypoint.
     // The name is serialized, so no runtime callback needs to be rehydrated.
@@ -131,6 +141,9 @@ public:
 
     // --- Pure-state methods ---
     void                   configureAsPlayer();
+    void                   configureNavigationRetryCooldowns(
+                                     const std::string& entityId,
+                                     std::vector<int> cooldownTicks = {});
     bool                   isMoving()                   const;
     const Eigen::Vector3f& getRequestedWalkTarget()     const { return requestedWalkTarget; }
     Eigen::Vector3f        getCurrentNavigationTarget()  const;
@@ -138,9 +151,13 @@ public:
     void                   setTarget(const Eigen::Vector3f& target,
                                      std::string callbackName,
                                      const LocationNetworkState& location,
-                                     const std::string& entityId);
+                                     const std::string& entityId,
+                                     const std::vector<PathFinder::DynamicObstacle>&
+                                         dynamicObstacles = {});
     void                   forceReplan(const LocationNetworkState& location,
-                                       const std::string& entityId);
+                                       const std::string& entityId,
+                                       const std::vector<PathFinder::DynamicObstacle>&
+                                           dynamicObstacles = {});
     void                   stopAndRotateToTarget(std::string characterId);
     void                   resetPlayerAfterDeath();
     void                   tryAttackCombo();
@@ -151,6 +168,9 @@ public:
     // --- Serialisation (runtime mutable fields only; creation info is reloaded from config) ---
     void save(nlohmann::json& out) const override;
     void load(const nlohmann::json& in) override;
+
+private:
+    void scheduleNextNavigationRetry();
 };
 
 using PlayerStateMap =
@@ -161,16 +181,28 @@ struct CharacterObservation {
     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 {
     bool attackImpact = false;
     bool deathAnimationComplete = false;
     bool navigationRequested = false;
+    bool forceNavigationReplan = false;
     Eigen::Vector3f navigationTarget = Eigen::Vector3f::Zero();
+    std::string blockingCharacterId;
     std::string arrivedCallbackName;
     std::string attackTargetId;
     Eigen::Vector3f attackDirection = Eigen::Vector3f::Zero();

src/Game.cpp

@@ -680,7 +680,7 @@ namespace FRG
 		// Developer workflow: uncomment to (re)generate the campaign's start-state JSON
 		// from this freshly-rebuilt state after changing gameplay/content JSON or Lua.
 		// Re-comment before shipping.
-		//saveInitialState();
+		saveInitialState();
 
 	}
 

src/GameState.h

@@ -33,7 +33,7 @@ struct StoredChatMessage {
 };
 
 struct GameState : public ISaveable {
-	static constexpr int kSchemaVersion = 2;
+	static constexpr int kSchemaVersion = 5;
 
 	// --- World ---
 	// Campaign being played (see CampaignRegistry).

src/LocationGameState.cpp

@@ -126,6 +126,58 @@ 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; });
+
+	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 (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,
+	std::string callbackName,
+	PlayerStateMap& players,
+	const std::string& forcedObstacleId)
+{
+	CharacterState* character = findCharacter(id, players);
+	if (!character || !location) return false;
+	character->setTarget(
+		target, std::move(callbackName), *location, id,
+		collectPathObstacles(id, players, forcedObstacleId));
+	return true;
+}
+
+bool LocationGameState::replanCharacter(
+	const std::string& id,
+	PlayerStateMap& players,
+	const std::string& forcedObstacleId)
+{
+	CharacterState* character = findCharacter(id, players);
+	if (!character || !location) return false;
+	character->forceReplan(
+		*location, id, collectPathObstacles(id, players, forcedObstacleId));
+	return true;
+}
+
 void LocationGameState::startTaxi()
 {
 	location->taxiCar.waypoints = location->taxiCarWaypoints;
@@ -330,18 +382,70 @@ LocationSimulationEvents LocationGameState::update(
 			if (toHome.norm() > CharacterState::kWalkThreshold &&
 				(npc.requestedWalkTarget - homeTarget).norm() >
 					CharacterState::kWalkThreshold) {
-				npc.setTarget(homeTarget, {}, *location, id);
+				setCharacterDestination(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; });
+
 	auto advance = [&](const std::string& id, CharacterState& state) {
 		CharacterUpdateContext context;
+		context.characterId = id;
+		context.movementObstacles = &movementObstacles;
 		auto attack = observations.find(state.attackTargetId);
 		if (attack != observations.end()) context.attackTarget = &attack->second;
 		auto face = observations.find(state.faceTargetId);
 		if (face != observations.end()) context.faceTarget = &face->second;
 		CharacterUpdateEvents events = state.update(deltaMs, context);
+		auto reservation = std::lower_bound(
+			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)});

src/LocationGameState.h

@@ -64,6 +64,20 @@ 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,
+		std::string callbackName,
+		PlayerStateMap& players,
+		const std::string& forcedObstacleId = {});
+	bool replanCharacter(
+		const std::string& id,
+		PlayerStateMap& players,
+		const std::string& forcedObstacleId = {});
 	void startTaxi();
 	void resetTaxi();
 	LocationSimulationEvents update(

src/LocationState.cpp

@@ -122,9 +122,19 @@ void loadSinglePlayerPersistence(
 	}
 
 
-	std::vector<Eigen::Vector3f> LocationNetworkState::planPathExceptMyself(const Eigen::Vector3f& start, const Eigen::Vector3f& end, const std::string& entityId) const
+	// Entity ownership stays above this navigation-only layer.
+	std::vector<Eigen::Vector3f> LocationNetworkState::planPathExceptMyself(
+		const Eigen::Vector3f& start,
+		const Eigen::Vector3f& end,
+		const std::string& entityId,
+		const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles) const
 	{
-		return navigationMaps[activeNavigationIndex].findPathToNearest(start, end, {});
+		(void)entityId;
+		if (activeNavigationIndex < 0 ||
+			activeNavigationIndex >= static_cast<int>(navigationMaps.size()))
+			return {};
+		return navigationMaps[activeNavigationIndex].findPathToNearest(
+			start, end, dynamicObstacles);
 	}
 
 	void LocationNetworkState::initNavigation(

src/LocationState.h

@@ -41,7 +41,11 @@ struct LocationNetworkState : public ISaveable {
     //This value is populated from the JSON configs and should not be changed during the regular gameplay.
     //It might be changed only in the editor mode during development.
     std::vector<PathFinder> navigationMaps;
-    std::vector<Eigen::Vector3f> planPathExceptMyself(const Eigen::Vector3f& start, const Eigen::Vector3f& end, const std::string& entityId) const;
+    std::vector<Eigen::Vector3f> planPathExceptMyself(
+        const Eigen::Vector3f& start,
+        const Eigen::Vector3f& end,
+        const std::string& entityId,
+        const std::vector<PathFinder::DynamicObstacle>& dynamicObstacles = {}) const;
     void initNavigation(
         const std::vector<std::string>& paths,
         const std::string& zipFile = std::string());

src/content/LocationDefinition.cpp

@@ -183,7 +183,12 @@ CharacterState NpcData::toCharacterState() const
 	state.visibleRange = visibleRange;
 	state.canAttack = canAttack;
 	state.enabled = enabled;
+	state.collisionRadius = navigationAvoidanceRadius;
+	state.movementContactRadius =
+		(std::min)(movementContactRadius, navigationAvoidanceRadius);
 	state.interactionRadius = interactionRadius;
+	state.configureNavigationRetryCooldowns(
+		id, navigationRetryCooldownTicks);
 
 	Eigen::Quaternionf correction = Eigen::Quaternionf::Identity();
 	if (modelCorrectionRotX != 0.f)
@@ -342,6 +347,34 @@ LocationDefinition LocationContentLoader::load(
 				item.value("visibleRange", VISIBLE_RANGE);
 			data.canAttack = item.value("canAttack", false);
 			data.enabled = item.value("enabled", true);
+			data.navigationAvoidanceRadius = item.value(
+				"navigationAvoidanceRadius",
+				item.value("collisionRadius", 0.45f));
+			data.navigationAvoidanceRadius =
+				(std::max)(0.05f, data.navigationAvoidanceRadius);
+			data.movementContactRadius = item.value(
+				"movementContactRadius",
+				data.navigationAvoidanceRadius * 0.72f);
+			data.movementContactRadius = (std::max)(
+				0.05f, (std::min)(
+					data.movementContactRadius,
+					data.navigationAvoidanceRadius));
+			const char* cooldownKey =
+				item.contains("navigationRetryCooldownTicks")
+					? "navigationRetryCooldownTicks"
+					: "navigationRetryCooldownMs";
+			if (item.contains(cooldownKey) &&
+				item.at(cooldownKey).is_array()) {
+				for (const auto& value : item.at(cooldownKey)) {
+					if (!value.is_number_integer()) continue;
+					int ticks = value.get<int>();
+					if (std::string(cooldownKey) ==
+						"navigationRetryCooldownMs") {
+						ticks = (std::max)(1, (ticks + 99) / 100);
+					}
+					data.navigationRetryCooldownTicks.push_back(ticks);
+				}
+			}
 			result.npcs.push_back(std::move(data));
 		}
 	}

src/content/LocationDefinition.h

@@ -72,7 +72,10 @@ struct NpcData {
 	float visibleRange = VISIBLE_RANGE;
 	bool canAttack = false;
 	bool enabled = true;
+	float navigationAvoidanceRadius = 0.45f;
+	float movementContactRadius = 0.32f;
 	float interactionRadius = 0.f;
+	std::vector<int> navigationRetryCooldownTicks;
 
 	CharacterState toCharacterState() const;
 };

src/game_server/InternalGameServer.cpp

@@ -323,9 +323,9 @@ void InternalGameServer::submit(const GameCommand& command)
 				LocationNetworkState& locationState = *location->second.location;
 				locationState.activeNavigationIndex = value.index;
 				for (const auto& [id, player] : state_.players)
-					if (player) player->forceReplan(locationState, id);
+					if (player) location->second.replanCharacter(id, state_.players);
 				for (const auto& [id, npc] : location->second.npcs)
-					if (npc) npc->forceReplan(locationState, id);
+					if (npc) location->second.replanCharacter(id, state_.players);
 			}
 		} else if constexpr (std::is_same_v<T, SetNavigationAreaAvailableCommand>) {
 			auto location = state_.locations.find(value.locationId);
@@ -336,6 +336,10 @@ void InternalGameServer::submit(const GameCommand& command)
 					index < static_cast<int>(locationState.navigationMaps.size())) {
 					locationState.navigationMaps[index].setAreaAvailable(
 						value.areaName, value.available);
+					for (const auto& [id, player] : state_.players)
+						if (player) location->second.replanCharacter(id, state_.players);
+					for (const auto& [id, npc] : location->second.npcs)
+						if (npc) location->second.replanCharacter(id, state_.players);
 				}
 			}
 		} else if constexpr (std::is_same_v<T, AddNavigationObstacleCommand>) {
@@ -349,6 +353,10 @@ void InternalGameServer::submit(const GameCommand& command)
 					polygon.name = value.name;
 					polygon.polygon = value.polygon;
 					locationState.navigationMaps[index].addObstaclePolygon(polygon);
+					for (const auto& [id, player] : state_.players)
+						if (player) location->second.replanCharacter(id, state_.players);
+					for (const auto& [id, npc] : location->second.npcs)
+						if (npc) location->second.replanCharacter(id, state_.players);
 				}
 			}
 		} else if constexpr (std::is_same_v<T, AddInventoryItemCommand>) {
@@ -424,14 +432,10 @@ void InternalGameServer::submit(const GameCommand& command)
 			if (character) character->attackTargetId.clear();
 		} else if constexpr (std::is_same_v<T, SetCharacterDestinationCommand>) {
 			auto location = state_.locations.find(value.locationId);
-			CharacterState* character =
-				state_.findCharacter(value.locationId, value.characterId);
-			if (character && location != state_.locations.end() &&
-				location->second.location) {
-				character->setTarget(
-					value.target, value.callbackName,
-					*location->second.location, value.characterId);
-			}
+			if (location != state_.locations.end())
+				location->second.setCharacterDestination(
+					value.characterId, value.target, value.callbackName,
+					state_.players);
 		} else if constexpr (std::is_same_v<T, StopCharacterCommand>) {
 			CharacterState* character =
 				state_.findCharacter(value.locationId, value.characterId);
@@ -443,13 +447,9 @@ void InternalGameServer::submit(const GameCommand& command)
 				character->stopAndRotateToTarget(value.targetId);
 		} else if constexpr (std::is_same_v<T, ReplanCharacterCommand>) {
 			auto location = state_.locations.find(value.locationId);
-			CharacterState* character =
-				state_.findCharacter(value.locationId, value.characterId);
-			if (character && location != state_.locations.end() &&
-				location->second.location) {
-				character->forceReplan(
-					*location->second.location, value.characterId);
-			}
+			if (location != state_.locations.end())
+				location->second.replanCharacter(
+					value.characterId, state_.players);
 		} else if constexpr (std::is_same_v<T, ResetCharacterAfterDeathCommand>) {
 			CharacterState* character =
 				state_.findCharacter(value.locationId, value.characterId);
@@ -518,13 +518,14 @@ void InternalGameServer::update(std::int64_t deltaMs)
 	for (const CharacterSimulationEvent& event :
 			pendingSimulationEvents_.characters) {
 		if (!event.events.navigationRequested) continue;
-		CharacterState* character = state_.findCharacter(
-			state_.currentLocationName, event.characterId);
-		if (character && stored->second.location) {
-			character->setTarget(
-				event.events.navigationTarget, {},
-				*stored->second.location, event.characterId);
-		}
+		if (event.events.forceNavigationReplan)
+			stored->second.replanCharacter(
+				event.characterId, state_.players,
+				event.events.blockingCharacterId);
+		else
+			stored->second.setCharacterDestination(
+				event.characterId, event.events.navigationTarget, {},
+				state_.players);
 	}
 	publishMutation();
 }

src/navigation/PathFinder.cpp

@@ -14,15 +14,6 @@ namespace FRG {
 using json = nlohmann::json;
 
 namespace {
-    const float INF_COST = (std::numeric_limits<float>::max)();
-
-    float distanceCells(const PathFinder::Cell& a, const PathFinder::Cell& b)
-    {
-        const float dx = static_cast<float>(a.x - b.x);
-        const float dz = static_cast<float>(a.z - b.z);
-        return std::sqrt(dx * dx + dz * dz);
-    }
-
     std::vector<Eigen::Vector2f> readPolygon(const json& item)
     {
         std::vector<Eigen::Vector2f> polygon;
@@ -127,397 +118,287 @@ void PathFinder::build(const std::string& configPath,
 std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
                                                   const Eigen::Vector3f& end) const
 {
-    if (!ready || walkable.empty()) {
-        return {};
-    }
-
-    Cell startCell;
-    Cell endCell;
-    if (!findNearestWalkableCell(start, startCell, walkable) ||
-        !findNearestWalkableCell(end, endCell, walkable)) {
-        return {};
-    }
-
-    if (startCell.x == endCell.x && startCell.z == endCell.z) {
-        return { cellCenter(endCell) };
-    }
-
-    struct QueueNode {
-        int index = 0;
-        float priority = 0.0f;
-
-        bool operator<(const QueueNode& other) const
-        {
-            if (priority != other.priority) return priority > other.priority;
-            return index > other.index;
-        }
-    };
-
-    const int cellCount = gridWidth * gridDepth;
-    std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
-    std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
-    std::priority_queue<QueueNode> open;
-
-    const int startIndex = indexOf(startCell);
-    const int endIndex = indexOf(endCell);
-
-    cost[static_cast<size_t>(startIndex)] = 0.0f;
-    open.push({ startIndex, 0.0f });
-
-    static const int offsets[8][2] = {
-        { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
-        { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
-    };
-
-    while (!open.empty()) {
-        const QueueNode current = open.top();
-        open.pop();
-
-        if (current.index == endIndex) {
-            break;
-        }
-
-        const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
-
-        for (const auto& offset : offsets) {
-            Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
-            if (!isCellWalkable(next, walkable)) {
-                continue;
-            }
-
-            const bool diagonal = offset[0] != 0 && offset[1] != 0;
-            if (diagonal) {
-                Cell horizontal{ currentCell.x + offset[0], currentCell.z };
-                Cell vertical{ currentCell.x, currentCell.z + offset[1] };
-                if (!isCellWalkable(horizontal, walkable) || !isCellWalkable(vertical, walkable)) {
-                    continue;
-                }
-            }
-
-            const int nextIndex = indexOf(next);
-            const float stepCost = diagonal ? 1.41421356f : 1.0f;
-            const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
-            if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
-                continue;
-            }
-
-            cost[static_cast<size_t>(nextIndex)] = newCost;
-            cameFrom[static_cast<size_t>(nextIndex)] = current.index;
-            const float priority = newCost + distanceCells(next, endCell);
-            open.push({ nextIndex, priority });
-        }
-    }
-
-    if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
-        return {};
-    }
-
-    std::vector<Cell> cells;
-    for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
-        cells.push_back({ current % gridWidth, current / gridWidth });
-        if (current == startIndex) {
-            break;
-        }
-    }
-    std::reverse(cells.begin(), cells.end());
-    cells = smoothCells(cells, walkable);
-
-    std::vector<Eigen::Vector3f> path;
-    path.reserve(cells.size());
-    for (const Cell& cell : cells) {
-        path.push_back(cellCenter(cell));
-    }
-
-    if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) {
-        path.erase(path.begin());
-    }
-
-    if (!path.empty()) {
-        Cell requestedEndCell;
-        if (worldToCell(end, requestedEndCell) &&
-            requestedEndCell.x == endCell.x &&
-            requestedEndCell.z == endCell.z) {
-            path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
-        }
-    }
-
-    return path;
+    const std::lock_guard<std::mutex> lock(*queryMutex);
+    return runSearch(start, end, 0, false);
 }
 
 std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
                                                   const Eigen::Vector3f& end,
                                                   const std::vector<DynamicObstacle>& dynamicObstacles) const
 {
-    if (!ready || walkable.empty()) {
-        return {};
-    }
-
-    std::vector<unsigned char> walkableGrid = walkable;
-    if (!dynamicObstacles.empty()) {
-        for (const DynamicObstacle& obstacle : dynamicObstacles) {
-            const float radius = obstacle.radius + agentRadius;
-            if (radius <= 0.0f) continue;
-
-            const float minWorldX = obstacle.position.x() - radius;
-            const float maxWorldX = obstacle.position.x() + radius;
-            const float minWorldZ = obstacle.position.z() - radius;
-            const float maxWorldZ = obstacle.position.z() + radius;
-
-            const int minCellX = (std::max)(0, static_cast<int>(std::floor((minWorldX - minX) / cellSize)));
-            const int maxCellX = (std::min)(gridWidth - 1, static_cast<int>(std::floor((maxWorldX - minX) / cellSize)));
-            const int minCellZ = (std::max)(0, static_cast<int>(std::floor((minWorldZ - minZ) / cellSize)));
-            const int maxCellZ = (std::min)(gridDepth - 1, static_cast<int>(std::floor((maxWorldZ - minZ) / cellSize)));
-
-            const float radiusSq = radius * radius;
-            for (int z = minCellZ; z <= maxCellZ; ++z) {
-                for (int x = minCellX; x <= maxCellX; ++x) {
-                    const Cell cell{ x, z };
-                    const Eigen::Vector3f center = cellCenter(cell);
-                    const float dx = center.x() - obstacle.position.x();
-                    const float dz = center.z() - obstacle.position.z();
-                    if (dx * dx + dz * dz <= radiusSq) {
-                        const int idx = indexOf(cell);
-                        if (idx >= 0 && idx < static_cast<int>(walkableGrid.size())) {
-                            walkableGrid[static_cast<size_t>(idx)] = 0;
-                        }
-                    }
-                }
-            }
-        }
-    }
-
-    Cell startCell;
-    Cell endCell;
-    if (!findNearestWalkableCell(start, startCell, walkableGrid) ||
-        !findNearestWalkableCell(end, endCell, walkableGrid)) {
-        return {};
-    }
-
-    if (startCell.x == endCell.x && startCell.z == endCell.z) {
-        return { cellCenter(endCell) };
-    }
-
-    struct QueueNode {
-        int index = 0;
-        float priority = 0.0f;
-
-        bool operator<(const QueueNode& other) const
-        {
-            if (priority != other.priority) return priority > other.priority;
-            return index > other.index;
-        }
-    };
-
-    const int cellCount = gridWidth * gridDepth;
-    std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
-    std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
-    std::priority_queue<QueueNode> open;
-
-    const int startIndex = indexOf(startCell);
-    const int endIndex = indexOf(endCell);
-
-    cost[static_cast<size_t>(startIndex)] = 0.0f;
-    open.push({ startIndex, 0.0f });
-
-    static const int offsets[8][2] = {
-        { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
-        { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
-    };
-
-    while (!open.empty()) {
-        const QueueNode current = open.top();
-        open.pop();
-
-        if (current.index == endIndex) {
-            break;
-        }
-
-        const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
-
-        for (const auto& offset : offsets) {
-            Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
-            if (!isCellWalkable(next, walkableGrid)) {
-                continue;
-            }
-
-            const bool diagonal = offset[0] != 0 && offset[1] != 0;
-            if (diagonal) {
-                Cell horizontal{ currentCell.x + offset[0], currentCell.z };
-                Cell vertical{ currentCell.x, currentCell.z + offset[1] };
-                if (!isCellWalkable(horizontal, walkableGrid) || !isCellWalkable(vertical, walkableGrid)) {
-                    continue;
-                }
-            }
-
-            const int nextIndex = indexOf(next);
-            const float stepCost = diagonal ? 1.41421356f : 1.0f;
-            const float newCost = cost[static_cast<size_t>(current.index)] + stepCost;
-            if (newCost >= cost[static_cast<size_t>(nextIndex)]) {
-                continue;
-            }
-
-            cost[static_cast<size_t>(nextIndex)] = newCost;
-            cameFrom[static_cast<size_t>(nextIndex)] = current.index;
-            const float priority = newCost + distanceCells(next, endCell);
-            open.push({ nextIndex, priority });
-        }
-    }
-
-    if (cameFrom[static_cast<size_t>(endIndex)] == -1) {
-        return {};
-    }
-
-    std::vector<Cell> cells;
-    for (int current = endIndex; current != -1; current = cameFrom[static_cast<size_t>(current)]) {
-        cells.push_back({ current % gridWidth, current / gridWidth });
-        if (current == startIndex) {
-            break;
-        }
-    }
-    std::reverse(cells.begin(), cells.end());
-    cells = smoothCells(cells, walkableGrid);
-
-    std::vector<Eigen::Vector3f> path;
-    path.reserve(cells.size());
-    for (const Cell& cell : cells) {
-        path.push_back(cellCenter(cell));
-    }
-
-    if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f) {
-        path.erase(path.begin());
-    }
-
-    if (!path.empty()) {
-        Cell requestedEndCell;
-        if (worldToCell(end, requestedEndCell) &&
-            requestedEndCell.x == endCell.x &&
-            requestedEndCell.z == endCell.z) {
-            path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
-        }
-    }
-
-    return path;
+    const std::lock_guard<std::mutex> lock(*queryMutex);
+    return runSearch(start, end, buildDynamicOverlay(dynamicObstacles), false);
 }
 
-std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
+std::uint32_t PathFinder::buildDynamicOverlay(
+    const std::vector<DynamicObstacle>& dynamicObstacles) const
+{
+    const size_t cellCount = walkable.size();
+    if (dynamicBlockedGeneration.size() != cellCount) {
+        dynamicBlockedGeneration.assign(cellCount, 0);
+        dynamicGeneration = 0;
+    }
+    if (++dynamicGeneration == 0) {
+        std::fill(dynamicBlockedGeneration.begin(), dynamicBlockedGeneration.end(), 0);
+        dynamicGeneration = 1;
+    }
+    for (const DynamicObstacle& obstacle : dynamicObstacles) {
+        // Include the cell half-diagonal so a segment between adjacent
+        // unblocked cell centres cannot geometrically cut through the circle.
+        const float radius =
+            obstacle.radius + agentRadius + cellSize * 0.70710678f;
+        if (radius <= 0.f) continue;
+        const int minCellX = (std::max)(
+            0, static_cast<int>(std::floor((obstacle.position.x() - radius - minX) / cellSize)));
+        const int maxCellX = (std::min)(
+            gridWidth - 1, static_cast<int>(std::floor((obstacle.position.x() + radius - minX) / cellSize)));
+        const int minCellZ = (std::max)(
+            0, static_cast<int>(std::floor((obstacle.position.z() - radius - minZ) / cellSize)));
+        const int maxCellZ = (std::min)(
+            gridDepth - 1, static_cast<int>(std::floor((obstacle.position.z() + radius - minZ) / cellSize)));
+        const float radiusSq = radius * radius;
+        for (int z = minCellZ; z <= maxCellZ; ++z) {
+            for (int x = minCellX; x <= maxCellX; ++x) {
+                const Cell cell{x, z};
+                const Eigen::Vector3f center = cellCenter(cell);
+                const float dx = center.x() - obstacle.position.x();
+                const float dz = center.z() - obstacle.position.z();
+                if (dx * dx + dz * dz <= radiusSq)
+                    dynamicBlockedGeneration[static_cast<size_t>(indexOf(cell))] =
+                        dynamicGeneration;
+            }
+        }
+    }
+    return dynamicGeneration;
+}
+
+bool PathFinder::isCellWalkableOverlay(
+    const Cell& cell, std::uint32_t generation) const
+{
+    if (!isCellWalkable(cell)) return false;
+    return generation == 0 ||
+        dynamicBlockedGeneration[static_cast<size_t>(indexOf(cell))] != generation;
+}
+
+bool PathFinder::findNearestWalkableCellOverlay(
+    const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const
+{
+    Cell origin;
+    if (!worldToCell(point, origin)) {
+        origin.x = (std::max)(0, (std::min)(
+            gridWidth - 1, static_cast<int>(std::floor((point.x() - minX) / cellSize))));
+        origin.z = (std::max)(0, (std::min)(
+            gridDepth - 1, static_cast<int>(std::floor((point.z() - minZ) / cellSize))));
+    }
+    if (isCellWalkableOverlay(origin, generation)) {
+        out = origin;
+        return true;
+    }
+    const int maxRadius =
+        (std::max)(4, static_cast<int>(std::ceil(8.0f / cellSize)));
+    for (int radius = 1; radius <= maxRadius; ++radius) {
+        for (int z = origin.z - radius; z <= origin.z + radius; ++z) {
+            for (int x = origin.x - radius; x <= origin.x + radius; ++x) {
+                if (std::abs(x - origin.x) != radius &&
+                    std::abs(z - origin.z) != radius) continue;
+                const Cell candidate{x, z};
+                if (isCellWalkableOverlay(candidate, generation)) {
+                    out = candidate;
+                    return true;
+                }
+            }
+        }
+    }
+    return false;
+}
+
+bool PathFinder::hasLineOfSightOverlay(
+    const Cell& from, const Cell& to, std::uint32_t generation) const
+{
+    int x = from.x;
+    int z = from.z;
+    const int dx = std::abs(to.x - from.x);
+    const int dz = std::abs(to.z - from.z);
+    const int sx = from.x < to.x ? 1 : -1;
+    const int sz = from.z < to.z ? 1 : -1;
+    int error = dx - dz;
+    while (true) {
+        if (!isCellWalkableOverlay({x, z}, generation)) return false;
+        if (x == to.x && z == to.z) return true;
+        const int twiceError = error * 2;
+        const int oldX = x;
+        const int oldZ = z;
+        if (twiceError > -dz) {
+            error -= dz;
+            x += sx;
+        }
+        if (twiceError < dx) {
+            error += dx;
+            z += sz;
+        }
+        if (x != oldX && z != oldZ &&
+            (!isCellWalkableOverlay({x, oldZ}, generation) ||
+             !isCellWalkableOverlay({oldX, z}, generation))) {
+            return false;
+        }
+    }
+}
+
+std::vector<PathFinder::Cell> PathFinder::smoothCellsOverlay(
+    const std::vector<Cell>& cells, std::uint32_t generation) const
+{
+    if (cells.size() <= 2) return cells;
+    std::vector<Cell> result;
+    result.push_back(cells.front());
+    size_t anchor = 0;
+    while (anchor + 1 < cells.size()) {
+        size_t next = cells.size() - 1;
+        while (next > anchor + 1 &&
+               !hasLineOfSightOverlay(cells[anchor], cells[next], generation)) {
+            --next;
+        }
+        result.push_back(cells[next]);
+        anchor = next;
+    }
+    return result;
+}
+
+std::vector<Eigen::Vector3f> PathFinder::runSearch(
     const Eigen::Vector3f& start,
     const Eigen::Vector3f& end,
-    const std::vector<unsigned char>& walkableGrid) const
+    std::uint32_t obstacleGeneration,
+    bool nearestReachable) const
 {
-    if (!ready || walkableGrid.empty()) return {};
-
+    if (!ready || walkable.empty()) return {};
     Cell startCell;
     Cell endCell;
-    if (!findNearestWalkableCell(start, startCell, walkableGrid)) return {};
-    if (!findNearestWalkableCell(end,   endCell,   walkableGrid)) return {};
+    if (!findNearestWalkableCellOverlay(start, startCell, obstacleGeneration) ||
+        !findNearestWalkableCellOverlay(end, endCell, obstacleGeneration)) return {};
+    if (startCell.x == endCell.x && startCell.z == endCell.z)
+        return {cellCenter(endCell)};
 
+    const size_t cellCount = walkable.size();
+    if (searchCost.size() != cellCount) {
+        searchCost.resize(cellCount);
+        searchCameFrom.resize(cellCount);
+        searchVisitedGeneration.assign(cellCount, 0);
+        searchGeneration = 0;
+    }
+    if (++searchGeneration == 0) {
+        std::fill(searchVisitedGeneration.begin(), searchVisitedGeneration.end(), 0);
+        searchGeneration = 1;
+    }
+    const auto heuristic = [](const Cell& a, const Cell& b) {
+        const int dx = std::abs(a.x - b.x);
+        const int dz = std::abs(a.z - b.z);
+        return 14 * (std::min)(dx, dz) + 10 * (std::max(dx, dz) - (std::min)(dx, dz));
+    };
     struct QueueNode {
-        int index = 0;
-        float priority = 0.0f;
+        int index;
+        int priority;
+        int cost;
         bool operator<(const QueueNode& other) const {
             if (priority != other.priority) return priority > other.priority;
+            if (cost != other.cost) return cost > other.cost;
             return index > other.index;
         }
     };
-
-    const int cellCount = gridWidth * gridDepth;
-    std::vector<float> cost(static_cast<size_t>(cellCount), INF_COST);
-    std::vector<int> cameFrom(static_cast<size_t>(cellCount), -1);
     std::priority_queue<QueueNode> open;
-
     const int startIndex = indexOf(startCell);
-    const int endIndex   = indexOf(endCell);
-
-    cost[static_cast<size_t>(startIndex)] = 0.f;
-    open.push({ startIndex, 0.f });
-
-    int   bestIndex = startIndex;
-    float bestDist  = distanceCells(startCell, endCell);
-
+    const int endIndex = indexOf(endCell);
+    searchVisitedGeneration[static_cast<size_t>(startIndex)] = searchGeneration;
+    searchCost[static_cast<size_t>(startIndex)] = 0;
+    searchCameFrom[static_cast<size_t>(startIndex)] = -1;
+    open.push({startIndex, heuristic(startCell, endCell), 0});
+    int bestIndex = startIndex;
+    int bestDistanceSq =
+        (startCell.x - endCell.x) * (startCell.x - endCell.x) +
+        (startCell.z - endCell.z) * (startCell.z - endCell.z);
     static const int offsets[8][2] = {
         { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
         { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }
     };
-
     while (!open.empty()) {
         const QueueNode current = open.top();
         open.pop();
-
+        const size_t currentOffset = static_cast<size_t>(current.index);
+        if (searchVisitedGeneration[currentOffset] != searchGeneration ||
+            searchCost[currentOffset] != current.cost) continue;
         if (current.index == endIndex) {
             bestIndex = endIndex;
             break;
         }
-
-        if (cost[static_cast<size_t>(current.index)] < current.priority - distanceCells({ current.index % gridWidth, current.index / gridWidth }, endCell)) {
-            continue; // stale entry
-        }
-
         const Cell currentCell{ current.index % gridWidth, current.index / gridWidth };
-        const float d = distanceCells(currentCell, endCell);
-        if (d < bestDist) {
-            bestDist  = d;
+        const int dx = currentCell.x - endCell.x;
+        const int dz = currentCell.z - endCell.z;
+        const int distanceSq = dx * dx + dz * dz;
+        if (distanceSq < bestDistanceSq ||
+            (distanceSq == bestDistanceSq && current.index < bestIndex)) {
+            bestDistanceSq = distanceSq;
             bestIndex = current.index;
         }
-
         for (const auto& offset : offsets) {
             Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] };
-            if (!isCellWalkable(next, walkableGrid)) continue;
-
+            if (!isCellWalkableOverlay(next, obstacleGeneration)) continue;
             const bool diagonal = offset[0] != 0 && offset[1] != 0;
             if (diagonal) {
                 Cell h{ currentCell.x + offset[0], currentCell.z };
                 Cell v{ currentCell.x, currentCell.z + offset[1] };
-                if (!isCellWalkable(h, walkableGrid) || !isCellWalkable(v, walkableGrid)) continue;
+                if (!isCellWalkableOverlay(h, obstacleGeneration) ||
+                    !isCellWalkableOverlay(v, obstacleGeneration)) continue;
             }
-
-            const int nextIndex   = indexOf(next);
-            const float stepCost  = diagonal ? 1.41421356f : 1.0f;
-            const float newCost   = cost[static_cast<size_t>(current.index)] + stepCost;
-            if (newCost >= cost[static_cast<size_t>(nextIndex)]) continue;
-
-            cost[static_cast<size_t>(nextIndex)]    = newCost;
-            cameFrom[static_cast<size_t>(nextIndex)] = current.index;
-            open.push({ nextIndex, newCost + distanceCells(next, endCell) });
+            const int nextIndex = indexOf(next);
+            const size_t nextOffset = static_cast<size_t>(nextIndex);
+            const int newCost = current.cost + (diagonal ? 14 : 10);
+            if (searchVisitedGeneration[nextOffset] == searchGeneration &&
+                newCost >= searchCost[nextOffset]) continue;
+            searchVisitedGeneration[nextOffset] = searchGeneration;
+            searchCost[nextOffset] = newCost;
+            searchCameFrom[nextOffset] = current.index;
+            open.push({nextIndex, newCost + heuristic(next, endCell), newCost});
         }
     }
-
+    if (!nearestReachable && bestIndex != endIndex) return {};
     if (bestIndex == startIndex) return {};
-
     std::vector<Cell> cells;
-    for (int cur = bestIndex; cur != -1; cur = cameFrom[static_cast<size_t>(cur)]) {
+    for (int cur = bestIndex; cur != -1;
+         cur = searchCameFrom[static_cast<size_t>(cur)]) {
         cells.push_back({ cur % gridWidth, cur / gridWidth });
         if (cur == startIndex) break;
     }
     std::reverse(cells.begin(), cells.end());
-    cells = smoothCells(cells, walkableGrid);
-
+    cells = smoothCellsOverlay(cells, obstacleGeneration);
     std::vector<Eigen::Vector3f> path;
     path.reserve(cells.size());
     for (const Cell& cell : cells) path.push_back(cellCenter(cell));
-
-    if (!path.empty() && (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() < cellSize * 0.75f)
+    if (!path.empty() &&
+        (path.front() - Eigen::Vector3f(start.x(), floorY, start.z())).norm() <
+            cellSize * 0.75f)
         path.erase(path.begin());
-
+    if (bestIndex == endIndex && !path.empty()) {
+        Cell requestedEndCell;
+        if (worldToCell(end, requestedEndCell) &&
+            requestedEndCell.x == endCell.x && requestedEndCell.z == endCell.z) {
+            path.back() = Eigen::Vector3f(end.x(), floorY, end.z());
+        }
+    }
     return path;
 }
 
 std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
     const Eigen::Vector3f& start, const Eigen::Vector3f& end) const
 {
-    auto path = findPath(start, end);
-    if (!path.empty()) return path;
-    return findNearestReachableImpl(start, end, walkable);
+    const std::lock_guard<std::mutex> lock(*queryMutex);
+    return runSearch(start, end, 0, true);
 }
 
 std::vector<Eigen::Vector3f> PathFinder::findPathToNearest(
     const Eigen::Vector3f& start, const Eigen::Vector3f& end,
     const std::vector<DynamicObstacle>& dynamicObstacles) const
 {
-    auto path = findPath(start, end, dynamicObstacles);
-    if (!path.empty()) return path;
-    path = findPath(start, end);
-    if (!path.empty()) return path;
-    return findNearestReachableImpl(start, end, walkable);
+    const std::lock_guard<std::mutex> lock(*queryMutex);
+    return runSearch(
+        start, end, buildDynamicOverlay(dynamicObstacles), true);
 }
 
 bool PathFinder::setAreaAvailable(const std::string& areaName, bool available)

src/navigation/PathFinder.h

@@ -1,6 +1,9 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <cstdint>
+#include <memory>
+#include <mutex>
 #include <string>
 #include <vector>
 
@@ -79,6 +82,17 @@ private:
     std::vector<unsigned char> walkable;
     std::vector<NavigationArea> areas;
 
+    // Reused by synchronous authoritative path queries. Generation stamps avoid
+    // copying/clearing the complete map for every dynamic-obstacle search.
+    mutable std::vector<std::uint32_t> dynamicBlockedGeneration;
+    mutable std::uint32_t dynamicGeneration = 0;
+    mutable std::vector<int> searchCost;
+    mutable std::vector<int> searchCameFrom;
+    mutable std::vector<std::uint32_t> searchVisitedGeneration;
+    mutable std::uint32_t searchGeneration = 0;
+    mutable std::shared_ptr<std::mutex> queryMutex =
+        std::make_shared<std::mutex>();
+
     void loadConfig(const std::string& configPath, const std::string& zipPath);
     bool loadGrid(const std::string& path, const std::string& zipPath);
     void resetGridBounds();
@@ -103,11 +117,20 @@ private:
 	bool hasLineOfSight(const Cell& from, const Cell& to, const std::vector<unsigned char>& walkableGrid) const;
 	std::vector<Cell> smoothCells(const std::vector<Cell>& cells, const std::vector<unsigned char>& walkableGrid) const;
 
-	// A* that runs to exhaustion and returns a path to the reachable cell
-	// geometrically closest to end when end itself is unreachable.
-	std::vector<Eigen::Vector3f> findNearestReachableImpl(const Eigen::Vector3f& start,
-	                                                      const Eigen::Vector3f& end,
-	                                                      const std::vector<unsigned char>& walkableGrid) const;
+    std::uint32_t buildDynamicOverlay(
+        const std::vector<DynamicObstacle>& dynamicObstacles) const;
+    bool isCellWalkableOverlay(const Cell& cell, std::uint32_t generation) const;
+    bool findNearestWalkableCellOverlay(
+        const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const;
+    bool hasLineOfSightOverlay(
+        const Cell& from, const Cell& to, std::uint32_t generation) const;
+    std::vector<Cell> smoothCellsOverlay(
+        const std::vector<Cell>& cells, std::uint32_t generation) const;
+    std::vector<Eigen::Vector3f> runSearch(
+        const Eigen::Vector3f& start,
+        const Eigen::Vector3f& end,
+        std::uint32_t obstacleGeneration,
+        bool nearestReachable) const;
 };
 
 } // namespace FRG