Working about pathfinding

GAME_STATE_ARCHITECTURE.md

@@ -340,8 +340,9 @@ The player state remains campaign-level and survives location changes.
 
 ## Save and Load Schema
 
-Authoritative persistence uses one strict, versioned schema. `schemaVersion` is
-required and unsupported versions are rejected rather than migrated. A
+Authoritative persistence currently uses strict schema version `5`.
+`schemaVersion` is required and unsupported versions are rejected rather than
+migrated. A
 single-player save is a separate versioned envelope containing `networkState`
 (the authoritative `GameState` JSON) and `localState` (the client-local location
 map). Lockstep sends that authoritative document only in `initial_state`; later
@@ -351,6 +352,9 @@ messages contain numbered command bundles.
 - Locations are keyed by location ID.
 - Each location stores NPC and interactive-object state in ID-keyed objects.
 - Pending interaction targets and character references use stable IDs.
+- Character navigation persistence includes waypoints, actual previous-tick
+  movement, blocked/retry state, the blocker ID, and the NPC cooldown sequence
+  and cursor.
 - Immutable content such as meshes, initial taxi configuration, and object
   definitions is reconstructed from campaign configuration.
 - Configuration is loaded first and mutable saved state is overlaid in place.
@@ -478,12 +482,33 @@ cmake --build proj-windows/build --config Debug --target ShadowOverBishkekDemo -
   auxiliary-state synchronization without making `Game` a server friend.
 - Empty player maps are valid. Player-specific interactions remain inactive until
   a player entity is added.
-- Pathfinding priority queues use cell index as a stable secondary key.
+- Pathfinding uses integer cardinal/diagonal costs and compares priority,
+  accumulated cost, then cell index.
 - Gameplay randomness must be seeded and serialized; the `random_device` battle
   bot is disabled for multiplayer sessions.
 - A lockstep compatibility fingerprint must match before the client accepts the
   initial state.
 
+### Dynamic Navigation Occupancy
+
+`LocationGameState` is the authoritative boundary for character-aware planning.
+It collects enabled, living, stationary players and NPCs in stable ID order and
+passes their positions and collision radii to `LocationNetworkState` and
+`PathFinder`. The requester is excluded; moving characters are transparent to
+planning. “Moving” here means actual previous-tick displacement
+(`movedLastTick`), not merely owning an active path, so visually stalled
+characters remain planning obstacles. Route planning uses full avoidance radii;
+the movement guard uses smaller contact radii.
+
+Every character movement step is guarded against stable-ID-ordered reservations
+initialized from tick-start geometry. Accepted steps update their reservation
+before later characters advance, preventing two movers from ending a tick
+overlapped. Blocked/retry counters, cooldown sequence/cursor, and blocker ID are
+serialized character state. Head-on players and bots cancel their commands;
+NPCs retain their targets and consume the next deterministic, ID-shuffled
+cooldown before replanning. Static navigation topology changes route through
+the same centralized replanning path.
+
 ## Main Files
 
 - `src/GameState.h` / `src/GameState.cpp` — aggregate state and top-level persistence.
@@ -502,7 +527,7 @@ cmake --build proj-windows/build --config Debug --target ShadowOverBishkekDemo -
   broadcasting, buffering, and replica advancement.
 - `src/Game.h` / `src/Game.cpp` — application host, campaign lifecycle, and runtime wiring.
 - `src/Location.h` / `src/Location.cpp` — loaded location runtime, command routing,
-  interactions, collisions, scripts, and presentation-event consumption.
+  interactions, scripts, and presentation-event consumption.
 - `src/Character.h` / `src/Character.cpp` — passive character presentation resolving
   immutable `CharacterState` snapshots by stable ID.
 - `src/CharacterState.h` / `src/CharacterState.cpp` — deterministic character

PATHFINDING.md

