fix(issue-5): add collision-aware pathfinding to Phaser office viewer (#24)

* fix(issue-5): add collision-aware pathfinding to Phaser office viewer

Agents in the Phaser office viewer previously moved in straight lines toward
zone anchors, ignoring walls, furniture, and collision geometry. This made
agents walk through rendered map obstacles.

Changes:

src/lib/office/pathfinding.ts (new):
  - Shared 2D A* pathfinding module for OfficeMap surfaces
  - Builds a nav grid from map objects (walls/furniture layers) and
    collision polygons with configurable cell size and padding
  - Diagonal corner-cutting prevention (checks both orthogonal neighbors)
  - Returns empty path on failure instead of raw destination fallback
  - Point-in-polygon rasterisation for collision polygon support
  - Intentionally placed in src/lib/office/ for reuse across office stacks

src/features/office/phaser/systems/AgentEffectsSystem.ts:
  - Computes A* waypoint paths when agent targets change
  - Follows waypoints sequentially instead of linear interpolation
  - Caches nav grid and invalidates on map identity change
  - Agents stay put when no valid path exists (no wall clipping)

tests/unit/officePathfinding.test.ts (new):
  - 12 unit tests covering grid construction, A* routing, corner-cutting
    prevention, collision polygon support, blocked-start recovery, and
    starter map integration

Fixes #5

* fix(test): remove unused NavGrid2D type import

---------

Co-authored-by: Neo <neo@openclaw.ai>

src/features/office/phaser/systems/AgentEffectsSystem.ts

@@ -2,6 +2,12 @@ import type Phaser from "phaser";
 
 import type { OfficeAgentPresence } from "@/lib/office/presence";
 import type { OfficeMap } from "@/lib/office/schema";
+import {
+  astar2D,
+  buildNavGrid2D,
+  type NavGrid2D,
+  type Waypoint,
+} from "@/lib/office/pathfinding";
 
 type AgentEffectsSystemParams = {
   scene: Phaser.Scene;
@@ -15,6 +21,12 @@ type AvatarState = {
   vx: number;
   vy: number;
   lastThoughtAt: number;
+  /** Current waypoint path the agent is following. */
+  path: Waypoint[];
+  /** Index of the next waypoint in `path` the agent is walking toward. */
+  pathIndex: number;
+  /** Stringified last-resolved target so we know when to re-path. */
+  lastTargetKey: string;
 };
 
 const THOUGHTS = ["coffee", "gamepad", "zzz", "idea", "music"] as const;
@@ -40,6 +52,13 @@ export class AgentEffectsSystem {
   private readonly scene: Phaser.Scene;
   private readonly avatars = new Map<string, AvatarState>();
 
+  /**
+   * Cached nav grid.  Rebuilt when the map identity changes so agents always
+   * pathfind against the current layout.
+   */
+  private navGrid: NavGrid2D | null = null;
+  private navGridMapVersion: string = "";
+
   constructor(params: AgentEffectsSystemParams) {
     this.scene = params.scene;
   }
@@ -50,6 +69,19 @@ export class AgentEffectsSystem {
     elapsedMs: number;
     thoughtBubblesEnabled: boolean;
   }) {
+    // Rebuild the nav grid when the map changes.
+    const mapKey = `${params.map.workspaceId}:${params.map.officeVersionId}`;
+    if (mapKey !== this.navGridMapVersion) {
+      this.navGrid = buildNavGrid2D(params.map);
+      this.navGridMapVersion = mapKey;
+      // Invalidate all cached paths since the world changed.
+      for (const entry of this.avatars.values()) {
+        entry.path = [];
+        entry.pathIndex = 0;
+        entry.lastTargetKey = "";
+      }
+    }
+
     const keep = new Set<string>();
     const zonesByType = new Map(
       params.map.zones.map((zone) => [zone.type, zone])
@@ -62,16 +94,29 @@ export class AgentEffectsSystem {
       entry.stateIcon.setText(agent.state === "error" ? "!" : "");
 
       const target = this.resolveTarget(agent.state, zonesByType);
-      const dx = target.x - entry.sprite.x;
-      const dy = target.y - entry.sprite.y;
-      const distance = Math.hypot(dx, dy);
-      const maxSpeed = 0.05 * params.elapsedMs;
-      if (distance > 0.1) {
-        const step = Math.min(maxSpeed, distance);
-        entry.sprite.x += (dx / distance) * step;
-        entry.sprite.y += (dy / distance) * step;
+      const targetKey = `${target.x}:${target.y}`;
+
+      // Re-path when target changes.
+      if (targetKey !== entry.lastTargetKey) {
+        entry.lastTargetKey = targetKey;
+        if (this.navGrid) {
+          entry.path = astar2D(
+            entry.sprite.x,
+            entry.sprite.y,
+            target.x,
+            target.y,
+            this.navGrid,
+          );
+        } else {
+          // Fallback: no grid available, stay put.
+          entry.path = [];
+        }
+        entry.pathIndex = 0;
       }
 
+      // Follow waypoint path.
+      this.stepAlongPath(entry, params.elapsedMs);
+
       entry.label.setPosition(entry.sprite.x, entry.sprite.y + 15);
       entry.stateIcon.setPosition(entry.sprite.x + 12, entry.sprite.y - 12);
       entry.thoughtIcon.setPosition(entry.sprite.x, entry.sprite.y - 20);
@@ -114,6 +159,35 @@ export class AgentEffectsSystem {
       entry.thoughtIcon.destroy();
     }
     this.avatars.clear();
+    this.navGrid = null;
+  }
+
+  /**
+   * Walk the agent sprite along the computed waypoint path.
+   *
+   * When the path is empty (no route found) the agent simply stays put,
+   * which is the correct behavior: visible stillness is preferable to
+   * walking through walls.
+   */
+  private stepAlongPath(entry: AvatarState, elapsedMs: number): void {
+    if (entry.path.length === 0 || entry.pathIndex >= entry.path.length) return;
+
+    const wp = entry.path[entry.pathIndex];
+    const dx = wp.x - entry.sprite.x;
+    const dy = wp.y - entry.sprite.y;
+    const dist = Math.hypot(dx, dy);
+    const maxSpeed = 0.05 * elapsedMs;
+
+    if (dist <= maxSpeed) {
+      // Arrived at waypoint — snap and advance.
+      entry.sprite.x = wp.x;
+      entry.sprite.y = wp.y;
+      entry.pathIndex += 1;
+    } else {
+      const step = Math.min(maxSpeed, dist);
+      entry.sprite.x += (dx / dist) * step;
+      entry.sprite.y += (dy / dist) * step;
+    }
   }
 
   private getOrCreate(agentId: string, name: string, state: OfficeAgentPresence["state"]) {
@@ -156,6 +230,9 @@ export class AgentEffectsSystem {
       vx: 0,
       vy: 0,
       lastThoughtAt: this.scene.time.now,
+      path: [],
+      pathIndex: 0,
+      lastTargetKey: "",
     };
     this.avatars.set(agentId, created);
     return created;

src/lib/office/pathfinding.ts

@@ -0,0 +1,400 @@
+/**
+ * 2D grid-based A* pathfinding for OfficeMap.
+ *
+ * Builds a nav grid from map objects (walls, furniture) and collision polygons,
+ * then runs A* to produce collision-aware waypoint paths.
+ *
+ * Designed for the Phaser viewer but intentionally placed in `src/lib/office/`
+ * so any office surface can reuse the same pathfinder without duplicating logic.
+ */
+import type { OfficeCollision, OfficeMap, OfficeMapObject } from "@/lib/office/schema";
+
+// ---------------------------------------------------------------------------
+// Grid constants
+// ---------------------------------------------------------------------------
+
+const DEFAULT_CELL_SIZE = 8;
+const DEFAULT_PAD = 4;
+
+// Asset IDs that represent solid obstacles agents cannot walk through.
+// Mirrors the furniture-layer intent: desks, tables, walls, machines, etc.
+const BLOCKING_ASSET_IDS = new Set([
+  "desk_modern",
+  "meeting_table",
+  "wall_block",
+  "arcade_machine",
+  "coffee_station",
+  "tv_wall",
+]);
+
+// Tags that mark an object as a solid obstacle regardless of asset ID.
+const BLOCKING_TAGS = new Set(["wall", "desk", "table", "obstacle"]);
+
+// Layer IDs whose objects should be evaluated for blocking.
+const BLOCKING_LAYERS = new Set(["walls", "furniture"]);
+
+// ---------------------------------------------------------------------------
+// Types
+// ---------------------------------------------------------------------------
+
+export type NavGrid2D = {
+  cells: Uint8Array;
+  cols: number;
+  rows: number;
+  cellSize: number;
+};
+
+export type Waypoint = { x: number; y: number };
+
+// ---------------------------------------------------------------------------
+// Grid construction
+// ---------------------------------------------------------------------------
+
+/**
+ * Returns true when `obj` should block agent movement.
+ *
+ * The check is deliberately broad: if an object lives on a blocking layer
+ * _or_ carries a blocking tag _or_ matches a known solid asset, it blocks.
+ */
+const isBlocking = (obj: OfficeMapObject): boolean => {
+  if (BLOCKING_LAYERS.has(obj.layerId)) return true;
+  if (BLOCKING_ASSET_IDS.has(obj.assetId)) return true;
+  for (const tag of obj.tags) {
+    if (BLOCKING_TAGS.has(tag)) return true;
+  }
+  return false;
+};
+
+/**
+ * Resolve approximate width/height for an asset.
+ *
+ * OfficeMapObject does not carry explicit dimensions, so we use a best-effort
+ * lookup keyed on `assetId`.  Unknown assets get a conservative default.
+ */
+const ASSET_SIZE: Record<string, [number, number]> = {
+  desk_modern: [64, 32],
+  meeting_table: [160, 80],
+  wall_block: [32, 32],
+  arcade_machine: [32, 48],
+  coffee_station: [64, 32],
+  tv_wall: [80, 10],
+  floor_tile: [32, 32],
+  plant_potted: [32, 32],
+};
+
+const getAssetSize = (assetId: string): [number, number] =>
+  ASSET_SIZE[assetId] ?? [32, 32];
+
+/**
+ * Build a nav grid from an OfficeMap.
+ *
+ * Objects on blocking layers and explicit collision polygons are rasterised
+ * into the grid.  A small padding is added around each obstacle so agents
+ * do not clip through corners.
+ */
+export function buildNavGrid2D(
+  map: OfficeMap,
+  cellSize: number = DEFAULT_CELL_SIZE,
+  pad: number = DEFAULT_PAD,
+): NavGrid2D {
+  const cols = Math.ceil(map.canvas.width / cellSize);
+  const rows = Math.ceil(map.canvas.height / cellSize);
+  const cells = new Uint8Array(cols * rows);
+
+  // --- Mark blocking objects -----------------------------------------------
+  for (const obj of map.objects) {
+    if (!isBlocking(obj)) continue;
+    const [w, h] = getAssetSize(obj.assetId);
+
+    // Objects are positioned at their center in the Phaser scene, so convert
+    // to top-left for grid rasterisation.
+    const x1 = obj.x - w / 2 - pad;
+    const y1 = obj.y - h / 2 - pad;
+    const x2 = obj.x + w / 2 + pad;
+    const y2 = obj.y + h / 2 + pad;
+
+    const c1 = Math.max(0, Math.floor(x1 / cellSize));
+    const c2 = Math.min(cols - 1, Math.floor(x2 / cellSize));
+    const r1 = Math.max(0, Math.floor(y1 / cellSize));
+    const r2 = Math.min(rows - 1, Math.floor(y2 / cellSize));
+
+    for (let r = r1; r <= r2; r++) {
+      for (let c = c1; c <= c2; c++) {
+        cells[r * cols + c] = 1;
+      }
+    }
+  }
+
+  // --- Mark explicit collision polygons ------------------------------------
+  for (const collision of map.collisions) {
+    if (!collision.blocked) continue;
+    rasterisePolygon(collision, cells, cols, rows, cellSize, pad);
+  }
+
+  return { cells, cols, rows, cellSize };
+}
+
+/**
+ * Rasterise a convex/concave collision polygon into the grid.
+ *
+ * Uses a simple bounding-box + point-in-polygon approach.  For the small
+ * grids used in the Phaser office viewer this is fast enough and avoids
+ * pulling in a full polygon rasteriser dependency.
+ */
+function rasterisePolygon(
+  collision: OfficeCollision,
+  cells: Uint8Array,
+  cols: number,
+  rows: number,
+  cellSize: number,
+  pad: number,
+): void {
+  const points = collision.shape.points;
+  if (points.length < 3) return;
+
+  // Bounding box
+  let minX = Infinity;
+  let minY = Infinity;
+  let maxX = -Infinity;
+  let maxY = -Infinity;
+  for (const p of points) {
+    if (p.x < minX) minX = p.x;
+    if (p.y < minY) minY = p.y;
+    if (p.x > maxX) maxX = p.x;
+    if (p.y > maxY) maxY = p.y;
+  }
+  minX -= pad;
+  minY -= pad;
+  maxX += pad;
+  maxY += pad;
+
+  const c1 = Math.max(0, Math.floor(minX / cellSize));
+  const c2 = Math.min(cols - 1, Math.floor(maxX / cellSize));
+  const r1 = Math.max(0, Math.floor(minY / cellSize));
+  const r2 = Math.min(rows - 1, Math.floor(maxY / cellSize));
+
+  for (let r = r1; r <= r2; r++) {
+    for (let c = c1; c <= c2; c++) {
+      const px = c * cellSize + cellSize / 2;
+      const py = r * cellSize + cellSize / 2;
+      if (pointInPolygon(px, py, points)) {
+        cells[r * cols + c] = 1;
+      }
+    }
+  }
+}
+
+/**
+ * Ray-casting point-in-polygon test.
+ */
+function pointInPolygon(
+  px: number,
+  py: number,
+  vertices: { x: number; y: number }[],
+): boolean {
+  let inside = false;
+  for (let i = 0, j = vertices.length - 1; i < vertices.length; j = i++) {
+    const xi = vertices[i].x;
+    const yi = vertices[i].y;
+    const xj = vertices[j].x;
+    const yj = vertices[j].y;
+    if (yi > py !== yj > py && px < ((xj - xi) * (py - yi)) / (yj - yi) + xi) {
+      inside = !inside;
+    }
+  }
+  return inside;
+}
+
+// ---------------------------------------------------------------------------
+// A* pathfinder
+// ---------------------------------------------------------------------------
+
+/**
+ * Find a collision-aware path from (sx, sy) to (ex, ey) on the given grid.
+ *
+ * Returns an array of waypoints (excluding the start) the agent should walk
+ * through in order.  Returns an empty array when no valid path exists — the
+ * caller should treat this as "stay put" rather than falling back to direct
+ * movement (which is the bug this module exists to fix).
+ *
+ * Diagonal moves are only allowed when both adjacent orthogonal cells are
+ * clear (no corner-cutting).
+ */
+export function astar2D(
+  sx: number,
+  sy: number,
+  ex: number,
+  ey: number,
+  grid: NavGrid2D,
+): Waypoint[] {
+  const { cells, cols, rows, cellSize } = grid;
+
+  const toCell = (x: number, y: number) => ({
+    c: clamp(Math.floor(x / cellSize), 0, cols - 1),
+    r: clamp(Math.floor(y / cellSize), 0, rows - 1),
+  });
+  const cellCenter = (c: number, r: number): Waypoint => ({
+    x: c * cellSize + cellSize / 2,
+    y: r * cellSize + cellSize / 2,
+  });
+
+  let { c: sc, r: sr } = toCell(sx, sy);
+  let { c: ec, r: er } = toCell(ex, ey);
+
+  // If start or end is inside a blocked cell, find the nearest free cell.
+  const startFree = findFreeCell(sc, sr, cells, cols, rows);
+  const endFree = findFreeCell(ec, er, cells, cols, rows);
+  if (!startFree || !endFree) return [];
+  sc = startFree.c;
+  sr = startFree.r;
+  ec = endFree.c;
+  er = endFree.r;
+
+  if (sc === ec && sr === er) return [{ x: ex, y: ey }];
+
+  // A* with binary-heap open set
+  const nodeCount = cols * rows;
+  const gCost = new Float32Array(nodeCount).fill(Infinity);
+  const parent = new Int32Array(nodeCount).fill(-1);
+  const visited = new Uint8Array(nodeCount);
+  const startIdx = sr * cols + sc;
+  const endIdx = er * cols + ec;
+  gCost[startIdx] = 0;
+
+  const open: [number, number][] = [];
+  heapPush(open, [startIdx, heuristic(sc, sr, ec, er)]);
+
+  const DIRS: [number, number, number][] = [
+    [1, 0, 1],
+    [-1, 0, 1],
+    [0, 1, 1],
+    [0, -1, 1],
+    [1, 1, 1.414],
+    [1, -1, 1.414],
+    [-1, 1, 1.414],
+    [-1, -1, 1.414],
+  ];
+
+  while (open.length > 0) {
+    const entry = heapPop(open);
+    if (!entry) break;
+    const [current] = entry;
+    if (visited[current]) continue;
+    visited[current] = 1;
+
+    if (current === endIdx) {
+      // Reconstruct path
+      const path: Waypoint[] = [];
+      let node = current;
+      while (node !== startIdx) {
+        const c = node % cols;
+        const r = Math.floor(node / cols);
+        path.push(cellCenter(c, r));
+        node = parent[node];
+      }
+      path.reverse();
+      // Replace the last waypoint with the exact destination
+      if (path.length > 0) {
+        path[path.length - 1] = { x: ex, y: ey };
+      } else {
+        path.push({ x: ex, y: ey });
+      }
+      return path;
+    }
+
+    const cc = current % cols;
+    const cr = Math.floor(current / cols);
+
+    for (const [dc, dr, cost] of DIRS) {
+      const nc = cc + dc;
+      const nr = cr + dr;
+      if (nc < 0 || nc >= cols || nr < 0 || nr >= rows) continue;
+      const ni = nr * cols + nc;
+      if (visited[ni] || cells[ni]) continue;
+
+      // Prevent diagonal corner-cutting
+      if (dc !== 0 && dr !== 0) {
+        if (cells[cr * cols + (cc + dc)] || cells[(cr + dr) * cols + cc]) {
+          continue;
+        }
+      }
+
+      const ng = gCost[current] + cost;
+      if (ng < gCost[ni]) {
+        gCost[ni] = ng;
+        parent[ni] = current;
+        heapPush(open, [ni, ng + heuristic(nc, nr, ec, er)]);
+      }
+    }
+  }
+
+  // No path found — return empty (caller should not fall back to direct movement)
+  return [];
+}
+
+// ---------------------------------------------------------------------------
+// Helpers
+// ---------------------------------------------------------------------------
+
+function clamp(v: number, lo: number, hi: number): number {
+  return Math.min(hi, Math.max(lo, v));
+}
+
+function heuristic(c1: number, r1: number, c2: number, r2: number): number {
+  return Math.hypot(c2 - c1, r2 - r1);
+}
+
+function findFreeCell(
+  c: number,
+  r: number,
+  cells: Uint8Array,
+  cols: number,
+  rows: number,
+): { c: number; r: number } | null {
+  if (!cells[r * cols + c]) return { c, r };
+  for (let dist = 1; dist < 12; dist++) {
+    for (let dr = -dist; dr <= dist; dr++) {
+      for (let dc = -dist; dc <= dist; dc++) {
+        if (Math.abs(dr) !== dist && Math.abs(dc) !== dist) continue;
+        const nr = r + dr;
+        const nc = c + dc;
+        if (nr < 0 || nr >= rows || nc < 0 || nc >= cols) continue;
+        if (!cells[nr * cols + nc]) return { c: nc, r: nr };
+      }
+    }
+  }
+  return null;
+}
+
+// Min-heap helpers
+function heapPush(heap: [number, number][], entry: [number, number]): void {
+  heap.push(entry);
+  let i = heap.length - 1;
+  while (i > 0) {
+    const pi = Math.floor((i - 1) / 2);
+    if (heap[pi][1] <= entry[1]) break;
+    heap[i] = heap[pi];
+    i = pi;
+  }
+  heap[i] = entry;
+}
+
+function heapPop(heap: [number, number][]): [number, number] | null {
+  if (heap.length === 0) return null;
+  const first = heap[0];
+  const last = heap.pop();
+  if (!last || heap.length === 0) return first;
+  let i = 0;
+  while (true) {
+    const li = i * 2 + 1;
+    const ri = li + 1;
+    if (li >= heap.length) break;
+    let si = li;
+    if (ri < heap.length && heap[ri][1] < heap[li][1]) si = ri;
+    if (heap[si][1] >= last[1]) break;
+    heap[i] = heap[si];
+    i = si;
+  }
+  heap[i] = last;
+  return first;
+}