diff --git a/GAME_STATE_ARCHITECTURE.md b/GAME_STATE_ARCHITECTURE.md index 17596c2..d892546 100644 --- a/GAME_STATE_ARCHITECTURE.md +++ b/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 diff --git a/PATHFINDING.md b/PATHFINDING.md index 0262e88..8005396 100644 --- a/PATHFINDING.md +++ b/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. diff --git a/WEBSOCKET_NETWORKING.md b/WEBSOCKET_NETWORKING.md index e9e1f46..d0edfa9 100644 --- a/WEBSOCKET_NETWORKING.md +++ b/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. diff --git a/resources/config/campaigns.json b/resources/config/campaigns.json index b5bb9f5..335b4dd 100644 --- a/resources/config/campaigns.json +++ b/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" } ] diff --git a/resources/config/locations/battle_zone_uni_exterior.json b/resources/config/locations/battle_zone_uni_exterior.json index b10b1a5..24120cf 100644 --- a/resources/config/locations/battle_zone_uni_exterior.json +++ b/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", diff --git a/resources/config/npcs_battle_zone.json b/resources/config/npcs_battle_zone.json index eb3fc4e..8d065f2 100644 --- a/resources/config/npcs_battle_zone.json +++ b/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 } ] } diff --git a/resources/config/start_state_battle_zone.json b/resources/config/start_state_battle_zone.json index 8732198..0f4e46a 100644 --- a/resources/config/start_state_battle_zone.json +++ b/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": [ - 0.0, - 0.0, - 0.0 + -6.4, + 0.0, + 13.0 ], "requestedWalkTarget": [ - 0.0, - 0.0, - 0.0 + -6.4, + 0.0, + 13.0 ], "showWeapon": true, "targetFacingAngle": 0.0, "visibleRange": 6.0, "walkTarget": [ - 0.0, - 0.0, - 0.0 + -6.4, + 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, diff --git a/resources/config/start_state_battle_zone_singleplayer.json b/resources/config/start_state_battle_zone_singleplayer.json index deb7098..5f8abad 100644 --- a/resources/config/start_state_battle_zone_singleplayer.json +++ b/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": [ - 0.0, - 0.0, - 0.0 + -6.8, + 0.0, + 13.0 ], "requestedWalkTarget": [ - 0.0, - 0.0, - 0.0 + -6.8, + 0.0, + 13.0 ], "showWeapon": true, "targetFacingAngle": 0.0, "visibleRange": 6.0, "walkTarget": [ - 0.0, - 0.0, - 0.0 + -6.8, + 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, diff --git a/server/CMakeLists.txt b/server/CMakeLists.txt index 44eac06..9f9f29a 100644 --- a/server/CMakeLists.txt +++ b/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) diff --git a/server/tests/LocationGameStateTests.cpp b/server/tests/LocationGameStateTests.cpp index c1e6de1..4d09e05 100644 --- a/server/tests/LocationGameStateTests.cpp +++ b/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 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(); + stationary->enabled = true; + stationary->hp = 10.f; + stationary->position = Eigen::Vector3f(4.f, 0.f, 0.f); + auto moving = std::make_shared(); + 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; } diff --git a/server/tests/PathFinderTests.cpp b/server/tests/PathFinderTests.cpp new file mode 100644 index 0000000..7555bfc --- /dev/null +++ b/server/tests/PathFinderTests.cpp @@ -0,0 +1,269 @@ +#include "CharacterState.h" +#include "LocationGameState.h" +#include "navigation/PathFinder.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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& lhs, + const std::vector& 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 obstacles; + for (int i = 0; i < queries; ++i) { + const float x = 4.5f + static_cast((i * 7) % 70); + const float z = 4.5f + static_cast((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(i % 30)); + const Eigen::Vector3f to( + 78.5f, 0.f, 78.5f - static_cast(i % 30)); + waypointCount += + pathFinder.findPathToNearest(from, to, obstacles).size(); + } + const auto elapsed = std::chrono::duration_cast( + 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 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 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(); + stationary->enabled = true; + stationary->hp = 10.f; + auto moving = std::make_shared(); + 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(); + 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(); + 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; +} diff --git a/server/tests/WebsocketLockstepTests.cpp b/server/tests/WebsocketLockstepTests.cpp index 0c667f1..ea271de 100644 --- a/server/tests/WebsocketLockstepTests.cpp +++ b/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(); + 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"); diff --git a/src/CharacterState.cpp b/src/CharacterState.cpp index 670c283..bd1642b 100644 --- a/src/CharacterState.cpp +++ b/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 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& 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{} - : 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& dynamicObstacles) { const Eigen::Vector3f normalizedTarget( requestedWalkTarget.x(), 0.f, requestedWalkTarget.z()); pathWaypoints = location.navigationMaps.empty() ? std::vector{} - : 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(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()); } diff --git a/src/CharacterState.h b/src/CharacterState.h index dbd049b..ddb9950 100644 --- a/src/CharacterState.h +++ b/src/CharacterState.h @@ -9,6 +9,7 @@ #include #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 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 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& + dynamicObstacles = {}); void forceReplan(const LocationNetworkState& location, - const std::string& entityId); + const std::string& entityId, + const std::vector& + 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* 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(); diff --git a/src/Game.cpp b/src/Game.cpp index f451f02..b61184d 100644 --- a/src/Game.cpp +++ b/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(); } diff --git a/src/GameState.h b/src/GameState.h index 17dc528..39bc901 100644 --- a/src/GameState.h +++ b/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). diff --git a/src/LocationGameState.cpp b/src/LocationGameState.cpp index 4d8ae5d..2a2d742 100644 --- a/src/LocationGameState.cpp +++ b/src/LocationGameState.cpp @@ -126,6 +126,58 @@ const CharacterState* LocationGameState::findCharacter( return nullptr; } +std::vector LocationGameState::collectPathObstacles( + const std::string& requesterId, + const PlayerStateMap& players, + const std::string& forcedObstacleId) const +{ + std::vector> characters; + characters.reserve(players.size() + npcs.size()); + for (const auto& [id, character] : players) + if (character) characters.emplace_back(id, character.get()); + for (const auto& [id, character] : npcs) + if (character) characters.emplace_back(id, character.get()); + std::sort(characters.begin(), characters.end(), + [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; }); + + std::vector 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 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)}); diff --git a/src/LocationGameState.h b/src/LocationGameState.h index 3a10a4d..bd35235 100644 --- a/src/LocationGameState.h +++ b/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 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( diff --git a/src/LocationState.cpp b/src/LocationState.cpp index a337f34..7be1b5b 100644 --- a/src/LocationState.cpp +++ b/src/LocationState.cpp @@ -122,9 +122,19 @@ void loadSinglePlayerPersistence( } - std::vector 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 LocationNetworkState::planPathExceptMyself( + const Eigen::Vector3f& start, + const Eigen::Vector3f& end, + const std::string& entityId, + const std::vector& dynamicObstacles) const { - return navigationMaps[activeNavigationIndex].findPathToNearest(start, end, {}); + (void)entityId; + if (activeNavigationIndex < 0 || + activeNavigationIndex >= static_cast(navigationMaps.size())) + return {}; + return navigationMaps[activeNavigationIndex].findPathToNearest( + start, end, dynamicObstacles); } void LocationNetworkState::initNavigation( diff --git a/src/LocationState.h b/src/LocationState.h index a5e573b..c683299 100644 --- a/src/LocationState.h +++ b/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 navigationMaps; - std::vector planPathExceptMyself(const Eigen::Vector3f& start, const Eigen::Vector3f& end, const std::string& entityId) const; + std::vector planPathExceptMyself( + const Eigen::Vector3f& start, + const Eigen::Vector3f& end, + const std::string& entityId, + const std::vector& dynamicObstacles = {}) const; void initNavigation( const std::vector& paths, const std::string& zipFile = std::string()); diff --git a/src/content/LocationDefinition.cpp b/src/content/LocationDefinition.cpp index 677e9ee..ac01e5c 100644 --- a/src/content/LocationDefinition.cpp +++ b/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(); + if (std::string(cooldownKey) == + "navigationRetryCooldownMs") { + ticks = (std::max)(1, (ticks + 99) / 100); + } + data.navigationRetryCooldownTicks.push_back(ticks); + } + } result.npcs.push_back(std::move(data)); } } diff --git a/src/content/LocationDefinition.h b/src/content/LocationDefinition.h index d8ebc87..dc76f8f 100644 --- a/src/content/LocationDefinition.h +++ b/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 navigationRetryCooldownTicks; CharacterState toCharacterState() const; }; diff --git a/src/game_server/InternalGameServer.cpp b/src/game_server/InternalGameServer.cpp index d4c5063..c1bab91 100644 --- a/src/game_server/InternalGameServer.cpp +++ b/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) { auto location = state_.locations.find(value.locationId); @@ -336,6 +336,10 @@ void InternalGameServer::submit(const GameCommand& command) index < static_cast(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) { @@ -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) { @@ -424,14 +432,10 @@ void InternalGameServer::submit(const GameCommand& command) if (character) character->attackTargetId.clear(); } else if constexpr (std::is_same_v) { 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) { 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) { 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) { 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(); } diff --git a/src/navigation/PathFinder.cpp b/src/navigation/PathFinder.cpp index 4963826..b4f38f5 100644 --- a/src/navigation/PathFinder.cpp +++ b/src/navigation/PathFinder.cpp @@ -14,15 +14,6 @@ namespace FRG { using json = nlohmann::json; namespace { - const float INF_COST = (std::numeric_limits::max)(); - - float distanceCells(const PathFinder::Cell& a, const PathFinder::Cell& b) - { - const float dx = static_cast(a.x - b.x); - const float dz = static_cast(a.z - b.z); - return std::sqrt(dx * dx + dz * dz); - } - std::vector readPolygon(const json& item) { std::vector polygon; @@ -127,397 +118,287 @@ void PathFinder::build(const std::string& configPath, std::vector 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 cost(static_cast(cellCount), INF_COST); - std::vector cameFrom(static_cast(cellCount), -1); - std::priority_queue open; - - const int startIndex = indexOf(startCell); - const int endIndex = indexOf(endCell); - - cost[static_cast(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(current.index)] + stepCost; - if (newCost >= cost[static_cast(nextIndex)]) { - continue; - } - - cost[static_cast(nextIndex)] = newCost; - cameFrom[static_cast(nextIndex)] = current.index; - const float priority = newCost + distanceCells(next, endCell); - open.push({ nextIndex, priority }); - } - } - - if (cameFrom[static_cast(endIndex)] == -1) { - return {}; - } - - std::vector cells; - for (int current = endIndex; current != -1; current = cameFrom[static_cast(current)]) { - cells.push_back({ current % gridWidth, current / gridWidth }); - if (current == startIndex) { - break; - } - } - std::reverse(cells.begin(), cells.end()); - cells = smoothCells(cells, walkable); - - std::vector 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 lock(*queryMutex); + return runSearch(start, end, 0, false); } std::vector PathFinder::findPath(const Eigen::Vector3f& start, const Eigen::Vector3f& end, const std::vector& dynamicObstacles) const { - if (!ready || walkable.empty()) { - return {}; - } - - std::vector walkableGrid = walkable; - if (!dynamicObstacles.empty()) { - for (const DynamicObstacle& obstacle : dynamicObstacles) { - const float radius = obstacle.radius + agentRadius; - if (radius <= 0.0f) continue; - - const float minWorldX = obstacle.position.x() - radius; - const float maxWorldX = obstacle.position.x() + radius; - const float minWorldZ = obstacle.position.z() - radius; - const float maxWorldZ = obstacle.position.z() + radius; - - const int minCellX = (std::max)(0, static_cast(std::floor((minWorldX - minX) / cellSize))); - const int maxCellX = (std::min)(gridWidth - 1, static_cast(std::floor((maxWorldX - minX) / cellSize))); - const int minCellZ = (std::max)(0, static_cast(std::floor((minWorldZ - minZ) / cellSize))); - const int maxCellZ = (std::min)(gridDepth - 1, static_cast(std::floor((maxWorldZ - minZ) / cellSize))); - - const float radiusSq = radius * radius; - for (int z = minCellZ; z <= maxCellZ; ++z) { - for (int x = minCellX; x <= maxCellX; ++x) { - const Cell cell{ x, z }; - const Eigen::Vector3f center = cellCenter(cell); - const float dx = center.x() - obstacle.position.x(); - const float dz = center.z() - obstacle.position.z(); - if (dx * dx + dz * dz <= radiusSq) { - const int idx = indexOf(cell); - if (idx >= 0 && idx < static_cast(walkableGrid.size())) { - walkableGrid[static_cast(idx)] = 0; - } - } - } - } - } - } - - Cell startCell; - Cell endCell; - if (!findNearestWalkableCell(start, startCell, walkableGrid) || - !findNearestWalkableCell(end, endCell, walkableGrid)) { - return {}; - } - - if (startCell.x == endCell.x && startCell.z == endCell.z) { - return { cellCenter(endCell) }; - } - - struct QueueNode { - int index = 0; - float priority = 0.0f; - - bool operator<(const QueueNode& other) const - { - if (priority != other.priority) return priority > other.priority; - return index > other.index; - } - }; - - const int cellCount = gridWidth * gridDepth; - std::vector cost(static_cast(cellCount), INF_COST); - std::vector cameFrom(static_cast(cellCount), -1); - std::priority_queue open; - - const int startIndex = indexOf(startCell); - const int endIndex = indexOf(endCell); - - cost[static_cast(startIndex)] = 0.0f; - open.push({ startIndex, 0.0f }); - - static const int offsets[8][2] = { - { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 }, - { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 } - }; - - while (!open.empty()) { - const QueueNode current = open.top(); - open.pop(); - - if (current.index == endIndex) { - break; - } - - const Cell currentCell{ current.index % gridWidth, current.index / gridWidth }; - - for (const auto& offset : offsets) { - Cell next{ currentCell.x + offset[0], currentCell.z + offset[1] }; - if (!isCellWalkable(next, walkableGrid)) { - continue; - } - - const bool diagonal = offset[0] != 0 && offset[1] != 0; - if (diagonal) { - Cell horizontal{ currentCell.x + offset[0], currentCell.z }; - Cell vertical{ currentCell.x, currentCell.z + offset[1] }; - if (!isCellWalkable(horizontal, walkableGrid) || !isCellWalkable(vertical, walkableGrid)) { - continue; - } - } - - const int nextIndex = indexOf(next); - const float stepCost = diagonal ? 1.41421356f : 1.0f; - const float newCost = cost[static_cast(current.index)] + stepCost; - if (newCost >= cost[static_cast(nextIndex)]) { - continue; - } - - cost[static_cast(nextIndex)] = newCost; - cameFrom[static_cast(nextIndex)] = current.index; - const float priority = newCost + distanceCells(next, endCell); - open.push({ nextIndex, priority }); - } - } - - if (cameFrom[static_cast(endIndex)] == -1) { - return {}; - } - - std::vector cells; - for (int current = endIndex; current != -1; current = cameFrom[static_cast(current)]) { - cells.push_back({ current % gridWidth, current / gridWidth }); - if (current == startIndex) { - break; - } - } - std::reverse(cells.begin(), cells.end()); - cells = smoothCells(cells, walkableGrid); - - std::vector 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 lock(*queryMutex); + return runSearch(start, end, buildDynamicOverlay(dynamicObstacles), false); } -std::vector PathFinder::findNearestReachableImpl( +std::uint32_t PathFinder::buildDynamicOverlay( + const std::vector& dynamicObstacles) const +{ + const size_t cellCount = walkable.size(); + if (dynamicBlockedGeneration.size() != cellCount) { + dynamicBlockedGeneration.assign(cellCount, 0); + dynamicGeneration = 0; + } + if (++dynamicGeneration == 0) { + std::fill(dynamicBlockedGeneration.begin(), dynamicBlockedGeneration.end(), 0); + dynamicGeneration = 1; + } + for (const DynamicObstacle& obstacle : dynamicObstacles) { + // Include the cell half-diagonal so a segment between adjacent + // unblocked cell centres cannot geometrically cut through the circle. + const float radius = + obstacle.radius + agentRadius + cellSize * 0.70710678f; + if (radius <= 0.f) continue; + const int minCellX = (std::max)( + 0, static_cast(std::floor((obstacle.position.x() - radius - minX) / cellSize))); + const int maxCellX = (std::min)( + gridWidth - 1, static_cast(std::floor((obstacle.position.x() + radius - minX) / cellSize))); + const int minCellZ = (std::max)( + 0, static_cast(std::floor((obstacle.position.z() - radius - minZ) / cellSize))); + const int maxCellZ = (std::min)( + gridDepth - 1, static_cast(std::floor((obstacle.position.z() + radius - minZ) / cellSize))); + const float radiusSq = radius * radius; + for (int z = minCellZ; z <= maxCellZ; ++z) { + for (int x = minCellX; x <= maxCellX; ++x) { + const Cell cell{x, z}; + const Eigen::Vector3f center = cellCenter(cell); + const float dx = center.x() - obstacle.position.x(); + const float dz = center.z() - obstacle.position.z(); + if (dx * dx + dz * dz <= radiusSq) + dynamicBlockedGeneration[static_cast(indexOf(cell))] = + dynamicGeneration; + } + } + } + return dynamicGeneration; +} + +bool PathFinder::isCellWalkableOverlay( + const Cell& cell, std::uint32_t generation) const +{ + if (!isCellWalkable(cell)) return false; + return generation == 0 || + dynamicBlockedGeneration[static_cast(indexOf(cell))] != generation; +} + +bool PathFinder::findNearestWalkableCellOverlay( + const Eigen::Vector3f& point, Cell& out, std::uint32_t generation) const +{ + Cell origin; + if (!worldToCell(point, origin)) { + origin.x = (std::max)(0, (std::min)( + gridWidth - 1, static_cast(std::floor((point.x() - minX) / cellSize)))); + origin.z = (std::max)(0, (std::min)( + gridDepth - 1, static_cast(std::floor((point.z() - minZ) / cellSize)))); + } + if (isCellWalkableOverlay(origin, generation)) { + out = origin; + return true; + } + const int maxRadius = + (std::max)(4, static_cast(std::ceil(8.0f / cellSize))); + for (int radius = 1; radius <= maxRadius; ++radius) { + for (int z = origin.z - radius; z <= origin.z + radius; ++z) { + for (int x = origin.x - radius; x <= origin.x + radius; ++x) { + if (std::abs(x - origin.x) != radius && + std::abs(z - origin.z) != radius) continue; + const Cell candidate{x, z}; + if (isCellWalkableOverlay(candidate, generation)) { + out = candidate; + return true; + } + } + } + } + return false; +} + +bool PathFinder::hasLineOfSightOverlay( + const Cell& from, const Cell& to, std::uint32_t generation) const +{ + int x = from.x; + int z = from.z; + const int dx = std::abs(to.x - from.x); + const int dz = std::abs(to.z - from.z); + const int sx = from.x < to.x ? 1 : -1; + const int sz = from.z < to.z ? 1 : -1; + int error = dx - dz; + while (true) { + if (!isCellWalkableOverlay({x, z}, generation)) return false; + if (x == to.x && z == to.z) return true; + const int twiceError = error * 2; + const int oldX = x; + const int oldZ = z; + if (twiceError > -dz) { + error -= dz; + x += sx; + } + if (twiceError < dx) { + error += dx; + z += sz; + } + if (x != oldX && z != oldZ && + (!isCellWalkableOverlay({x, oldZ}, generation) || + !isCellWalkableOverlay({oldX, z}, generation))) { + return false; + } + } +} + +std::vector PathFinder::smoothCellsOverlay( + const std::vector& cells, std::uint32_t generation) const +{ + if (cells.size() <= 2) return cells; + std::vector result; + result.push_back(cells.front()); + size_t anchor = 0; + while (anchor + 1 < cells.size()) { + size_t next = cells.size() - 1; + while (next > anchor + 1 && + !hasLineOfSightOverlay(cells[anchor], cells[next], generation)) { + --next; + } + result.push_back(cells[next]); + anchor = next; + } + return result; +} + +std::vector PathFinder::runSearch( const Eigen::Vector3f& start, const Eigen::Vector3f& end, - const std::vector& 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 cost(static_cast(cellCount), INF_COST); - std::vector cameFrom(static_cast(cellCount), -1); std::priority_queue open; - const int startIndex = indexOf(startCell); - const int endIndex = indexOf(endCell); - - cost[static_cast(startIndex)] = 0.f; - open.push({ startIndex, 0.f }); - - int bestIndex = startIndex; - float bestDist = distanceCells(startCell, endCell); - + const int endIndex = indexOf(endCell); + searchVisitedGeneration[static_cast(startIndex)] = searchGeneration; + searchCost[static_cast(startIndex)] = 0; + searchCameFrom[static_cast(startIndex)] = -1; + open.push({startIndex, heuristic(startCell, endCell), 0}); + int bestIndex = startIndex; + int bestDistanceSq = + (startCell.x - endCell.x) * (startCell.x - endCell.x) + + (startCell.z - endCell.z) * (startCell.z - endCell.z); static const int offsets[8][2] = { { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 }, { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 } }; - while (!open.empty()) { const QueueNode current = open.top(); open.pop(); - + const size_t currentOffset = static_cast(current.index); + if (searchVisitedGeneration[currentOffset] != searchGeneration || + searchCost[currentOffset] != current.cost) continue; if (current.index == endIndex) { bestIndex = endIndex; break; } - - if (cost[static_cast(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(current.index)] + stepCost; - if (newCost >= cost[static_cast(nextIndex)]) continue; - - cost[static_cast(nextIndex)] = newCost; - cameFrom[static_cast(nextIndex)] = current.index; - open.push({ nextIndex, newCost + distanceCells(next, endCell) }); + const int nextIndex = indexOf(next); + const size_t nextOffset = static_cast(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 cells; - for (int cur = bestIndex; cur != -1; cur = cameFrom[static_cast(cur)]) { + for (int cur = bestIndex; cur != -1; + cur = searchCameFrom[static_cast(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 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 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 lock(*queryMutex); + return runSearch(start, end, 0, true); } std::vector PathFinder::findPathToNearest( const Eigen::Vector3f& start, const Eigen::Vector3f& end, const std::vector& 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 lock(*queryMutex); + return runSearch( + start, end, buildDynamicOverlay(dynamicObstacles), true); } bool PathFinder::setAreaAvailable(const std::string& areaName, bool available) diff --git a/src/navigation/PathFinder.h b/src/navigation/PathFinder.h index f428213..7b47473 100644 --- a/src/navigation/PathFinder.h +++ b/src/navigation/PathFinder.h @@ -1,6 +1,9 @@ #pragma once #include +#include +#include +#include #include #include @@ -79,6 +82,17 @@ private: std::vector walkable; std::vector areas; + // Reused by synchronous authoritative path queries. Generation stamps avoid + // copying/clearing the complete map for every dynamic-obstacle search. + mutable std::vector dynamicBlockedGeneration; + mutable std::uint32_t dynamicGeneration = 0; + mutable std::vector searchCost; + mutable std::vector searchCameFrom; + mutable std::vector searchVisitedGeneration; + mutable std::uint32_t searchGeneration = 0; + mutable std::shared_ptr queryMutex = + std::make_shared(); + 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& walkableGrid) const; std::vector smoothCells(const std::vector& cells, const std::vector& 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 findNearestReachableImpl(const Eigen::Vector3f& start, - const Eigen::Vector3f& end, - const std::vector& walkableGrid) const; + std::uint32_t buildDynamicOverlay( + const std::vector& 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 smoothCellsOverlay( + const std::vector& cells, std::uint32_t generation) const; + std::vector runSearch( + const Eigen::Vector3f& start, + const Eigen::Vector3f& end, + std::uint32_t obstacleGeneration, + bool nearestReachable) const; }; } // namespace FRG