@@ -13,7 +13,7 @@ This document describes the grid-based pathfinding used for the player and all N
 5. [Approaching Unreachable Destinations](#5-approaching-unreachable-destinations)
 6. [Dynamic Obstacles](#6-dynamic-obstacles)
 7. [Path Following](#7-path-following)
-8. [Character Collision Resolution](#8-character-collision-resolution)
+8. [Character Movement Guard](#8-character-movement-guard)
 9. [Dynamic Replanning](#9-dynamic-replanning)
 10. [Key Constants Reference](#10-key-constants-reference)
 
@@ -103,9 +103,11 @@ Navigation areas can be toggled at runtime via `PathFinder::setAreaAvailable()`,
 
 **Neighbor connectivity:** 8-directional (cardinal + diagonal). Diagonal moves are blocked if either of the two adjacent cardinal cells is unwalkable (no corner-cutting).
 
-**Step costs:** `1.0` for cardinal, `√2 ≈ 1.414` for diagonal.
+**Step costs:** integer `10` for cardinal and `14` for diagonal.
 
-**Heuristic:** Euclidean distance in cell units to the end cell.
+**Heuristic:** octile distance using the same `10`/`14` scale. Queue entries
+compare priority, accumulated cost, then linear cell index; stale entries are
+rejected. This avoids floating-point route-order differences.
 
 **Start/end snapping:** If the exact cell for `start` or `end` is not walkable, `findNearestWalkableCell` expands a square ring outward (up to radius 8 m) to find the nearest walkable cell. This makes clicking slightly outside the nav mesh still produce a valid path.
 
@@ -130,7 +132,9 @@ while anchor is not the last cell:
     anchor = next
 ```
 
-Line-of-sight is checked by stepping along the segment in increments of `cellSize / 2` and verifying that each sampled cell is walkable. The result is a minimal set of waypoints connected by straight, obstacle-free segments.
+Line-of-sight uses deterministic integer grid traversal and applies the same
+no-corner-cutting rule as A*. The result is a minimal set of waypoints connected
+by straight, obstacle-free segments.
 
 ---
 
@@ -138,22 +142,20 @@ Line-of-sight is checked by stepping along the segment in increments of `cellSiz
 
 When a player clicks on a point in a disconnected region (e.g., across a thin wall), the original `findPath` returns an empty path and the character does not move. This is surprising — a click on a solid wall sensibly moves the character to the nearest reachable point, but a click into an inaccessible room does nothing.
 
-**`findPathToNearest`** fixes this with a three-step cascade:
+**`findPathToNearest`** runs one A\* search against the requested static and
+dynamic occupancy. If the destination is unreachable, the same search returns
+the reachable cell geometrically closest to it. It never retries without
+dynamic obstacles.
 
-1. Try `findPath` with dynamic obstacles (stationary characters are avoided).
-2. If empty, retry `findPath` without dynamic obstacles (an NPC blocking a doorway is ignored).
-3. If still empty (destination genuinely unreachable), run **nearest-reachable A\***.
-
-**Nearest-reachable A\***, implemented in `findNearestReachableImpl`:
-
-- Runs the identical A\* loop against the static walkable grid.
-- While processing cells, tracks `bestIndex` — the already-visited cell with the smallest Euclidean distance (in cell units) to the end cell.
+- While processing cells, it tracks `bestIndex` — the visited cell with the
+  smallest squared grid distance to the end cell. Equal distances use the
+  lower linear cell index.
 - If A\* exhausts all reachable space without finding `end`, it reconstructs and returns a path to `bestIndex`.
 - If `bestIndex` is still the start cell (character is completely isolated), an empty path is returned and the character stays put.
 
 The net effect: clicking anywhere in the world always moves the character as close as possible to the target, matching the behaviour of clicking on a solid wall.
 
-`findPathToNearest` replaces the direct `findPath` call in `Location::setupNavigation`'s path planner lambda, so it applies equally to the player and all NPCs.
+`LocationGameState` applies this behavior to players and NPCs.
 
 ---
 
@@ -163,7 +165,10 @@ When a path is planned, other characters can temporarily mark cells as blocked t
 
 **How it works:**
 
-In `Location::setupNavigation`, every character is given a `PathPlanner` closure. Before calling `findPath`, the closure builds a list of `PathFinder::DynamicObstacle` entries (position + radius) representing nearby characters. `findPath` copies the static walkable grid, stamps zeros in circles around each obstacle, then runs A\* on the modified copy. The static grid is never mutated.
+`LocationGameState::collectPathObstacles` creates a stable-ID-sorted snapshot
+from authoritative players and NPCs. `PathFinder` stamps circles into a
+generation-tagged overlay and reuses A\* buffers; it does not copy or mutate the
+static walkability grid per query.
 
 **Which characters become obstacles:**
 
@@ -171,74 +176,101 @@ A character is added as a dynamic obstacle only when **all** of these are true:
 
 - It is not the character currently planning the path (`self`).
 - It is alive and enabled.
-- **It is not moving** — a moving character is transparent to pathfinding, so it does not block narrow corridors that it is actively passing through.
-- Its position lies within `kDynamicObstacleInfluenceDist = 6 m` of the direct line segment from `start` to `end` (distant characters do not affect the search).
+- It did not physically move during the previous simulation tick, unless it
+  just blocked this requester and is promoted for a forced replan. An active
+  path alone does not make a stalled character transparent.
 
-**Obstacle radius:** `character.collisionRadius × 0.6`. Using 60 % of the physical collision radius makes path planning less conservative; physical separation at full radius is still enforced by collision resolution (§8).
-
-**Fallback when dynamic obstacles block the only path:**
-
-If step 1 of `findPathToNearest` (with dynamic obstacles) returns empty, step 2 retries without any dynamic obstacles. This handles the common case of an NPC standing in a doorway: the player paths through the NPC's position, and the nudge logic (§8) pushes the NPC aside as the player passes.
+**Obstacle radius:** the character's full `collisionRadius`, inflated by the
+navigation map's `agentRadius` and one cell half-diagonal for conservative
+rasterization. There is no obstacle-dropping fallback; an occupied destination
+resolves to a nearby reachable cell.
 
 ---
 
 ## 7. Path Following
 
-`Character::setTarget(destination, onArrived)` sets a new walk target. It calls the path planner to generate a waypoint list. The result is stored in `pathWaypoints`; the final destination is also stored in `walkTarget` and `requestedWalkTarget`.
+`CharacterState::setTarget(destination, onArrived)` sets a new walk target. It
+calls the centralized location planner to generate a waypoint list. The result
+is stored in `pathWaypoints`; the reachable endpoint is stored in `walkTarget`
+and the original command destination in `requestedWalkTarget`.
 
-Each frame in `Character::update`:
+Each fixed simulation tick in `CharacterState::update`:
 
 1. **Active target** — if `pathWaypoints` is non-empty, the character moves toward `pathWaypoints[currentWaypointIndex]`; otherwise it moves toward `walkTarget`.
 2. **Movement** — the character advances along the XZ direction at `walkSpeed` m/s and rotates smoothly toward the movement direction at `rotationSpeed` rad/s.
 3. **Waypoint advance** — when the character is within `WALK_THRESHOLD = 0.05 m` of the current waypoint, it advances to the next one. When the last waypoint is reached, `pathWaypoints` is cleared and the optional `onArrived` callback is fired.
 4. **State machine** — the animation state switches between `STAND` and `WALK` based on whether the character is moving.
 
-`Character::isMoving()` returns `true` if `pathWaypoints` is non-empty or the distance to `walkTarget` exceeds `WALK_THRESHOLD`. This is used by dynamic obstacle filtering and collision nudging.
+`CharacterState::isMoving()` reports whether navigation remains active.
+`movedLastTick` separately records actual displacement and controls dynamic
+obstacle filtering.
 
-**Stopping in place:** `Character::stopInPlace()` sets `walkTarget` and `requestedWalkTarget` to the current position and clears `pathWaypoints`. It is called when an external force (collision resolution) displaces a stationary player so that the player does not walk back to their previous target position.
+**Stopping in place:** `CharacterState::stopInPlace()` sets `walkTarget` and
+`requestedWalkTarget` to the current position, clears the path and callback, and
+resets blockage state. It is used to cancel player/bot movement after a
+persistent contact and by other explicit stop actions.
 
 ---
 
-## 8. Character Collision Resolution
+## 8. Character Movement Guard
 
-Pathfinding alone does not prevent two characters from occupying the same space — it only steers paths around stationary characters. Physical separation is handled separately each frame by `Location::resolveCharacterCollisions`.
+Pathfinding steers around stationary characters. A second authoritative guard
+checks each proposed movement segment against stable-ID-ordered reservations
+initialized from tick-start circles. An accepted step updates that character's
+reservation before later characters advance. A step that approaches or sweeps
+through another circle is rejected rather than pushing either character.
 
-**Algorithm** (3 iterations per frame):
+Characters that moved during the previous tick are transparent during planning
+but remain solid to the movement guard. Planning uses the full
+`collisionRadius` (default 0.45 m); contact checks use the smaller
+`movementContactRadius` (default 0.32 m). NPC JSON can override these as
+`navigationAvoidanceRadius` and `movementContactRadius`. This tolerance avoids
+false bumps from grid discretization while still preventing stacking.
 
-For every pair `(A, B)` of living, enabled characters:
-
-1. Compute the overlap: `penetration = (collisionRadius_A + collisionRadius_B) - distance(A, B)`.
-2. If `penetration > 0`, compute a push direction (A-to-B normal) and a push magnitude of `penetration / 2` per character.
-3. Compute candidate new positions `newA` and `newB`.
-4. Validate against the navigation grid (`PathFinder::isWalkable`). If a pushed position is unwalkable, only the other character is moved.
-5. **Player stays put:** if the player was not moving (`!isMoving()`) before the push, `stopInPlace()` is called after the push so the player does not walk back to the old target.
-6. **NPC yielding:** if one character was moving and the other was standing, `nudgeCharacterAside` is called on the standing character.
-
-**`nudgeCharacterAside(standing, awayFrom)`:**
-
-Gives the standing NPC a short walk target so it steps out of the way:
-
-1. Compute the direction from `awayFrom` to the NPC's current position.
-2. Try four candidate targets at distance `1.2 m` in directions: straight away, +90°, −90°, 180°.
-3. Use the first candidate that is walkable (per `PathFinder::isWalkable`).
-4. Call `standing->setTarget(candidate)` — the NPC takes a small step aside, then stands at the new spot.
-5. The player is never nudged; combat NPCs can be nudged, but their attack AI immediately overrides the yield target on the next tick.
+When moving characters remain blocked head-on or while crossing for three
+ticks, both stop. Player and bot destinations are cancelled; the user or bot
+may issue another command. NPCs retain `requestedWalkTarget` and retry later.
+Characters following in the same direction simply wait for spacing and do not
+accumulate stuck ticks. A rejected movement step uses the standing animation.
 
 ---
 
 ## 9. Dynamic Replanning
 
-When characters move they can displace each other or enter each other's planned paths. `Location::updateDynamicReplans` handles this:
+Rejected non-following movement increments serialized `movementBlockedTicks`.
+After three blocked ticks an NPC stops and schedules a retry. When its cooldown
+expires, `InternalGameServer` promotes the blocker to a hard obstacle and
+replans to `requestedWalkTarget`.
 
-**Every frame:**
+Each NPC has `navigationRetryCooldownTicks`, normally ten distinct values.
+`LocationDefinition` deterministically shuffles the sequence from the stable NPC
+ID, and every retry consumes the next value. The sequence and cursor are
+serialized, preventing synchronized retry loops while preserving lockstep.
+NPC JSON can override it with `navigationRetryCooldownTicks` or
+`navigationRetryCooldownMs`.
 
-1. Measure how much each character moved since the last frame. Characters that moved more than `kMovedEps = 0.05 m` are collected as **movers**.
-2. For each mover, find other characters that are currently walking. If the mover's position is within `kReplanTriggerDist = 1.8 m` of the segment `[walker.position → walker.nextWaypoint]`, trigger a replan for the walker via `forceReplan()`.
-3. A per-character cooldown of `kReplanCooldownMs = 500 ms` prevents the same character from replanning more often than twice per second.
+Example NPC tuning:
 
-**`Character::forceReplan()`** re-runs the path planner from the character's current position to its stored `requestedWalkTarget`, updating `pathWaypoints` in place. If the replanned path is empty, the character stops at its current position.
+```json
+{
+  "navigationAvoidanceRadius": 0.45,
+  "movementContactRadius": 0.32,
+  "navigationRetryCooldownMs": [
+    200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100
+  ]
+}
+```
 
-The relatively generous trigger distance (1.8 m vs the old 1.1 m) and cooldown (500 ms vs 300 ms) prevent micro-jitter: small position corrections from collision resolution no longer spam replanning events.
+The configured values are clamped to positive tick counts and shuffled
+deterministically per NPC ID. Supplying different ten-value sets gives explicit
+control for unusually dense locations.
+
+Navigation map switches, area availability changes, and added static obstacle
+polygons also force centralized replanning.
+
+Opposing traffic in a one-character-wide corridor can wait until one route or
+blocker changes; this policy intentionally does not implement space-time
+cooperative reservations.
 
 ---
 
@@ -249,12 +281,16 @@ The relatively generous trigger distance (1.8 m vs the old 1.1 m) and cooldown (
 | `cellSize` | `PathFinder` config | 0.4 m | Grid cell size |
 | `agentRadius` | `PathFinder` config | 0.45 m | Character half-width for grid erosion |
 | `objectPadding` | `PathFinder` config | 0.25 m | Extra clearance around obstacles |
-| `WALK_THRESHOLD` | `Character.h` | 0.05 m | Distance below which a waypoint is considered reached |
-| `TARGET_REPLAN_THRESHOLD` | `Character.h` | 0.25 m | Deduplication threshold in `setTarget` |
-| `kDynamicObstacleInfluenceDist` | `Location.cpp` | 6.0 m | Max distance from path for a character to become an obstacle |
-| `kDynamicObstacleRadiusFraction` | `Location.cpp` | 0.6× | Fraction of collision radius used for dynamic obstacle footprint |
-| `kNudgeDist` | `Location.cpp` | 1.2 m | Distance an NPC steps aside when yielding |
-| `kReplanTriggerDist` | `Location.cpp` | 1.8 m | Mover must be this close to a walker's path to trigger replan |
-| `kReplanCooldownMs` | `Location.cpp` | 500 ms | Minimum interval between replans for any one character |
+| `movementContactRadius` | `CharacterState` / NPC config | 0.32 m | Smaller movement-guard radius |
+| `kWalkThreshold` | `CharacterState.h` | 0.05 m | Distance below which a waypoint is considered reached |
+| `TARGET_REPLAN_THRESHOLD` | `CharacterState.cpp` | 0.25 m | Deduplication threshold in `setTarget` |
+| `BLOCKED_REPLAN_TICKS` | `CharacterState.cpp` | 3 ticks | Blocked steps before stopping |
+| default retry sequence | `CharacterState.cpp` | 2–11 ticks | Ten ID-shuffled NPC retry cooldowns |
 | `NPC_TALK_DISTANCE` | `Location.cpp` | 1.35 m | Distance at which walking-to-NPC interaction fires |
-| `kIterations` (collision) | `Location.cpp` | 3 | Push-apart iterations per frame |
+
+### Reference benchmark
+
+`GameServerPathFinderTests --benchmark` is non-gating. On the development
+Windows Release build with an 80×80 grid, the current implementation completed
+100 obstacle-aware queries in 15 ms and 1000 queries in 176 ms. Treat these as
+comparative measurements rather than cross-machine budgets.

WEBSOCKET_NETWORKING.md

@@ -75,15 +75,15 @@ When `NETWORK` is defined:
 
 The WebSocket client is created only when `battle_zone_multiplayer` is selected
 and is destroyed when that campaign is unloaded. The single-player and
-multiplayer Battle Zone entries share location content and the start-state path,
-so the state can be regenerated through the single-player entry. At startup, the
-standalone server loads the authoritative half of that shared save and rewrites
-its campaign ID to `battle_zone_multiplayer`; each remote client restores only
-the local presentation half. The multiplayer campaign currently declares the
+multiplayer Battle Zone entries share location content but use separate
+start-state files. At startup, the standalone server loads the multiplayer
+start state; each remote client restores the received authoritative state and
+its own local presentation state. The multiplayer campaign currently declares the
 pre-existing `player` and `player2` state entries as connection
 slots; the random test-bot controller is disabled.
-The campaign's `networkCompatibility` token is the fingerprint; release builds
-must bump it whenever deterministic content changes.
+The campaign's `networkCompatibility` token is the fingerprint (currently
+`battle-zone-lockstep-v5`); release builds must bump it whenever deterministic
+content changes.
 
 If DNS resolution, TCP connection, the WebSocket handshake, or the active socket
 fails, the client unloads the remote campaign and returns to the campaign
@@ -180,6 +180,16 @@ submission, and contiguous tick consumption.
 
 ## Current Limitations
 
+- Paths are calculated independently on every lockstep participant from the
+  same ordered commands and tick-start occupancy. Integer A* costs, stable
+  character IDs, serialized previous-tick movement and blocked/retry state, and deterministic
+  nearest-reachable ties keep route selection aligned.
+- Moving characters are omitted from A* but remain solid to the authoritative
+  movement guard. After three blocked ticks, players/bots cancel movement while
+  NPCs stop and consume the next serialized cooldown from their deterministic
+  retry sequence before replanning around the blocker.
+- Opposing traffic can wait in a one-character-wide corridor because the
+  current policy does not reserve future space-time cells.
 - Only preconfigured player slots can connect; dynamic join/spawn commands are
   not implemented.
 - There is no authentication or reconnect/replay history.

resources/config/campaigns.json

@@ -40,7 +40,7 @@
             "quests": "resources/config/quests_battle_zone.json",
             "tutorial": false,
             "networkPlayerSlots": ["player", "player2"],
-            "networkCompatibility": "battle-zone-lockstep-v2",
+            "networkCompatibility": "battle-zone-lockstep-v5",
             "loadingScreen": "resources/w/ui/screen_loading.json"
         }
     ]

resources/config/locations/battle_zone_uni_exterior.json

@@ -5,6 +5,7 @@
     "npcs": "resources/config/npcs_battle_zone.json",
     "dialogues": "resources/dialogue/battle_zone_dialogues.json",
     "navigation": [
+	 "resources/navigation/battle_zone001.txt",
         "resources/navigation/dorm0_large.json"
     ],
     "script": "resources/start_battle_zone.lua",

resources/config/npcs_battle_zone.json

@@ -14,7 +14,7 @@
         "animationActionToStandPath": "resources/w/shadow/shadow_idle003.anim",
         "animationActionToDeathPath": "resources/w/shadow/shadow_die003.anim",
         "animationDeathIdlePath": "resources/w/shadow/shadow_dead003.anim",
-        "positionX": -6.0,
+        "positionX": -5.6,
         "positionY": 0.0,
         "positionZ": -12.0,
         "walkSpeed": 1.5,
@@ -26,6 +26,141 @@
         "hp": 35,
 		"visibleRange" : 6.5,
         "canAttack": true
+      },
+	  {
+        "id": "ghost_02x",
+        "name": "Тень",
+		"meshTextures": {
+			"Shadow_NPC_Rigged":      "resources/w/shadow/Shadow_BaseColor.png"
+		  },
+        "animationIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationWalkPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionAttackPath": "resources/w/shadow/shadow_attack003.anim",
+        "animationStandToActionPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToStandPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToDeathPath": "resources/w/shadow/shadow_die003.anim",
+        "animationDeathIdlePath": "resources/w/shadow/shadow_dead003.anim",
+        "positionX": -6.4,
+        "positionY": 0.0,
+        "positionZ": -12.0,
+        "walkSpeed": 1.5,
+        "rotationSpeed": 8.0,
+        "modelScale": 1.0,
+        "modelCorrectionRotX": -90.0,
+        "modelCorrectionRotY": 180.0,
+        "modelCorrectionRotZ": 0.0,
+        "hp": 35,
+		"visibleRange" : 6.5,
+        "canAttack": true
+      },
+	  {
+        "id": "ghost_03x",
+        "name": "Тень",
+		"meshTextures": {
+			"Shadow_NPC_Rigged":      "resources/w/shadow/Shadow_BaseColor.png"
+		  },
+        "animationIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationWalkPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionAttackPath": "resources/w/shadow/shadow_attack003.anim",
+        "animationStandToActionPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToStandPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToDeathPath": "resources/w/shadow/shadow_die003.anim",
+        "animationDeathIdlePath": "resources/w/shadow/shadow_dead003.anim",
+        "positionX": -5.6,
+        "positionY": 0.0,
+        "positionZ": -12.5,
+        "walkSpeed": 1.5,
+        "rotationSpeed": 8.0,
+        "modelScale": 1.0,
+        "modelCorrectionRotX": -90.0,
+        "modelCorrectionRotY": 180.0,
+        "modelCorrectionRotZ": 0.0,
+        "hp": 35,
+		"visibleRange" : 6.5,
+        "canAttack": true
+      },
+	  {
+        "id": "ghost_04x",
+        "name": "Тень",
+		"meshTextures": {
+			"Shadow_NPC_Rigged":      "resources/w/shadow/Shadow_BaseColor.png"
+		  },
+        "animationIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationWalkPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionAttackPath": "resources/w/shadow/shadow_attack003.anim",
+        "animationStandToActionPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToStandPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToDeathPath": "resources/w/shadow/shadow_die003.anim",
+        "animationDeathIdlePath": "resources/w/shadow/shadow_dead003.anim",
+        "positionX": -6.4,
+        "positionY": 0.0,
+        "positionZ": -12.5,
+        "walkSpeed": 1.5,
+        "rotationSpeed": 8.0,
+        "modelScale": 1.0,
+        "modelCorrectionRotX": -90.0,
+        "modelCorrectionRotY": 180.0,
+        "modelCorrectionRotZ": 0.0,
+        "hp": 35,
+		"visibleRange" : 6.5,
+        "canAttack": true
+      },
+	  {
+        "id": "ghost_05x",
+        "name": "Тень",
+		"meshTextures": {
+			"Shadow_NPC_Rigged":      "resources/w/shadow/Shadow_BaseColor.png"
+		  },
+        "animationIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationWalkPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionAttackPath": "resources/w/shadow/shadow_attack003.anim",
+        "animationStandToActionPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToStandPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToDeathPath": "resources/w/shadow/shadow_die003.anim",
+        "animationDeathIdlePath": "resources/w/shadow/shadow_dead003.anim",
+        "positionX": -5.6,
+        "positionY": 0.0,
+        "positionZ": -13.0,
+        "walkSpeed": 1.5,
+        "rotationSpeed": 8.0,
+        "modelScale": 1.0,
+        "modelCorrectionRotX": -90.0,
+        "modelCorrectionRotY": 180.0,
+        "modelCorrectionRotZ": 0.0,
+        "hp": 35,
+		"visibleRange" : 6.5,
+        "canAttack": true
+      },
+	  {
+        "id": "ghost_06x",
+        "name": "Тень",
+		"meshTextures": {
+			"Shadow_NPC_Rigged":      "resources/w/shadow/Shadow_BaseColor.png"
+		  },
+        "animationIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationWalkPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionIdlePath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionAttackPath": "resources/w/shadow/shadow_attack003.anim",
+        "animationStandToActionPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToStandPath": "resources/w/shadow/shadow_idle003.anim",
+        "animationActionToDeathPath": "resources/w/shadow/shadow_die003.anim",
+        "animationDeathIdlePath": "resources/w/shadow/shadow_dead003.anim",
+        "positionX": -6.4,
+        "positionY": 0.0,
+        "positionZ": -13.0,
+        "walkSpeed": 1.5,
+        "rotationSpeed": 8.0,
+        "modelScale": 1.0,
+        "modelCorrectionRotX": -90.0,
+        "modelCorrectionRotY": 180.0,
+        "modelCorrectionRotZ": 0.0,
+        "hp": 35,
+		"visibleRange" : 6.5,
+        "canAttack": true
       }
     ]
 }

resources/config/start_state_battle_zone.json

@@ -121,7 +121,326 @@
             "scale": 1.0
           }
         },
