# 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 |