261 lines
14 KiB
Markdown
261 lines
14 KiB
Markdown
# Pathfinding System
|
||
|
||
This document describes the grid-based pathfinding used for the player and all NPCs, including the collision avoidance and movement quality improvements.
|
||
|
||
---
|
||
|
||
## Table of Contents
|
||
|
||
1. [Grid Representation](#1-grid-representation)
|
||
2. [Building the Walkable Grid](#2-building-the-walkable-grid)
|
||
3. [A\* Path Search](#3-a-path-search)
|
||
4. [Path Smoothing](#4-path-smoothing)
|
||
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)
|
||
9. [Dynamic Replanning](#9-dynamic-replanning)
|
||
10. [Key Constants Reference](#10-key-constants-reference)
|
||
|
||
---
|
||
|
||
## 1. Grid Representation
|
||
|
||
The world is divided into a uniform 2D grid in the XZ plane (Y is ignored during pathfinding; all characters walk on a flat floor at `floorY`).
|
||
|
||
Each cell is either **walkable** (`1`) or **blocked** (`0`). The grid is stored as a flat `std::vector<unsigned char>` indexed by `z * gridWidth + x`.
|
||
|
||
**Parameters** (all configurable in the JSON config file):
|
||
|
||
| Parameter | Default | Description |
|
||
|---|---|---|
|
||
| `cellSize` | 0.4 m | Width and depth of one cell |
|
||
| `agentRadius` | 0.45 m | Half-width of a character — used to erode free space |
|
||
| `objectPadding` | 0.25 m | Extra clearance added around obstacle polygons |
|
||
| `boundaryPadding` | 0.0 m | Inward erosion from the edges of navigation areas |
|
||
| `floorY` | 0.0 | Y coordinate placed on every path waypoint |
|
||
|
||
**Grid bounds** are computed from the union of all navigation area polygons plus a padding margin of `cellSize * 2 + agentRadius + objectPadding` on every side.
|
||
|
||
**Cell coordinate conversion:**
|
||
|
||
```
|
||
cell.x = floor((worldX - minX) / cellSize)
|
||
cell.z = floor((worldZ - minZ) / cellSize)
|
||
|
||
cellCenter.x = minX + (cell.x + 0.5) * cellSize
|
||
cellCenter.z = minZ + (cell.z + 0.5) * cellSize
|
||
```
|
||
|
||
---
|
||
|
||
## 2. Building the Walkable Grid
|
||
|
||
The grid can be loaded in two ways.
|
||
|
||
### 2a. Pre-computed grid (`.txt` file)
|
||
|
||
A plain-text file with a small header followed by rows of `1`/`0` characters:
|
||
|
||
```
|
||
cellSize 0.4
|
||
agentRadius 0.45
|
||
floorY 0.0
|
||
...
|
||
minX -5.0
|
||
minZ -5.0
|
||
gridWidth 50
|
||
gridDepth 50
|
||
11111111...
|
||
10000001...
|
||
```
|
||
|
||
This format is generated by `PathFinder::saveGrid()` after building from polygons and can be loaded much faster than recomputing from geometry.
|
||
|
||
### 2b. Polygon-based config (`.json` file)
|
||
|
||
The JSON file lists **navigation areas** (convex or concave walkable regions) and **obstacle polygons** (impassable zones within those regions):
|
||
|
||
```json
|
||
{
|
||
"cellSize": 0.4,
|
||
"areas": [
|
||
{ "name": "main_room", "available": true, "polygon": [[x,z], ...] }
|
||
],
|
||
"obstacles": [
|
||
{ "name": "table", "polygon": [[x,z], ...] }
|
||
]
|
||
}
|
||
```
|
||
|
||
**Build steps:**
|
||
|
||
1. **Mark available areas walkable** — every cell whose center lies inside any `available` navigation area polygon gets `walkable = 1`. If `boundaryPadding > 0`, cells too close to the outer edge of the area are left blocked.
|
||
2. **Mark obstacle polygons blocked** — cells whose center lies inside an obstacle polygon, or within `agentRadius + objectPadding` of its edges, are set to `0`.
|
||
|
||
Navigation areas can be toggled at runtime via `PathFinder::setAreaAvailable()`, which rebuilds the entire grid. This is used to open or close doors, gated areas, etc.
|
||
|
||
---
|
||
|
||
## 3. A\* Path Search
|
||
|
||
`PathFinder::findPath(start, end)` runs a standard A\* on the walkable grid.
|
||
|
||
**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.
|
||
|
||
**Heuristic:** Euclidean distance in cell units to the end cell.
|
||
|
||
**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.
|
||
|
||
**Path reconstruction:** After A\* completes, the cell chain is walked via `cameFrom[]` from `end` back to `start`, reversed, then smoothed (see §4).
|
||
|
||
**First-waypoint trimming:** If the first waypoint is within `cellSize × 0.75` of `start`, it is dropped (the character is already close enough).
|
||
|
||
**Last-waypoint precision:** If the requested `end` maps to the same cell as the snapped end cell, the last waypoint is replaced with the exact `end` world position rather than the cell centre.
|
||
|
||
---
|
||
|
||
## 4. Path Smoothing
|
||
|
||
Raw A\* paths follow the grid diagonals and produce staircase-shaped routes. A **string-pulling** (line-of-sight) pass compresses them:
|
||
|
||
```
|
||
anchor = path[0]
|
||
result = [anchor]
|
||
while anchor is not the last cell:
|
||
find the furthest cell 'next' from anchor with unobstructed line of sight
|
||
result.append(next)
|
||
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.
|
||
|
||
---
|
||
|
||
## 5. Approaching Unreachable Destinations
|
||
|
||
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:
|
||
|
||
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.
|
||
- 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.
|
||
|
||
---
|
||
|
||
## 6. Dynamic Obstacles
|
||
|
||
When a path is planned, other characters can temporarily mark cells as blocked to make the character walk around them rather than through them.
|
||
|
||
**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.
|
||
|
||
**Which characters become obstacles:**
|
||
|
||
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).
|
||
|
||
**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.
|
||
|
||
---
|
||
|
||
## 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`.
|
||
|
||
Each frame in `Character::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.
|
||
|
||
**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.
|
||
|
||
---
|
||
|
||
## 8. Character Collision Resolution
|
||
|
||
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`.
|
||
|
||
**Algorithm** (3 iterations per frame):
|
||
|
||
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.
|
||
|
||
---
|
||
|
||
## 9. Dynamic Replanning
|
||
|
||
When characters move they can displace each other or enter each other's planned paths. `Location::updateDynamicReplans` handles this:
|
||
|
||
**Every frame:**
|
||
|
||
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.
|
||
|
||
**`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.
|
||
|
||
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.
|
||
|
||
---
|
||
|
||
## 10. Key Constants Reference
|
||
|
||
| Constant | Location | Value | Description |
|
||
|---|---|---|---|
|
||
| `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 |
|
||
| `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 |
|