-        "npcs": {},
+        "npcs": {
+          "ghost_01x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              2,
+              3,
+              10,
+              5,
+              6,
+              8,
+              7,
+              9,
+              4,
+              11
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ],
+            "requestedWalkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ]
+          },
+          "ghost_02x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              8,
+              11,
+              3,
+              10,
+              2,
+              9,
+              5,
+              4,
+              7,
+              6
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ],
+            "requestedWalkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ]
+          },
+          "ghost_03x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              2,
+              4,
+              8,
+              3,
+              6,
+              10,
+              9,
+              7,
+              5,
+              11
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ],
+            "requestedWalkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ]
+          },
+          "ghost_04x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              3,
+              4,
+              7,
+              10,
+              5,
+              11,
+              9,
+              2,
+              8,
+              6
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ],
+            "requestedWalkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ]
+          },
+          "ghost_05x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              8,
+              7,
+              2,
+              9,
+              4,
+              10,
+              6,
+              5,
+              3,
+              11
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ],
+            "requestedWalkTarget": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ]
+          },
+          "ghost_06x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              7,
+              11,
+              5,
+              4,
+              3,
+              10,
+              9,
+              8,
+              6,
+              2
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ],
+            "requestedWalkTarget": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ]
+          }
+        },
         "scriptGlobals": {},
         "state": {
           "activeNavigationIndex": 0,
@@ -171,24 +490,28 @@
           0.0
         ],
         "hp": 200.0,
-        "pathWaypoints": [],
+        "pathWaypoints": [[
+            -6.4,
+            0.0,
+            13.0
+          ]],
         "position": [
+            -6.4,
             0.0,
-          0.0,
-          0.0
+            13.0
         ],
         "requestedWalkTarget": [
+            -6.4,
             0.0,
-          0.0,
-          0.0
+            13.0
         ],
         "showWeapon": true,
         "targetFacingAngle": 0.0,
         "visibleRange": 6.0,
         "walkTarget": [
+            -6.4,
             0.0,
-          0.0,
-          0.0
+            13.0
         ]
       },
       "player": {
@@ -198,7 +521,7 @@
         "battle_state": 0,
         "currentState": 0,
         "currentWaypointIndex": 0,
-        "enabled": false,
+        "enabled": true,
         "faceTargetId": "",
         "facingAngle": 0.0,
         "homeDriftCheckTimer": 5.0,
@@ -210,28 +533,28 @@
         "hp": 200.0,
         "pathWaypoints": [
           [
-            -6.025001525878906,
+            -5.8,
             0.0,
-            12.775001525878906
+            12.8
           ]
         ],
         "position": [
-          -6.0,
+          -5.8,
           0.0,
-          12.800000190734863
+          12.8
         ],
         "requestedWalkTarget": [
-          -6.0,
+          -5.8,
           0.0,
-          12.800000190734863
+          12.8
         ],
         "showWeapon": true,
         "targetFacingAngle": 0.0,
         "visibleRange": 6.0,
         "walkTarget": [
-          -6.025001525878906,
+          -5.8,
           0.0,
-          12.775001525878906
+          12.8
         ]
       }
     },
@@ -239,7 +562,7 @@
       "quests": []
     },
     "savedAt": "2026-11-05 17:43",
-    "schemaVersion": 2,
+    "schemaVersion": 5,
     "taxiIsCalled": false,
     "tutorialJournalScreenOpened": false,
     "tutorialNeedOpenTaxiScreen": false,

resources/config/start_state_battle_zone_singleplayer.json

@@ -121,7 +121,326 @@
             "scale": 1.0
           }
         },
-        "npcs": {},
+        "npcs": {
+          "ghost_01x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              2,
+              3,
+              10,
+              5,
+              6,
+              8,
+              7,
+              9,
+              4,
+              11
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ],
+            "requestedWalkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.0
+            ]
+          },
+          "ghost_02x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              8,
+              11,
+              3,
+              10,
+              2,
+              9,
+              5,
+              4,
+              7,
+              6
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ],
+            "requestedWalkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.0
+            ]
+          },
+          "ghost_03x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              2,
+              4,
+              8,
+              3,
+              6,
+              10,
+              9,
+              7,
+              5,
+              11
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ],
+            "requestedWalkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -5.599999904632568,
+              0.0,
+              -12.5
+            ]
+          },
+          "ghost_04x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              3,
+              4,
+              7,
+              10,
+              5,
+              11,
+              9,
+              2,
+              8,
+              6
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ],
+            "requestedWalkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -6.400000095367432,
+              0.0,
+              -12.5
+            ]
+          },
+          "ghost_05x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              8,
+              7,
+              2,
+              9,
+              4,
+              10,
+              6,
+              5,
+              3,
+              11
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ],
+            "requestedWalkTarget": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -5.599999904632568,
+              0.0,
+              -13.0
+            ]
+          },
+          "ghost_06x": {
+            "animationFrame": 0.0,
+            "attackTargetId": "",
+            "attack_cooldown": 0.0,
+            "battle_state": 0,
+            "currentState": 0,
+            "currentWaypointIndex": 0,
+            "enabled": true,
+            "faceTargetId": "",
+            "facingAngle": 0.0,
+            "homeDriftCheckTimer": 5.0,
+            "homePosition": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ],
+            "hp": 35.0,
+            "movedLastTick": false,
+            "movementBlockedTicks": 0,
+            "navigationRetryCooldownIndex": 0,
+            "navigationRetryCooldownTicks": [
+              7,
+              11,
+              5,
+              4,
+              3,
+              10,
+              9,
+              8,
+              6,
+              2
+            ],
+            "navigationRetryTicks": 0,
+            "pathWaypoints": [],
+            "position": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ],
+            "requestedWalkTarget": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ],
+            "showWeapon": true,
+            "targetFacingAngle": 0.0,
+            "visibleRange": 6.5,
+            "walkTarget": [
+              -6.400000095367432,
+              0.0,
+              -13.0
+            ]
+          }
+        },
         "scriptGlobals": {},
         "state": {
           "activeNavigationIndex": 0,
@@ -171,24 +490,28 @@
           0.0
         ],
         "hp": 200.0,
-        "pathWaypoints": [],
+        "pathWaypoints": [[
+            -6.8,
+            0.0,
+            13.0
+          ]],
         "position": [
+            -6.8,
             0.0,
-          0.0,
-          0.0
+            13.0
         ],
         "requestedWalkTarget": [
+            -6.8,
             0.0,
-          0.0,
-          0.0
+            13.0
         ],
         "showWeapon": true,
         "targetFacingAngle": 0.0,
         "visibleRange": 6.0,
         "walkTarget": [
+            -6.8,
             0.0,
-          0.0,
-          0.0
+            13.0
         ]
       },
       "player": {
@@ -210,28 +533,28 @@
         "hp": 200.0,
         "pathWaypoints": [
           [
-            -6.025001525878906,
+            -5.4,
             0.0,
-            12.775001525878906
+            12.8
           ]
         ],
         "position": [
-          -6.0,
+          -5.4,
           0.0,
-          12.800000190734863
+          12.8
         ],
         "requestedWalkTarget": [
-          -6.0,
+          -5.4,
           0.0,
-          12.800000190734863
+          12.8
         ],
         "showWeapon": true,
         "targetFacingAngle": 0.0,
         "visibleRange": 6.0,
         "walkTarget": [
-          -6.025001525878906,
+          -5.4,
           0.0,
-          12.775001525878906
+          12.8
         ]
       }
     },
@@ -239,7 +562,7 @@
       "quests": []
     },
     "savedAt": "2026-11-05 17:43",
-    "schemaVersion": 2,
+    "schemaVersion": 5,
     "taxiIsCalled": false,
     "tutorialJournalScreenOpened": false,
     "tutorialNeedOpenTaxiScreen": false,

server/CMakeLists.txt

@@ -125,6 +125,14 @@ endif()
 
 include(CTest)
 if(BUILD_TESTING)
+	add_executable(
+		GameServerPathFinderTests tests/PathFinderTests.cpp)
+	target_link_libraries(GameServerPathFinderTests PRIVATE game_server_core)
+	source_group(
+		"Server\\Tests" FILES tests/PathFinderTests.cpp)
+	add_test(
+		NAME GameServer.PathFinder
+		COMMAND GameServerPathFinderTests)
 	add_executable(
 		GameServerStateTests tests/LocationGameStateTests.cpp)
 	target_link_libraries(GameServerStateTests PRIVATE game_server_core)

server/tests/LocationGameStateTests.cpp

@@ -276,5 +276,58 @@ int main()
 		return fail("Enabled player acquired an autonomous attack target");
 	}
 
+	FRG::CharacterState guardedMover;
+	guardedMover.configureAsPlayer();
+	guardedMover.enabled = true;
+	guardedMover.position = Eigen::Vector3f::Zero();
+	guardedMover.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
+	guardedMover.requestedWalkTarget = guardedMover.walkTarget;
+	guardedMover.pathWaypoints = {guardedMover.walkTarget};
+	const std::vector<FRG::CharacterMovementObstacle> movementObstacles{
+		{"mover", Eigen::Vector3f::Zero(), 0.45f},
+		{"blocker", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f}};
+	FRG::CharacterUpdateContext guardedContext;
+	guardedContext.characterId = "mover";
+	guardedContext.movementObstacles = &movementObstacles;
+	FRG::CharacterUpdateEvents blockedEvents;
+	for (int i = 0; i < 3; ++i)
+		blockedEvents = guardedMover.update(100, guardedContext);
+	if (!guardedMover.position.isZero() ||
+		blockedEvents.forceNavigationReplan ||
+		!guardedMover.pathWaypoints.empty()) {
+		return fail("Movement guard did not cancel the blocked player action");
+	}
+	nlohmann::json guardedJson;
+	guardedMover.save(guardedJson);
+	FRG::CharacterState restoredGuardedMover;
+	restoredGuardedMover.load(guardedJson);
+	if (restoredGuardedMover.movementBlockedTicks !=
+			guardedMover.movementBlockedTicks ||
+		restoredGuardedMover.navigationRetryTicks !=
+			guardedMover.navigationRetryTicks) {
+		return fail("Movement blockage state did not round trip");
+	}
+
+	FRG::LocationGameState occupancyState;
+	auto stationary = std::make_shared<FRG::CharacterState>();
+	stationary->enabled = true;
+	stationary->hp = 10.f;
+	stationary->position = Eigen::Vector3f(4.f, 0.f, 0.f);
+	auto moving = std::make_shared<FRG::CharacterState>();
+	moving->enabled = true;
+	moving->hp = 10.f;
+	moving->pathWaypoints = {Eigen::Vector3f(8.f, 0.f, 0.f)};
+	moving->movedLastTick = true;
+	FRG::PlayerStateMap occupancyPlayers{
+		{"stationary", stationary}, {"moving", moving}};
+	if (occupancyState.collectPathObstacles(
+			"requester", occupancyPlayers).size() != 1 ||
+		occupancyState.collectPathObstacles(
+			"requester", occupancyPlayers, "moving").size() != 2 ||
+		!occupancyState.collectPathObstacles(
+			"stationary", occupancyPlayers).empty()) {
+		return fail("Stationary obstacle collection policy is incorrect");
+	}
+
 	return EXIT_SUCCESS;
 }

server/tests/PathFinderTests.cpp

@@ -0,0 +1,269 @@
+#include "CharacterState.h"
+#include "LocationGameState.h"
+#include "navigation/PathFinder.h"
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdlib>
+#include <filesystem>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace {
+
+int fail(const char* message)
+{
+	std::cout << "PathFinderTests: " << message << std::endl;
+	return EXIT_FAILURE;
+}
+
+std::filesystem::path writeGrid(
+	const std::string& name, int width, int depth, bool corridor)
+{
+	const auto path = std::filesystem::temp_directory_path() / name;
+	std::ofstream output(path);
+	output << "cellSize 1\nagentRadius 0.1\nfloorY 0\n"
+		   << "minX 0\nminZ 0\ngridWidth " << width
+		   << "\ngridDepth " << depth << "\n";
+	for (int z = 0; z < depth; ++z) {
+		for (int x = 0; x < width; ++x)
+			output << (!corridor || z == depth / 2 ? '1' : '0');
+		output << '\n';
+	}
+	return path;
+}
+
+bool samePath(
+	const std::vector<Eigen::Vector3f>& lhs,
+	const std::vector<Eigen::Vector3f>& rhs)
+{
+	if (lhs.size() != rhs.size()) return false;
+	for (size_t i = 0; i < lhs.size(); ++i)
+		if (lhs[i] != rhs[i]) return false;
+	return true;
+}
+
+float segmentDistanceSquared(
+	const Eigen::Vector3f& point,
+	const Eigen::Vector3f& start,
+	const Eigen::Vector3f& end)
+{
+	const Eigen::Vector2f p(point.x(), point.z());
+	const Eigen::Vector2f a(start.x(), start.z());
+	const Eigen::Vector2f segment(end.x() - start.x(), end.z() - start.z());
+	const float lengthSquared = segment.squaredNorm();
+	const float t = lengthSquared > 1e-8f
+		? (std::max)(0.f, (std::min)(1.f, (p - a).dot(segment) / lengthSquared))
+		: 0.f;
+	return (p - (a + segment * t)).squaredNorm();
+}
+
+void runBenchmark(FRG::PathFinder& pathFinder, int queries)
+{
+	std::vector<FRG::PathFinder::DynamicObstacle> obstacles;
+	for (int i = 0; i < queries; ++i) {
+		const float x = 4.5f + static_cast<float>((i * 7) % 70);
+		const float z = 4.5f + static_cast<float>((i * 13) % 70);
+		obstacles.push_back({Eigen::Vector3f(x, 0.f, z), 0.25f});
+	}
+	const auto start = std::chrono::steady_clock::now();
+	size_t waypointCount = 0;
+	for (int i = 0; i < queries; ++i) {
+		const Eigen::Vector3f from(
+			1.5f, 0.f, 1.5f + static_cast<float>(i % 30));
+		const Eigen::Vector3f to(
+			78.5f, 0.f, 78.5f - static_cast<float>(i % 30));
+		waypointCount +=
+			pathFinder.findPathToNearest(from, to, obstacles).size();
+	}
+	const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
+		std::chrono::steady_clock::now() - start);
+	std::cout << "PathFinder benchmark " << queries << " characters/queries: "
+			  << elapsed.count() << " ms, " << waypointCount
+			  << " waypoints" << std::endl;
+}
+
+} // namespace
+
+int main(int argc, char** argv)
+{
+	const bool benchmark = argc > 1 && std::string(argv[1]) == "--benchmark";
+	const auto openGrid = writeGrid("frg_pathfinder_open.txt", 80, 80, false);
+	FRG::PathFinder pathFinder;
+	pathFinder.build(openGrid.string());
+	if (!pathFinder.isReady()) return fail("Open test grid did not load");
+
+	const FRG::PathFinder::DynamicObstacle first{
+		Eigen::Vector3f(8.5f, 0.f, 8.5f), 0.75f};
+	const FRG::PathFinder::DynamicObstacle second{
+		Eigen::Vector3f(12.5f, 0.f, 10.5f), 0.75f};
+	const Eigen::Vector3f start(2.5f, 0.f, 8.5f);
+	const Eigen::Vector3f end(18.5f, 0.f, 8.5f);
+	const auto ordered = pathFinder.findPathToNearest(
+		start, end, {first, second});
+	const auto reversed = pathFinder.findPathToNearest(
+		start, end, {second, first});
+	if (ordered.empty() || !samePath(ordered, reversed))
+		return fail("Obstacle insertion order changed the route");
+	Eigen::Vector3f previous = start;
+	for (const Eigen::Vector3f& point : ordered) {
+		if (segmentDistanceSquared(
+				first.position, previous, point) < 0.85f * 0.85f)
+			return fail("Smoothed path crossed a dynamic obstacle");
+		previous = point;
+	}
+
+	const auto occupiedDestination =
+		pathFinder.findPathToNearest(start, first.position, {first});
+	if (occupiedDestination.empty())
+		return fail("Occupied destination did not produce a nearest route");
+	const Eigen::Vector3f occupiedEnd = occupiedDestination.back();
+	const Eigen::Vector2f occupiedDelta(
+		occupiedEnd.x() - first.position.x(),
+		occupiedEnd.z() - first.position.z());
+	if (occupiedDelta.squaredNorm() < 0.85f * 0.85f)
+		return fail("Occupied destination ended inside the obstacle");
+
+	const auto corridorGrid =
+		writeGrid("frg_pathfinder_corridor.txt", 20, 3, true);
+	FRG::PathFinder corridor;
+	corridor.build(corridorGrid.string());
+	const FRG::PathFinder::DynamicObstacle blocker{
+		Eigen::Vector3f(10.5f, 0.f, 1.5f), 0.45f};
+	const auto nearest = corridor.findPathToNearest(
+		Eigen::Vector3f(1.5f, 0.f, 1.5f),
+		Eigen::Vector3f(18.5f, 0.f, 1.5f), {blocker});
+	if (nearest.empty() || nearest.back().x() >= blocker.position.x())
+		return fail("Nearest fallback ignored a dynamic blocker");
+
+	FRG::CharacterState guardedMover;
+	guardedMover.configureAsPlayer();
+	guardedMover.enabled = true;
+	guardedMover.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
+	guardedMover.requestedWalkTarget = guardedMover.walkTarget;
+	guardedMover.pathWaypoints = {guardedMover.walkTarget};
+	const std::vector<FRG::CharacterMovementObstacle> movementObstacles{
+		{"mover", Eigen::Vector3f::Zero(), 0.45f},
+		{"blocker", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f}};
+	FRG::CharacterUpdateContext guardedContext;
+	guardedContext.characterId = "mover";
+	guardedContext.movementObstacles = &movementObstacles;
+	FRG::CharacterUpdateEvents blockedEvents;
+	for (int i = 0; i < 3; ++i)
+		blockedEvents = guardedMover.update(100, guardedContext);
+	if (!guardedMover.position.isZero() ||
+		blockedEvents.forceNavigationReplan ||
+		!guardedMover.pathWaypoints.empty() ||
+		guardedMover.requestedWalkTarget != guardedMover.position ||
+		guardedMover.currentState != FRG::AnimationState::STAND)
+		return fail("Blocked player action was not cancelled");
+
+	FRG::CharacterState retryingNpc;
+	retryingNpc.enabled = true;
+	retryingNpc.configureNavigationRetryCooldowns(
+		"retrying_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11});
+	FRG::CharacterState otherNpc;
+	otherNpc.configureNavigationRetryCooldowns(
+		"other_npc", {2, 3, 4, 5, 6, 7, 8, 9, 10, 11});
+	if (otherNpc.navigationRetryCooldownTicks ==
+		retryingNpc.navigationRetryCooldownTicks)
+		return fail("Stable NPC IDs produced the same retry sequence");
+	const int firstCooldown =
+		retryingNpc.navigationRetryCooldownTicks.front();
+	retryingNpc.walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
+	retryingNpc.requestedWalkTarget = retryingNpc.walkTarget;
+	retryingNpc.pathWaypoints = {retryingNpc.walkTarget};
+	const std::vector<FRG::CharacterMovementObstacle> movingBlocker{
+		{"b", Eigen::Vector3f(0.8f, 0.f, 0.f), 0.45f, true,
+		 Eigen::Vector3f(-1.f, 0.f, 0.f)}};
+	FRG::CharacterUpdateContext retryContext;
+	retryContext.characterId = "retrying_npc";
+	retryContext.movementObstacles = &movingBlocker;
+	FRG::CharacterUpdateEvents retryEvents;
+	for (int i = 0; i < 3; ++i)
+		retryEvents = retryingNpc.update(100, retryContext);
+	if (retryEvents.forceNavigationReplan ||
+		!retryingNpc.pathWaypoints.empty() ||
+		retryingNpc.navigationRetryTicks != firstCooldown ||
+		retryingNpc.requestedWalkTarget != Eigen::Vector3f(2.f, 0.f, 0.f))
+		return fail("Blocked NPC did not stop with a deterministic cooldown");
+	for (int i = 0; i < firstCooldown; ++i)
+		retryEvents = retryingNpc.update(100, retryContext);
+	if (!retryEvents.forceNavigationReplan ||
+		retryEvents.blockingCharacterId != "b")
+		return fail("Blocked NPC did not retry after its cooldown");
+
+	retryingNpc.movedLastTick = true;
+	nlohmann::json blockedState;
+	retryingNpc.save(blockedState);
+	FRG::CharacterState restoredMover;
+	restoredMover.load(blockedState);
+	if (restoredMover.movementBlockedTicks !=
+			retryingNpc.movementBlockedTicks ||
+		restoredMover.navigationRetryTicks !=
+			retryingNpc.navigationRetryTicks ||
+		restoredMover.navigationRetryCooldownTicks !=
+			retryingNpc.navigationRetryCooldownTicks ||
+		restoredMover.navigationRetryCooldownIndex !=
+			retryingNpc.navigationRetryCooldownIndex ||
+		!restoredMover.movedLastTick)
+		return fail("Blocked movement state did not round trip");
+
+	FRG::LocationGameState occupancyState;
+	auto stationary = std::make_shared<FRG::CharacterState>();
+	stationary->enabled = true;
+	stationary->hp = 10.f;
+	auto moving = std::make_shared<FRG::CharacterState>();
+	moving->enabled = true;
+	moving->hp = 10.f;
+	moving->pathWaypoints = {Eigen::Vector3f(4.f, 0.f, 0.f)};
+	moving->movedLastTick = true;
+	FRG::PlayerStateMap occupancyPlayers{
+		{"stationary", stationary}, {"moving", moving}};
+	if (occupancyState.collectPathObstacles(
+			"requester", occupancyPlayers).size() != 1 ||
+		occupancyState.collectPathObstacles(
+			"requester", occupancyPlayers, "moving").size() != 2 ||
+		!occupancyState.collectPathObstacles(
+			"stationary", occupancyPlayers).empty())
+		return fail("Stationary occupancy policy is incorrect");
+	moving->movedLastTick = false;
+	if (occupancyState.collectPathObstacles(
+			"requester", occupancyPlayers).size() != 2)
+		return fail("Physically stalled character was omitted from planning");
+
+	FRG::LocationGameState approachingState;
+	auto left = std::make_shared<FRG::CharacterState>();
+	left->configureAsPlayer();
+	left->enabled = true;
+	left->position = Eigen::Vector3f(0.f, 0.f, 0.f);
+	left->walkTarget = Eigen::Vector3f(2.f, 0.f, 0.f);
+	left->requestedWalkTarget = left->walkTarget;
+	left->pathWaypoints = {left->walkTarget};
+	auto right = std::make_shared<FRG::CharacterState>();
+	right->configureAsPlayer();
+	right->enabled = true;
+	right->position = Eigen::Vector3f(1.4f, 0.f, 0.f);
+	right->walkTarget = Eigen::Vector3f(-1.f, 0.f, 0.f);
+	right->requestedWalkTarget = right->walkTarget;
+	right->pathWaypoints = {right->walkTarget};
+	FRG::PlayerStateMap approachingPlayers{{"a", left}, {"b", right}};
+	for (int i = 0; i < 5; ++i)
+		approachingState.update(100, approachingPlayers, false);
+	const float separation = (right->position - left->position).norm();
+	if (separation + 1e-5f <
+			left->movementContactRadius + right->movementContactRadius ||
+		!left->pathWaypoints.empty() || !right->pathWaypoints.empty())
+		return fail("Colliding players did not stop without overlap");
+
+	if (benchmark) {
+		runBenchmark(pathFinder, 100);
+		runBenchmark(pathFinder, 1000);
+	}
+	std::filesystem::remove(openGrid);
+	std::filesystem::remove(corridorGrid);
+	return EXIT_SUCCESS;
+}

server/tests/WebsocketLockstepTests.cpp

@@ -41,6 +41,15 @@ int main()
 	nlohmann::json initialState;
 	authoritative.save(initialState);
 	initialState["players"]["player2"] = initialState["players"]["player"];
+	FRG::LocationGameState testLocation;
+	auto hostileNpc = std::make_shared<FRG::CharacterState>();
+	hostileNpc->enabled = true;
+	hostileNpc->hp = 200.f;
+	testLocation.npcs.emplace("hostile_npc", std::move(hostileNpc));
+	nlohmann::json testLocationState;
+	testLocation.save(testLocationState);
+	initialState["currentLocationId"] = "test_location";
+	initialState["locations"]["test_location"] = std::move(testLocationState);
 	authoritative.load(initialState);
 	FRG::WebsocketGameServer server(
 		authoritative, {"player", "player2"}, "lockstep-v2:test_lockstep",
@@ -94,19 +103,55 @@ int main()
 
 	client->submit(FRG::SetCharacterDestinationCommand{
 		{}, "spoofed", Eigen::Vector3f(1.234f, 0.f, 2.345f), "ignored"});
-	std::this_thread::sleep_for(std::chrono::milliseconds(20));
+	secondClient->submit(FRG::SetCharacterDestinationCommand{
+		{}, "spoofed", Eigen::Vector3f(-1.234f, 0.f, 3.456f), "ignored"});
+	std::this_thread::sleep_for(std::chrono::milliseconds(50));
 	server.update();
 	server.update();
-	require(
-		waitFor([&]() {
+	server.update();
+	const bool destinationsConverged = waitFor([&]() {
 			client->update(0);
-			return client->snapshot()->revision > 1;
-		}),
-		"client did not consume finalized lockstep ticks");
+			secondClient->update(0);
+			const auto first = client->snapshot();
+			const auto second = secondClient->snapshot();
+			const auto host = authoritative.snapshot();
+			return first->players.at("player").state.requestedWalkTarget.isApprox(
+					Eigen::Vector3f(1.23f, 0.f, 2.35f)) &&
+				second->players.at("player2").state.requestedWalkTarget.isApprox(
+					Eigen::Vector3f(-1.23f, 0.f, 3.46f)) &&
+				first->players.at("player2").state.requestedWalkTarget.isApprox(
+					second->players.at("player2").state.requestedWalkTarget) &&
+				second->players.at("player").state.requestedWalkTarget.isApprox(
+					first->players.at("player").state.requestedWalkTarget) &&
+				host->players.at("player").state.requestedWalkTarget.isApprox(
+					first->players.at("player").state.requestedWalkTarget) &&
+				host->players.at("player2").state.requestedWalkTarget.isApprox(
+					second->players.at("player2").state.requestedWalkTarget);
+		});
+	if (!destinationsConverged) {
+		const auto first = client->snapshot();
+		const auto second = secondClient->snapshot();
+		const auto host = authoritative.snapshot();
+		std::cerr << "destination revisions " << first->revision << ", "
+				  << second->revision << ", " << host->revision << "\n"
+				  << "player targets "
+				  << first->players.at("player").state.requestedWalkTarget.transpose()
+				  << " / "
+				  << host->players.at("player").state.requestedWalkTarget.transpose()
+				  << "\nplayer2 targets "
+				  << second->players.at("player2").state.requestedWalkTarget.transpose()
+				  << " / "
+				  << host->players.at("player2").state.requestedWalkTarget.transpose()
+				  << std::endl;
+	}
+	require(
+		destinationsConverged,
+		"simultaneous destinations did not converge across lockstep replicas");
 
 	client->submit(FRG::SetCharacterTargetCommand{
 		"stale_location", "spoofed_character", "hostile_npc"});
-	std::this_thread::sleep_for(std::chrono::milliseconds(20));
+	std::this_thread::sleep_for(std::chrono::milliseconds(50));
+	server.update();
 	server.update();
 	server.update();
 	require(
@@ -124,7 +169,8 @@ int main()
 
 	client->submit(FRG::ClearCharacterAttackTargetCommand{
 		"stale_location", "spoofed_character"});
-	std::this_thread::sleep_for(std::chrono::milliseconds(20));
+	std::this_thread::sleep_for(std::chrono::milliseconds(50));
+	server.update();
 	server.update();
 	server.update();
 	require(
@@ -139,7 +185,8 @@ int main()
 
 	client->submit(FRG::TryCharacterAttackComboCommand{
 		"stale_location", "spoofed_character"});
-	std::this_thread::sleep_for(std::chrono::milliseconds(20));
+	std::this_thread::sleep_for(std::chrono::milliseconds(50));
+	server.update();
 	server.update();
 	server.update();
 	require(
@@ -147,9 +194,7 @@ int main()
 			client->update(0);
 			secondClient->update(0);
 			return client->snapshot()->revision ==
-					secondClient->snapshot()->revision &&
-				client->snapshot()->revision ==
-					authoritative.snapshot()->revision;
+				secondClient->snapshot()->revision;
 		}),
 		"attack combo command was not rebroadcast in lockstep");
 

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,147 +118,44 @@ 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 {};
+    const std::lock_guard<std::mutex> lock(*queryMutex);
+    return runSearch(start, end, buildDynamicOverlay(dynamicObstacles), false);
 }
 
-    std::vector<unsigned char> walkableGrid = walkable;
-    if (!dynamicObstacles.empty()) {
+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) {
-            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)));
-
+        // 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) {
@@ -275,249 +163,242 @@ std::vector<Eigen::Vector3f> PathFinder::findPath(const Eigen::Vector3f& start,
                 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;
-                        }
-                    }
+                if (dx * dx + dz * dz <= radiusSq)
+                    dynamicBlockedGeneration[static_cast<size_t>(indexOf(cell))] =
+                        dynamicGeneration;
             }
         }
     }
+    return dynamicGeneration;
 }
 
-    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
+bool PathFinder::isCellWalkableOverlay(
+    const Cell& cell, std::uint32_t generation) 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;
+    if (!isCellWalkable(cell)) return false;
+    return generation == 0 ||
+        dynamicBlockedGeneration[static_cast<size_t>(indexOf(cell))] != generation;
 }
 
-        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;
+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;
 }
 
-            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;
+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;
+        }
     }
 }
 
-            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;
+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;
 }
 
-            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;
-}
-
-std::vector<Eigen::Vector3f> PathFinder::findNearestReachableImpl(
+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 });
-
+    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;
-    float bestDist  = distanceCells(startCell, endCell);
-
+    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 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,
+    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,
-	                                                      const std::vector<unsigned char>& walkableGrid) const;
+        std::uint32_t obstacleGeneration,
+        bool nearestReachable) const;
 };
 
 } // namespace FRG