healer-man/src/game/partyNavigation.ts

import type { BufferGeometry } from "three";

export const PARTY_NAVIGATION_WELD_TOLERANCE = 0.001;
export const MAX_PARTY_NAVIGATION_DETOUR_POLYGONS = 50_000;
export const MAX_PARTY_NAVIGATION_DETOUR_LINKS = 100_000;
export const MAX_PARTY_NAVIGATION_DETAIL_TRIANGLES = 200_000;
export const PARTY_NAVIGATION_SURFACE_BUCKET_SIZE = 8;
const MAX_PARTY_NAVIGATION_SURFACE_BUCKET_REFERENCES = 2_000_000;

export type PartyNavigationPosition = readonly [x: number, y: number, z: number];
export type PartyNavigationPoint = PartyNavigationPosition | Readonly<{ x: number; y: number; z: number }>;

export interface PartyNavigationEdge {
  readonly to: number;
  readonly cost: number;
  /** Center of an exact Detour portal; present only for a source-pinned graph. */
  readonly point?: PartyNavigationPosition;
}

export interface PartyNavigationNode {
  readonly id: number;
  readonly position: PartyNavigationPosition;
  readonly neighbors: readonly PartyNavigationEdge[];
}

export type PartyNavigationTriangle = readonly [a: number, b: number, c: number];
export type PartyNavigationPortal = readonly [a: number, b: number];

export interface PartyNavigationTriangleEdge {
  readonly to: number;
  readonly portal: PartyNavigationPortal;
  readonly cost: number;
}

export interface PartyNavigationTriangleNode {
  readonly id: number;
  readonly centroid: PartyNavigationPosition;
  readonly neighbors: readonly PartyNavigationTriangleEdge[];
}

export interface PartyNavigationGraph {
  readonly nodes: readonly PartyNavigationNode[];
  /** Triangle node indices retained by the builder, in matching node-index order. */
  readonly triangles?: readonly PartyNavigationTriangle[];
  /** Shared-edge adjacency for the retained triangles, in matching index order. */
  readonly triangleNodes?: readonly PartyNavigationTriangleNode[];
  readonly weldTolerance: number;
  readonly sourceVertexCount: number;
  readonly sourceTriangleCount: number;
  /** Number of welded nodes before disconnected islands were discarded. */
  readonly weldedNodeCount: number;
  readonly componentCount: number;
  readonly discardedNodeCount: number;
  /** Packed original detail triangles and their source Detour polygon IDs. */
  readonly detourSurface?: PartyNavigationDetourSurface;
}

export interface PartyNavigationDetourSurface {
  /** Three positions per triangle, in the original GLB detail-mesh order. */
  readonly positions: Float32Array;
  readonly polygonByTriangle: Uint32Array;
  readonly bucketSize: number;
  readonly buckets: ReadonlyMap<string, readonly number[]>;
}

export interface PartyNavigationDetourTopology {
  readonly schemaVersion: 1;
  readonly kind: "recast-detour-surface-links";
  readonly polygonCount: number;
  readonly detailTriangleCount: number;
  readonly geometryTriangleCount: number;
  readonly directedLinkCount: number;
  readonly unrepresentedLinkCount: number;
  /** Encoded records from the `runewakerNavigationTopology` GLB extra. */
  readonly polygons: readonly (readonly number[])[];
  readonly links: readonly (readonly number[])[];
}

export interface PartyNavigationBuildOptions {
  /** Validation may retain floor triangles while gameplay routes off-mesh links through nodes. */
  readonly retainSurfaceTriangles?: boolean;
  /** Keep every disconnected source island selectable; routes still cannot cross components without links. */
  readonly retainAllComponents?: boolean;
  readonly weldTolerance?: number;
  /** Defensive runtime ceiling; checked before graph allocations begin. */
  readonly maxSourceTriangles?: number;
  readonly offMeshLinks?: readonly {
    readonly start: PartyNavigationPosition;
    readonly end: PartyNavigationPosition;
    readonly bidirectional: boolean;
  }[];
}

export interface PartyNavigationNearestNode {
  readonly node: PartyNavigationNode;
  readonly distance: number;
}

export interface PartyNavigationPathOptions {
  /** Reject endpoints farther than this distance from the retained navigation surface. */
  readonly maxSnapDistance?: number;
  /** Optional start-only override for maxSnapDistance. */
  readonly maxStartSnapDistance?: number;
  /** Optional end-only override for maxSnapDistance. */
  readonly maxEndSnapDistance?: number;
  /** Maximum 3D deviation when removing redundant, collinear waypoints. */
  readonly simplifyTolerance?: number;
  /** Optional collision/clearance validation for each reconstructed corridor segment. */
  readonly routeSegmentAllowed?: (
    start: PartyNavigationPosition,
    end: PartyNavigationPosition,
  ) => boolean;
}

interface MutableWeldedNode {
  readonly representative: [number, number, number];
  sumX: number;
  sumY: number;
  sumZ: number;
  samples: number;
  readonly neighbors: Set<number>;
}

interface OpenEntry {
  readonly id: number;
  readonly g: number;
  readonly h: number;
  readonly f: number;
}

export interface PartyNavigationSurfaceMatch {
  readonly position: PartyNavigationPosition;
  readonly triangleId: number;
  readonly distance: number;
  readonly polygonId?: number;
}

const DISTANCE_EPSILON = 1e-9;

function pointComponents(point: PartyNavigationPoint): PartyNavigationPosition {
  const values: PartyNavigationPosition = "x" in point
    ? [point.x, point.y, point.z]
    : [point[0], point[1], point[2]];
  if (!values.every(Number.isFinite)) {
    throw new Error("Party navigation points must contain finite coordinates.");
  }
  return values;
}

function distanceSquared(left: PartyNavigationPosition, right: PartyNavigationPosition): number {
  const dx = right[0] - left[0];
  const dy = right[1] - left[1];
  const dz = right[2] - left[2];
  return dx * dx + dy * dy + dz * dz;
}

function distance(left: PartyNavigationPosition, right: PartyNavigationPosition): number {
  return Math.sqrt(distanceSquared(left, right));
}

function bucketCoordinate(value: number, tolerance: number): number {
  return Math.floor(value / tolerance);
}

function bucketKey(x: number, y: number, z: number): string {
  return `${x},${y},${z}`;
}

function validatedTolerance(value: number | undefined): number {
  const tolerance = value ?? PARTY_NAVIGATION_WELD_TOLERANCE;
  if (!Number.isFinite(tolerance) || tolerance <= 0) {
    throw new Error("Party navigation weld tolerance must be a positive finite number.");
  }
  return tolerance;
}

function connectedComponents(nodes: readonly MutableWeldedNode[]): number[][] {
  const visited = new Uint8Array(nodes.length);
  const components: number[][] = [];

  for (let seed = 0; seed < nodes.length; seed += 1) {
    if (visited[seed]) continue;
    visited[seed] = 1;
    const pending = [seed];
    const component: number[] = [];

    while (pending.length) {
      const id = pending.pop()!;
      component.push(id);
      const neighbors = Array.from(nodes[id].neighbors).sort((left, right) => right - left);
      for (const neighbor of neighbors) {
        if (visited[neighbor]) continue;
        visited[neighbor] = 1;
        pending.push(neighbor);
      }
    }

    component.sort((left, right) => left - right);
    components.push(component);
  }

  return components;
}

export function buildPartyNavigationTriangleNodes(
  triangles: readonly PartyNavigationTriangle[],
  positions: readonly PartyNavigationPosition[],
): readonly PartyNavigationTriangleNode[] {
  const centroids = triangles.map(([a, b, c]): PartyNavigationPosition => Object.freeze([
    (positions[a][0] + positions[b][0] + positions[c][0]) / 3,
    (positions[a][1] + positions[b][1] + positions[c][1]) / 3,
    (positions[a][2] + positions[b][2] + positions[c][2]) / 3,
  ]) as PartyNavigationPosition);
  const edgeOwners = new Map<string, Array<{ triangleId: number; portal: PartyNavigationPortal }>>();
  triangles.forEach(([a, b, c], triangleId) => {
    for (const [left, right] of [[a, b], [b, c], [c, a]] as const) {
      const portal = Object.freeze([
        Math.min(left, right),
        Math.max(left, right),
      ]) as PartyNavigationPortal;
      const key = `${portal[0]},${portal[1]}`;
      const owners = edgeOwners.get(key);
      if (owners) owners.push({ triangleId, portal });
      else edgeOwners.set(key, [{ triangleId, portal }]);
    }
  });

  const neighbors: PartyNavigationTriangleEdge[][] = triangles.map(() => []);
  for (const owners of edgeOwners.values()) {
    for (let leftIndex = 0; leftIndex < owners.length; leftIndex += 1) {
      for (let rightIndex = leftIndex + 1; rightIndex < owners.length; rightIndex += 1) {
        const left = owners[leftIndex];
        const right = owners[rightIndex];
        const cost = distance(centroids[left.triangleId], centroids[right.triangleId]);
        neighbors[left.triangleId].push(Object.freeze({
          to: right.triangleId,
          portal: left.portal,
          cost,
        }));
        neighbors[right.triangleId].push(Object.freeze({
          to: left.triangleId,
          portal: right.portal,
          cost,
        }));
      }
    }
  }

  return Object.freeze(centroids.map((centroid, id): PartyNavigationTriangleNode => Object.freeze({
    id,
    centroid,
    neighbors: Object.freeze(neighbors[id].sort((left, right) => (
      left.to - right.to
      || left.portal[0] - right.portal[0]
      || left.portal[1] - right.portal[1]
    ))),
  })));
}

/**
 * Converts an indexed or non-indexed triangle geometry into a deterministic
 * navigation graph. Export seams within one millimeter are welded. By default
 * only the largest connected surface is retained; source imports can opt into
 * preserving all disconnected surfaces for local queries and routing.
 */
export function buildPartyNavigationGraph(
  geometry: BufferGeometry,
  options: PartyNavigationBuildOptions = {},
): PartyNavigationGraph {
  const tolerance = validatedTolerance(options.weldTolerance);
  const position = geometry.getAttribute("position");
  if (!position || position.itemSize < 3 || position.count < 3) {
    throw new Error("Party navigation geometry requires at least three 3D positions.");
  }

  const index = geometry.getIndex();
  const indexCount = index?.count ?? position.count;
  if (indexCount < 3 || indexCount % 3 !== 0) {
    throw new Error("Party navigation geometry must contain complete triangles.");
  }
  const sourceTriangleCount = indexCount / 3;
  if (
    options.maxSourceTriangles !== undefined
    && (
      !Number.isInteger(options.maxSourceTriangles)
      || options.maxSourceTriangles <= 0
    )
  ) {
    throw new Error("Party navigation maximum source triangles must be a positive integer.");
  }
  if (
    options.maxSourceTriangles !== undefined
    && sourceTriangleCount > options.maxSourceTriangles
  ) {
    throw new Error(
      `Party navigation geometry has ${sourceTriangleCount} triangles; maximum is ${options.maxSourceTriangles}.`,
    );
  }

  const sourceToWelded = new Int32Array(position.count);
  const welded: MutableWeldedNode[] = [];
  const buckets = new Map<string, number[]>();
  const toleranceSquared = tolerance * tolerance;

  for (let sourceId = 0; sourceId < position.count; sourceId += 1) {
    const x = position.getX(sourceId);
    const y = position.getY(sourceId);
    const z = position.getZ(sourceId);
    if (![x, y, z].every(Number.isFinite)) {
      throw new Error(`Party navigation vertex ${sourceId} contains a non-finite coordinate.`);
    }

    const bucketX = bucketCoordinate(x, tolerance);
    const bucketY = bucketCoordinate(y, tolerance);
    const bucketZ = bucketCoordinate(z, tolerance);
    let bestId = -1;
    let bestDistanceSquared = Number.POSITIVE_INFINITY;

    for (let offsetX = -1; offsetX <= 1; offsetX += 1) {
      for (let offsetY = -1; offsetY <= 1; offsetY += 1) {
        for (let offsetZ = -1; offsetZ <= 1; offsetZ += 1) {
          const candidates = buckets.get(bucketKey(
            bucketX + offsetX,
            bucketY + offsetY,
            bucketZ + offsetZ,
          ));
          if (!candidates) continue;
          for (const candidateId of candidates) {
            const candidate = welded[candidateId].representative;
            const dx = candidate[0] - x;
            const dy = candidate[1] - y;
            const dz = candidate[2] - z;
            const candidateDistanceSquared = dx * dx + dy * dy + dz * dz;
            if (candidateDistanceSquared > toleranceSquared + DISTANCE_EPSILON) continue;
            if (
              candidateDistanceSquared < bestDistanceSquared - DISTANCE_EPSILON
              || (Math.abs(candidateDistanceSquared - bestDistanceSquared) <= DISTANCE_EPSILON
                && candidateId < bestId)
            ) {
              bestId = candidateId;
              bestDistanceSquared = candidateDistanceSquared;
            }
          }
        }
      }
    }

    if (bestId < 0) {
      bestId = welded.length;
      welded.push({
        representative: [x, y, z],
        sumX: x,
        sumY: y,
        sumZ: z,
        samples: 1,
        neighbors: new Set(),
      });
      const key = bucketKey(bucketX, bucketY, bucketZ);
      const contents = buckets.get(key);
      if (contents) contents.push(bestId);
      else buckets.set(key, [bestId]);
    } else {
      const node = welded[bestId];
      node.sumX += x;
      node.sumY += y;
      node.sumZ += z;
      node.samples += 1;
    }
    sourceToWelded[sourceId] = bestId;
  }

  const sourceIndex = (offset: number): number => {
    const value = index ? index.getX(offset) : offset;
    if (!Number.isInteger(value) || value < 0 || value >= position.count) {
      throw new Error(`Party navigation index ${offset} references an invalid vertex.`);
    }
    return value;
  };
  const connect = (left: number, right: number) => {
    if (left === right) return;
    welded[left].neighbors.add(right);
    welded[right].neighbors.add(left);
  };
  const weldedTriangles: PartyNavigationTriangle[] = [];
  const blockedOffMeshDirections = new Set<string>();

  for (let offset = 0; offset < indexCount; offset += 3) {
    const a = sourceToWelded[sourceIndex(offset)];
    const b = sourceToWelded[sourceIndex(offset + 1)];
    const c = sourceToWelded[sourceIndex(offset + 2)];
    connect(a, b);
    connect(b, c);
    connect(c, a);
    if (a !== b && b !== c && c !== a) {
      weldedTriangles.push(Object.freeze([a, b, c]));
    }
  }

  const nearestWeldedNode = (point: PartyNavigationPosition): number => {
    let nearest = 0;
    let nearestDistance = Number.POSITIVE_INFINITY;
    welded.forEach((node, id) => {
      const candidate = distanceSquared(node.representative, point);
      if (candidate < nearestDistance) {
        nearest = id;
        nearestDistance = candidate;
      }
    });
    return nearest;
  };
  for (const link of options.offMeshLinks ?? []) {
    const start = nearestWeldedNode(link.start);
    const end = nearestWeldedNode(link.end);
    connect(start, end); // Weak connectivity determines which authored surfaces are retained.
    if (!link.bidirectional) blockedOffMeshDirections.add(`${end}:${start}`);
  }

  const components = connectedComponents(welded);
  const largest = components.reduce<number[]>((best, candidate) => {
    if (candidate.length > best.length) return candidate;
    if (candidate.length < best.length) return best;
    return (candidate[0] ?? Number.POSITIVE_INFINITY) < (best[0] ?? Number.POSITIVE_INFINITY)
      ? candidate
      : best;
  }, []);
  if (!largest.length) throw new Error("Party navigation geometry produced no usable graph nodes.");
  const retained = options.retainAllComponents
    ? components.flat()
    : largest;

  const oldToNew = new Int32Array(welded.length);
  oldToNew.fill(-1);
  retained.forEach((oldId, newId) => { oldToNew[oldId] = newId; });

  const retainedPositions = retained.map((oldId): PartyNavigationPosition => {
    const node = welded[oldId];
    return Object.freeze([
      node.sumX / node.samples,
      node.sumY / node.samples,
      node.sumZ / node.samples,
    ]) as PartyNavigationPosition;
  });
  const nodes = retained.map((oldId, id): PartyNavigationNode => {
    const neighbors = Array.from(welded[oldId].neighbors)
      .filter((neighbor) => oldToNew[neighbor] >= 0)
      .filter((neighbor) => !blockedOffMeshDirections.has(`${oldId}:${neighbor}`))
      .sort((left, right) => left - right)
      .map((neighbor): PartyNavigationEdge => {
        const to = oldToNew[neighbor];
        return Object.freeze({ to, cost: distance(retainedPositions[id], retainedPositions[to]) });
      });
    return Object.freeze({
      id,
      position: retainedPositions[id],
      neighbors: Object.freeze(neighbors),
    });
  });
  const triangles = weldedTriangles
    .filter(([a, b, c]) => oldToNew[a] >= 0 && oldToNew[b] >= 0 && oldToNew[c] >= 0)
    .map(([a, b, c]): PartyNavigationTriangle => Object.freeze([
      oldToNew[a],
      oldToNew[b],
      oldToNew[c],
    ]));
  if (!triangles.length) {
    throw new Error("Party navigation geometry produced no usable retained triangles.");
  }
  const usesOffMeshLinks = Boolean(options.offMeshLinks?.length);
  const triangleNodes = usesOffMeshLinks ? undefined : buildPartyNavigationTriangleNodes(triangles, retainedPositions);

  return Object.freeze({
    nodes: Object.freeze(nodes),
    ...(usesOffMeshLinks && !options.retainSurfaceTriangles ? {} : {
      triangles: Object.freeze(triangles),
      triangleNodes,
    }),
    weldTolerance: tolerance,
    sourceVertexCount: position.count,
    sourceTriangleCount,
    weldedNodeCount: welded.length,
    componentCount: components.length,
    discardedNodeCount: welded.length - nodes.length,
  });
}

function topologyRecordNumber(record: readonly number[], index: number, label: string): number {
  const value = record[index];
  if (typeof value !== "number" || !Number.isFinite(value)) {
    throw new Error(`Party navigation Detour topology ${label} contains a non-finite value.`);
  }
  return value;
}

function topologyInteger(
  record: readonly number[],
  index: number,
  label: string,
  minimum = 0,
): number {
  const value = topologyRecordNumber(record, index, label);
  if (!Number.isInteger(value) || value < minimum) {
    throw new Error(`Party navigation Detour topology ${label} contains an invalid integer.`);
  }
  return value;
}

/**
 * Builds the runtime graph from Recast's serialized Detour polygons and links.
 * Surface projection continues to use the original detail triangles, while
 * A* follows only the directed links authored by Detour.
 */
export function buildPartyNavigationGraphFromDetourTopology(
  geometry: BufferGeometry,
  rawTopology: unknown,
): PartyNavigationGraph {
  if (!rawTopology || typeof rawTopology !== "object") {
    throw new Error("Party navigation Detour topology must be an object.");
  }
  const topology = rawTopology as Partial<PartyNavigationDetourTopology> & {
    readonly unrepresentedLinkCount?: number;
  };
  if (topology.schemaVersion !== 1 || topology.kind !== "recast-detour-surface-links") {
    throw new Error("Party navigation Detour topology has an unsupported schema.");
  }
  const polygonCount = topology.polygonCount;
  const detailTriangleCount = topology.detailTriangleCount;
  const geometryTriangleCount = topology.geometryTriangleCount;
  const directedLinkCount = topology.directedLinkCount;
  if (
    typeof polygonCount !== "number"
    || !Number.isInteger(polygonCount)
    || polygonCount <= 0
    || polygonCount > MAX_PARTY_NAVIGATION_DETOUR_POLYGONS
  ) {
    throw new Error(`Party navigation Detour topology exceeds the ${MAX_PARTY_NAVIGATION_DETOUR_POLYGONS}-polygon limit.`);
  }
  if (
    typeof directedLinkCount !== "number"
    || !Number.isInteger(directedLinkCount)
    || directedLinkCount < 0
    || directedLinkCount > MAX_PARTY_NAVIGATION_DETOUR_LINKS
  ) {
    throw new Error(`Party navigation Detour topology exceeds the ${MAX_PARTY_NAVIGATION_DETOUR_LINKS}-link limit.`);
  }
  if (
    typeof detailTriangleCount !== "number"
    || !Number.isInteger(detailTriangleCount)
    || detailTriangleCount <= 0
    || detailTriangleCount > MAX_PARTY_NAVIGATION_DETAIL_TRIANGLES
    || typeof geometryTriangleCount !== "number"
    || !Number.isInteger(geometryTriangleCount)
    || geometryTriangleCount !== detailTriangleCount
  ) {
    throw new Error(`Party navigation Detour detail geometry exceeds the ${MAX_PARTY_NAVIGATION_DETAIL_TRIANGLES}-triangle limit or has inconsistent counts.`);
  }
  if (
    !Array.isArray(topology.polygons)
    || topology.polygons.length !== polygonCount
    || !Array.isArray(topology.links)
    || topology.links.length !== directedLinkCount
    || topology.unrepresentedLinkCount !== 0
  ) {
    throw new Error("Party navigation Detour topology records are incomplete.");
  }

  const position = geometry.getAttribute("position");
  if (!position || position.itemSize < 3 || position.count < 3) {
    throw new Error("Party navigation detail geometry requires at least three 3D positions.");
  }
  const index = geometry.getIndex();
  const indexCount = index?.count ?? position.count;
  if (indexCount % 3 !== 0 || indexCount / 3 !== geometryTriangleCount) {
    throw new Error("Party navigation Detour topology does not match the detail triangle geometry.");
  }

  const polygonByTriangle = new Int32Array(detailTriangleCount);
  polygonByTriangle.fill(-1);
  const polygonVertices: number[] = [];
  for (let polygonId = 0; polygonId < polygonCount; polygonId += 1) {
    const record = topology.polygons[polygonId];
    if (!Array.isArray(record) || record.length !== 9) {
      throw new Error(`Party navigation Detour polygon ${polygonId} has an invalid record.`);
    }
    const id = topologyInteger(record, 0, `polygon ${polygonId} id`);
    const tileIndex = topologyInteger(record, 1, `polygon ${polygonId} tile`);
    topologyInteger(record, 2, `polygon ${polygonId} tile x`, Number.MIN_SAFE_INTEGER);
    topologyInteger(record, 3, `polygon ${polygonId} tile y`, Number.MIN_SAFE_INTEGER);
    topologyInteger(record, 4, `polygon ${polygonId} tile layer`, Number.MIN_SAFE_INTEGER);
    topologyInteger(record, 5, `polygon ${polygonId} tile polygon`);
    const firstTriangle = topologyInteger(record, 6, `polygon ${polygonId} first triangle`);
    const triangleCount = topologyInteger(record, 7, `polygon ${polygonId} triangle count`);
    const vertexCount = topologyInteger(record, 8, `polygon ${polygonId} vertex count`);
    if (
      id !== polygonId
      || tileIndex < 0
      || triangleCount <= 0
      || vertexCount < 3
      || vertexCount > 12
      || firstTriangle + triangleCount > detailTriangleCount
    ) {
      throw new Error(`Party navigation Detour polygon ${polygonId} has an invalid detail range.`);
    }
    for (let triangleId = firstTriangle; triangleId < firstTriangle + triangleCount; triangleId += 1) {
      if (polygonByTriangle[triangleId] !== -1) {
        throw new Error(`Party navigation Detour detail triangle ${triangleId} is assigned more than once.`);
      }
      polygonByTriangle[triangleId] = polygonId;
    }
    polygonVertices.push(vertexCount);
  }
  for (let triangleId = 0; triangleId < detailTriangleCount; triangleId += 1) {
    if (polygonByTriangle[triangleId] < 0) {
      throw new Error(`Party navigation Detour detail triangle ${triangleId} has no source polygon.`);
    }
  }

  const trianglePositions = new Float32Array(detailTriangleCount * 9);
  const weightedCenters = new Float64Array(polygonCount * 4);
  const triangleIndex = (offset: number): number => {
    const value = index ? index.getX(offset) : offset;
    if (!Number.isInteger(value) || value < 0 || value >= position.count) {
      throw new Error(`Party navigation detail index ${offset} references an invalid vertex.`);
    }
    return value;
  };
  const bucketSize = PARTY_NAVIGATION_SURFACE_BUCKET_SIZE;
  const buckets = new Map<string, number[]>();
  let bucketReferences = 0;
  for (let triangleId = 0; triangleId < detailTriangleCount; triangleId += 1) {
    const positionOffset = triangleId * 9;
    const sourceOffset = triangleId * 3;
    const vertexIds = [
      triangleIndex(sourceOffset),
      triangleIndex(sourceOffset + 1),
      triangleIndex(sourceOffset + 2),
    ];
    const points = vertexIds.map((vertexId): PartyNavigationPosition => {
      const point: PartyNavigationPosition = [
        position.getX(vertexId),
        position.getY(vertexId),
        position.getZ(vertexId),
      ];
      if (!point.every(Number.isFinite)) {
        throw new Error(`Party navigation detail triangle ${triangleId} has a non-finite vertex.`);
      }
      return point;
    });
    for (let vertex = 0; vertex < 3; vertex += 1) {
      trianglePositions[positionOffset + vertex * 3] = points[vertex][0];
      trianglePositions[positionOffset + vertex * 3 + 1] = points[vertex][1];
      trianglePositions[positionOffset + vertex * 3 + 2] = points[vertex][2];
    }

    const [a, b, c] = points;
    const ab: PartyNavigationPosition = [b[0] - a[0], b[1] - a[1], b[2] - a[2]];
    const ac: PartyNavigationPosition = [c[0] - a[0], c[1] - a[1], c[2] - a[2]];
    const crossX = ab[1] * ac[2] - ab[2] * ac[1];
    const crossY = ab[2] * ac[0] - ab[0] * ac[2];
    const crossZ = ab[0] * ac[1] - ab[1] * ac[0];
    const area = Math.max(1e-12, Math.hypot(crossX, crossY, crossZ) * 0.5);
    const polygonId = polygonByTriangle[triangleId];
    const centerOffset = polygonId * 4;
    weightedCenters[centerOffset] += ((a[0] + b[0] + c[0]) / 3) * area;
    weightedCenters[centerOffset + 1] += ((a[1] + b[1] + c[1]) / 3) * area;
    weightedCenters[centerOffset + 2] += ((a[2] + b[2] + c[2]) / 3) * area;
    weightedCenters[centerOffset + 3] += area;

    const minCellX = Math.floor(Math.min(a[0], b[0], c[0]) / bucketSize);
    const maxCellX = Math.floor(Math.max(a[0], b[0], c[0]) / bucketSize);
    const minCellZ = Math.floor(Math.min(a[2], b[2], c[2]) / bucketSize);
    const maxCellZ = Math.floor(Math.max(a[2], b[2], c[2]) / bucketSize);
    const references = (maxCellX - minCellX + 1) * (maxCellZ - minCellZ + 1);
    if (
      !Number.isSafeInteger(references)
      || references <= 0
      || bucketReferences + references > MAX_PARTY_NAVIGATION_SURFACE_BUCKET_REFERENCES
    ) {
      throw new Error("Party navigation Detour detail surface exceeds the spatial-index budget.");
    }
    bucketReferences += references;
    for (let cellX = minCellX; cellX <= maxCellX; cellX += 1) {
      for (let cellZ = minCellZ; cellZ <= maxCellZ; cellZ += 1) {
        const key = bucketKey(cellX, cellZ, 0);
        const contents = buckets.get(key);
        if (contents) contents.push(triangleId);
        else buckets.set(key, [triangleId]);
      }
    }
  }

  const centers: PartyNavigationPosition[] = [];
  const weakNeighbors = Array.from({ length: polygonCount }, () => new Set<number>());
  const outgoing: Array<PartyNavigationEdge[]> = Array.from({ length: polygonCount }, () => []);
  for (let polygonId = 0; polygonId < polygonCount; polygonId += 1) {
    const offset = polygonId * 4;
    const weight = weightedCenters[offset + 3];
    if (!Number.isFinite(weight) || weight <= 0) {
      throw new Error(`Party navigation Detour polygon ${polygonId} has no usable detail surface.`);
    }
    centers.push(Object.freeze([
      weightedCenters[offset] / weight,
      weightedCenters[offset + 1] / weight,
      weightedCenters[offset + 2] / weight,
    ]) as PartyNavigationPosition);
  }

  for (let linkId = 0; linkId < directedLinkCount; linkId += 1) {
    const link = topology.links[linkId];
    if (!Array.isArray(link) || link.length !== 14) {
      throw new Error(`Party navigation Detour link ${linkId} has an invalid record.`);
    }
    const from = topologyInteger(link, 0, `link ${linkId} source`);
    const to = topologyInteger(link, 1, `link ${linkId} destination`);
    const edge = topologyInteger(link, 2, `link ${linkId} edge`);
    const side = topologyInteger(link, 3, `link ${linkId} side`);
    const bmin = topologyInteger(link, 4, `link ${linkId} portal start`);
    const bmax = topologyInteger(link, 5, `link ${linkId} portal end`);
    topologyInteger(link, 6, `link ${linkId} first vertex`);
    topologyInteger(link, 7, `link ${linkId} second vertex`);
    if (
      from >= polygonCount
      || to >= polygonCount
      || edge >= polygonVertices[from]
      || (side > 7 && side !== 255)
      || bmin > 255
      || bmax > 255
      || bmin > bmax
    ) {
      throw new Error(`Party navigation Detour link ${linkId} references an invalid source portal.`);
    }
    const start: PartyNavigationPosition = [
      topologyRecordNumber(link, 8, `link ${linkId} start x`),
      topologyRecordNumber(link, 9, `link ${linkId} start y`),
      topologyRecordNumber(link, 10, `link ${linkId} start z`),
    ];
    const end: PartyNavigationPosition = [
      topologyRecordNumber(link, 11, `link ${linkId} end x`),
      topologyRecordNumber(link, 12, `link ${linkId} end y`),
      topologyRecordNumber(link, 13, `link ${linkId} end z`),
    ];
    // Detour uses side=0xff for an internal full-edge link. Boundary links
    // encode the traversable edge interval as bmin/bmax in byte coordinates.
    const ratio = side === 255 ? 0.5 : (bmin + bmax) / 510;
    const portal = Object.freeze([
      start[0] + (end[0] - start[0]) * ratio,
      start[1] + (end[1] - start[1]) * ratio,
      start[2] + (end[2] - start[2]) * ratio,
    ]) as PartyNavigationPosition;
    outgoing[from].push(Object.freeze({
      to,
      cost: distance(centers[from], centers[to]),
      point: portal,
    }));
    weakNeighbors[from].add(to);
    weakNeighbors[to].add(from);
  }

  const nodes = centers.map((center, id): PartyNavigationNode => Object.freeze({
    id,
    position: center,
    neighbors: Object.freeze(outgoing[id].sort((left, right) => (
      left.to - right.to
      || (left.point?.[0] ?? 0) - (right.point?.[0] ?? 0)
      || (left.point?.[1] ?? 0) - (right.point?.[1] ?? 0)
      || (left.point?.[2] ?? 0) - (right.point?.[2] ?? 0)
    ))),
  }));
  const visited = new Uint8Array(polygonCount);
  let componentCount = 0;
  for (let seed = 0; seed < polygonCount; seed += 1) {
    if (visited[seed]) continue;
    componentCount += 1;
    visited[seed] = 1;
    const pending = [seed];
    while (pending.length) {
      const current = pending.pop()!;
      for (const neighbor of weakNeighbors[current]) {
        if (visited[neighbor]) continue;
        visited[neighbor] = 1;
        pending.push(neighbor);
      }
    }
  }

  const detourSurface: PartyNavigationDetourSurface = Object.freeze({
    positions: trianglePositions,
    polygonByTriangle: Uint32Array.from(polygonByTriangle),
    bucketSize,
    buckets: new Map(Array.from(buckets, ([key, values]) => [key, Object.freeze(values)])),
  });
  return Object.freeze({
    nodes: Object.freeze(nodes),
    detourSurface,
    weldTolerance: PARTY_NAVIGATION_WELD_TOLERANCE,
    sourceVertexCount: position.count,
    sourceTriangleCount: geometryTriangleCount,
    weldedNodeCount: polygonCount,
    componentCount,
    discardedNodeCount: 0,
  });
}

export function findNearestPartyNavigationNode(
  graph: PartyNavigationGraph,
  point: PartyNavigationPoint,
  maxDistance = Number.POSITIVE_INFINITY,
): PartyNavigationNearestNode | null {
  const target = pointComponents(point);
  const limit = Number.isFinite(maxDistance) ? Math.max(0, maxDistance) : Number.POSITIVE_INFINITY;
  let nearest: PartyNavigationNode | null = null;
  let nearestDistanceSquared = limit * limit;

  for (const node of graph.nodes) {
    const candidateDistanceSquared = distanceSquared(target, node.position);
    if (
      candidateDistanceSquared < nearestDistanceSquared - DISTANCE_EPSILON
      || (Math.abs(candidateDistanceSquared - nearestDistanceSquared) <= DISTANCE_EPSILON
        && (nearest === null || node.id < nearest.id))
    ) {
      nearest = node;
      nearestDistanceSquared = candidateDistanceSquared;
    }
  }

  return nearest ? Object.freeze({ node: nearest, distance: Math.sqrt(nearestDistanceSquared) }) : null;
}

function closestPointOnSegment(
  point: PartyNavigationPosition,
  start: PartyNavigationPosition,
  end: PartyNavigationPosition,
): PartyNavigationPosition {
  const segmentX = end[0] - start[0];
  const segmentY = end[1] - start[1];
  const segmentZ = end[2] - start[2];
  const lengthSquared = segmentX * segmentX + segmentY * segmentY + segmentZ * segmentZ;
  if (lengthSquared <= DISTANCE_EPSILON) return [start[0], start[1], start[2]];
  const projection = Math.max(0, Math.min(1, (
    (point[0] - start[0]) * segmentX
    + (point[1] - start[1]) * segmentY
    + (point[2] - start[2]) * segmentZ
  ) / lengthSquared));
  return [
    start[0] + segmentX * projection,
    start[1] + segmentY * projection,
    start[2] + segmentZ * projection,
  ];
}

/** Returns the closest point on a filled 3D triangle, including its edges. */
function closestPointOnTriangle(
  point: PartyNavigationPosition,
  a: PartyNavigationPosition,
  b: PartyNavigationPosition,
  c: PartyNavigationPosition,
): PartyNavigationPosition {
  const ab: PartyNavigationPosition = [b[0] - a[0], b[1] - a[1], b[2] - a[2]];
  const ac: PartyNavigationPosition = [c[0] - a[0], c[1] - a[1], c[2] - a[2]];
  const crossX = ab[1] * ac[2] - ab[2] * ac[1];
  const crossY = ab[2] * ac[0] - ab[0] * ac[2];
  const crossZ = ab[0] * ac[1] - ab[1] * ac[0];
  if (crossX * crossX + crossY * crossY + crossZ * crossZ <= DISTANCE_EPSILON) {
    const candidates = [
      closestPointOnSegment(point, a, b),
      closestPointOnSegment(point, b, c),
      closestPointOnSegment(point, c, a),
    ];
    return candidates.reduce((best, candidate) => (
      distanceSquared(point, candidate) < distanceSquared(point, best) - DISTANCE_EPSILON
        ? candidate
        : best
    ));
  }

  const ap: PartyNavigationPosition = [point[0] - a[0], point[1] - a[1], point[2] - a[2]];
  const d1 = ab[0] * ap[0] + ab[1] * ap[1] + ab[2] * ap[2];
  const d2 = ac[0] * ap[0] + ac[1] * ap[1] + ac[2] * ap[2];
  if (d1 <= 0 && d2 <= 0) return [a[0], a[1], a[2]];

  const bp: PartyNavigationPosition = [point[0] - b[0], point[1] - b[1], point[2] - b[2]];
  const d3 = ab[0] * bp[0] + ab[1] * bp[1] + ab[2] * bp[2];
  const d4 = ac[0] * bp[0] + ac[1] * bp[1] + ac[2] * bp[2];
  if (d3 >= 0 && d4 <= d3) return [b[0], b[1], b[2]];

  const vc = d1 * d4 - d3 * d2;
  if (vc <= 0 && d1 >= 0 && d3 <= 0) {
    const along = d1 / (d1 - d3);
    return [a[0] + ab[0] * along, a[1] + ab[1] * along, a[2] + ab[2] * along];
  }

  const cp: PartyNavigationPosition = [point[0] - c[0], point[1] - c[1], point[2] - c[2]];
  const d5 = ab[0] * cp[0] + ab[1] * cp[1] + ab[2] * cp[2];
  const d6 = ac[0] * cp[0] + ac[1] * cp[1] + ac[2] * cp[2];
  if (d6 >= 0 && d5 <= d6) return [c[0], c[1], c[2]];

  const vb = d5 * d2 - d1 * d6;
  if (vb <= 0 && d2 >= 0 && d6 <= 0) {
    const along = d2 / (d2 - d6);
    return [a[0] + ac[0] * along, a[1] + ac[1] * along, a[2] + ac[2] * along];
  }

  const va = d3 * d6 - d5 * d4;
  if (va <= 0 && d4 - d3 >= 0 && d5 - d6 >= 0) {
    const along = (d4 - d3) / ((d4 - d3) + (d5 - d6));
    return [
      b[0] + (c[0] - b[0]) * along,
      b[1] + (c[1] - b[1]) * along,
      b[2] + (c[2] - b[2]) * along,
    ];
  }

  const denominator = 1 / (va + vb + vc);
  const alongB = vb * denominator;
  const alongC = vc * denominator;
  return [
    a[0] + ab[0] * alongB + ac[0] * alongC,
    a[1] + ab[1] * alongB + ac[1] * alongC,
    a[2] + ab[2] * alongB + ac[2] * alongC,
  ];
}

export function findNearestPartyNavigationSurface(
  graph: PartyNavigationGraph,
  point: PartyNavigationPoint,
  maxDistance = Number.POSITIVE_INFINITY,
): PartyNavigationSurfaceMatch | null {
  if (graph.detourSurface) {
    const target = pointComponents(point);
    const limit = Number.isFinite(maxDistance) ? Math.max(0, maxDistance) : Number.POSITIVE_INFINITY;
    const surface = graph.detourSurface;
    const candidates: number[] = [];
    if (!Number.isFinite(limit)) {
      for (let triangleId = 0; triangleId < surface.polygonByTriangle.length; triangleId += 1) {
        candidates.push(triangleId);
      }
    } else {
      const minCellX = Math.floor((target[0] - limit) / surface.bucketSize);
      const maxCellX = Math.floor((target[0] + limit) / surface.bucketSize);
      const minCellZ = Math.floor((target[2] - limit) / surface.bucketSize);
      const maxCellZ = Math.floor((target[2] + limit) / surface.bucketSize);
      const cellCount = (maxCellX - minCellX + 1) * (maxCellZ - minCellZ + 1);
      if (!Number.isSafeInteger(cellCount) || cellCount > 4_096) {
        for (let triangleId = 0; triangleId < surface.polygonByTriangle.length; triangleId += 1) {
          candidates.push(triangleId);
        }
      } else {
        const found = new Set<number>();
        for (let cellX = minCellX; cellX <= maxCellX; cellX += 1) {
          for (let cellZ = minCellZ; cellZ <= maxCellZ; cellZ += 1) {
            for (const triangleId of surface.buckets.get(bucketKey(cellX, cellZ, 0)) ?? []) {
              found.add(triangleId);
            }
          }
        }
        candidates.push(...Array.from(found).sort((left, right) => left - right));
      }
    }

    let nearestPosition: PartyNavigationPosition | null = null;
    let nearestTriangleId = -1;
    let nearestDistanceSquared = limit * limit;
    for (const triangleId of candidates) {
      const offset = triangleId * 9;
      const candidate = closestPointOnTriangle(
        target,
        [surface.positions[offset], surface.positions[offset + 1], surface.positions[offset + 2]],
        [surface.positions[offset + 3], surface.positions[offset + 4], surface.positions[offset + 5]],
        [surface.positions[offset + 6], surface.positions[offset + 7], surface.positions[offset + 8]],
      );
      const candidateDistanceSquared = distanceSquared(target, candidate);
      if (
        candidateDistanceSquared < nearestDistanceSquared - DISTANCE_EPSILON
        || (Math.abs(candidateDistanceSquared - nearestDistanceSquared) <= DISTANCE_EPSILON
          && (nearestTriangleId < 0 || triangleId < nearestTriangleId))
      ) {
        nearestPosition = candidate;
        nearestTriangleId = triangleId;
        nearestDistanceSquared = candidateDistanceSquared;
      }
    }

    return nearestPosition
      ? Object.freeze({
        position: Object.freeze([...nearestPosition]) as PartyNavigationPosition,
        triangleId: nearestTriangleId,
        polygonId: surface.polygonByTriangle[nearestTriangleId],
        distance: Math.sqrt(nearestDistanceSquared),
      })
      : null;
  }
  const triangles = graph.triangles;
  if (!triangles?.length) {
    const nearest = findNearestPartyNavigationNode(graph, point, maxDistance);
    return nearest
      ? Object.freeze({
        position: nearest.node.position,
        triangleId: -1,
        distance: nearest.distance,
      })
      : null;
  }
  const target = pointComponents(point);
  const limit = Number.isFinite(maxDistance) ? Math.max(0, maxDistance) : Number.POSITIVE_INFINITY;
  let nearestPosition: PartyNavigationPosition | null = null;
  let nearestTriangleId = -1;
  let nearestDistanceSquared = limit * limit;

  for (let triangleId = 0; triangleId < triangles.length; triangleId += 1) {
    const [a, b, c] = triangles[triangleId];
    const candidate = closestPointOnTriangle(
      target,
      graph.nodes[a].position,
      graph.nodes[b].position,
      graph.nodes[c].position,
    );
    const candidateDistanceSquared = distanceSquared(target, candidate);
    if (
      candidateDistanceSquared < nearestDistanceSquared - DISTANCE_EPSILON
      || (Math.abs(candidateDistanceSquared - nearestDistanceSquared) <= DISTANCE_EPSILON
        && (nearestTriangleId < 0 || triangleId < nearestTriangleId))
    ) {
      nearestPosition = candidate;
      nearestTriangleId = triangleId;
      nearestDistanceSquared = candidateDistanceSquared;
    }
  }

  return nearestPosition
    ? Object.freeze({
      position: Object.freeze([...nearestPosition]) as PartyNavigationPosition,
      triangleId: nearestTriangleId,
      distance: Math.sqrt(nearestDistanceSquared),
    })
    : null;
}

function pointToSegmentDistance(
  point: PartyNavigationPosition,
  start: PartyNavigationPosition,
  end: PartyNavigationPosition,
): number {
  return distance(point, closestPointOnSegment(point, start, end));
}

/** Removes duplicate and effectively-collinear points without cutting corners. */
export function simplifyPartyNavigationPath(
  points: readonly PartyNavigationPoint[],
  tolerance = PARTY_NAVIGATION_WELD_TOLERANCE,
): readonly PartyNavigationPosition[] {
  const safeTolerance = Number.isFinite(tolerance) ? Math.max(0, tolerance) : 0;
  const simplified: PartyNavigationPosition[] = [];

  for (const rawPoint of points) {
    const point = Object.freeze([...pointComponents(rawPoint)]) as PartyNavigationPosition;
    if (simplified.length && distance(simplified[simplified.length - 1], point) <= safeTolerance) continue;
    simplified.push(point);

    while (simplified.length >= 3) {
      const end = simplified[simplified.length - 1];
      const middle = simplified[simplified.length - 2];
      const start = simplified[simplified.length - 3];
      if (pointToSegmentDistance(middle, start, end) > safeTolerance) break;
      simplified.splice(simplified.length - 2, 1);
    }
  }

  return Object.freeze(simplified);
}

function compareOpenEntries(left: OpenEntry, right: OpenEntry): number {
  return left.f - right.f || left.h - right.h || left.id - right.id;
}

class OpenHeap {
  private readonly entries: OpenEntry[] = [];

  get length(): number {
    return this.entries.length;
  }

  push(entry: OpenEntry): void {
    const entries = this.entries;
    entries.push(entry);
    let index = entries.length - 1;
    while (index > 0) {
      const parent = Math.floor((index - 1) / 2);
      if (compareOpenEntries(entries[parent], entry) <= 0) break;
      entries[index] = entries[parent];
      index = parent;
    }
    entries[index] = entry;
  }

  pop(): OpenEntry | null {
    const entries = this.entries;
    if (!entries.length) return null;
    const first = entries[0];
    const last = entries.pop()!;
    if (entries.length) {
      let index = 0;
      while (true) {
        const left = index * 2 + 1;
        if (left >= entries.length) break;
        const right = left + 1;
        const bestChild = right < entries.length
          && compareOpenEntries(entries[right], entries[left]) < 0
          ? right
          : left;
        if (compareOpenEntries(entries[bestChild], last) >= 0) break;
        entries[index] = entries[bestChild];
        index = bestChild;
      }
      entries[index] = last;
    }
    return first;
  }
}

function findPartyNavigationNodePath(
  graph: PartyNavigationGraph,
  startId: number,
  endId: number,
  blocked?: Uint8Array,
): readonly number[] | null {
  if (blocked?.[startId] || blocked?.[endId]) return null;
  if (startId === endId) return Object.freeze([startId]);
  const nodeCount = graph.nodes.length;
  const cameFrom = new Int32Array(nodeCount);
  cameFrom.fill(-1);
  const scores = new Float64Array(nodeCount);
  scores.fill(Number.POSITIVE_INFINITY);
  scores[startId] = 0;
  const closed = new Uint8Array(nodeCount);
  const open = new OpenHeap();
  const initialHeuristic = distance(graph.nodes[startId].position, graph.nodes[endId].position);
  open.push({ id: startId, g: 0, h: initialHeuristic, f: initialHeuristic });

  while (open.length) {
    const current = open.pop()!;
    if (closed[current.id] || current.g > scores[current.id] + DISTANCE_EPSILON) continue;
    if (current.id === endId) break;
    closed[current.id] = 1;

    for (const edge of graph.nodes[current.id].neighbors) {
      if (blocked?.[edge.to] || closed[edge.to]) continue;
      const tentative = scores[current.id] + edge.cost;
      if (tentative >= scores[edge.to] - DISTANCE_EPSILON) continue;
      cameFrom[edge.to] = current.id;
      scores[edge.to] = tentative;
      const heuristic = distance(graph.nodes[edge.to].position, graph.nodes[endId].position);
      open.push({ id: edge.to, g: tentative, h: heuristic, f: tentative + heuristic });
    }
  }

  if (cameFrom[endId] < 0) return null;
  const nodeIds = [endId];
  let cursor = endId;
  while (cursor !== startId && nodeIds.length <= nodeCount) {
    cursor = cameFrom[cursor];
    if (cursor < 0) return null;
    nodeIds.push(cursor);
  }
  if (cursor !== startId) return null;
  nodeIds.reverse();
  return Object.freeze(nodeIds);
}

function findPartyNavigationTrianglePath(
  triangleNodes: readonly PartyNavigationTriangleNode[],
  startId: number,
  endId: number,
  blocked?: Uint8Array,
): readonly number[] | null {
  if (blocked?.[startId] || blocked?.[endId]) return null;
  if (startId === endId) return Object.freeze([startId]);
  const nodeCount = triangleNodes.length;
  const cameFrom = new Int32Array(nodeCount);
  cameFrom.fill(-1);
  const scores = new Float64Array(nodeCount);
  scores.fill(Number.POSITIVE_INFINITY);
  scores[startId] = 0;
  const closed = new Uint8Array(nodeCount);
  const open = new OpenHeap();
  const initialHeuristic = distance(triangleNodes[startId].centroid, triangleNodes[endId].centroid);
  open.push({ id: startId, g: 0, h: initialHeuristic, f: initialHeuristic });

  while (open.length) {
    const current = open.pop()!;
    if (closed[current.id] || current.g > scores[current.id] + DISTANCE_EPSILON) continue;
    if (current.id === endId) break;
    closed[current.id] = 1;

    for (const edge of triangleNodes[current.id].neighbors) {
      if (blocked?.[edge.to] || closed[edge.to]) continue;
      const tentative = scores[current.id] + edge.cost;
      if (tentative >= scores[edge.to] - DISTANCE_EPSILON) continue;
      cameFrom[edge.to] = current.id;
      scores[edge.to] = tentative;
      const heuristic = distance(triangleNodes[edge.to].centroid, triangleNodes[endId].centroid);
      open.push({ id: edge.to, g: tentative, h: heuristic, f: tentative + heuristic });
    }
  }

  if (cameFrom[endId] < 0) return null;
  const triangleIds = [endId];
  let cursor = endId;
  while (cursor !== startId && triangleIds.length <= nodeCount) {
    cursor = cameFrom[cursor];
    if (cursor < 0) return null;
    triangleIds.push(cursor);
  }
  if (cursor !== startId) return null;
  triangleIds.reverse();
  return Object.freeze(triangleIds);
}

/** Compatibility for hand-authored graphs that predate retained triangle data. */
function findPartyNavigationNodeSnappedPath(
  graph: PartyNavigationGraph,
  start: PartyNavigationPoint,
  end: PartyNavigationPoint,
  options: PartyNavigationPathOptions,
): readonly PartyNavigationPosition[] | null {
  const maxSnapDistance = options.maxSnapDistance ?? Number.POSITIVE_INFINITY;
  const startMatch = findNearestPartyNavigationNode(
    graph,
    start,
    options.maxStartSnapDistance ?? maxSnapDistance,
  );
  const endMatch = findNearestPartyNavigationNode(
    graph,
    end,
    options.maxEndSnapDistance ?? maxSnapDistance,
  );
  if (!startMatch || !endMatch) return null;
  const nodeIds = findPartyNavigationNodePath(graph, startMatch.node.id, endMatch.node.id);
  return nodeIds
    ? simplifyPartyNavigationPath(
      nodeIds.map((id) => graph.nodes[id].position),
      options.simplifyTolerance ?? graph.weldTolerance,
    )
    : null;
}

/**
 * Projects both endpoints to the retained surface, runs deterministic A* over
 * triangle centroids, and crosses each shared edge through its midpoint.
 */
export function findPartyNavigationPath(
  graph: PartyNavigationGraph,
  start: PartyNavigationPoint,
  end: PartyNavigationPoint,
  options: PartyNavigationPathOptions = {},
): readonly PartyNavigationPosition[] | null {
  if (graph.detourSurface) {
    const maxSnapDistance = options.maxSnapDistance ?? Number.POSITIVE_INFINITY;
    const startMatch = findNearestPartyNavigationSurface(
      graph,
      start,
      options.maxStartSnapDistance ?? maxSnapDistance,
    );
    const endMatch = findNearestPartyNavigationSurface(
      graph,
      end,
      options.maxEndSnapDistance ?? maxSnapDistance,
    );
    if (
      !startMatch
      || !endMatch
      || startMatch.polygonId === undefined
      || endMatch.polygonId === undefined
    ) return null;

    const blocked = new Uint8Array(graph.nodes.length);
    const maximumReroutes = Math.min(64, graph.nodes.length);
    for (let reroute = 0; reroute <= maximumReroutes; reroute += 1) {
      const polygonIds = findPartyNavigationNodePath(
        graph,
        startMatch.polygonId,
        endMatch.polygonId,
        blocked,
      );
      if (!polygonIds) return null;
      const path: PartyNavigationPosition[] = [startMatch.position];
      let missingPortal = false;
      for (let index = 0; index < polygonIds.length - 1; index += 1) {
        const edge = graph.nodes[polygonIds[index]].neighbors.find((candidate) => (
          candidate.to === polygonIds[index + 1] && candidate.point !== undefined
        ));
        if (!edge?.point) {
          missingPortal = true;
          break;
        }
        path.push(edge.point);
      }
      if (missingPortal) return null;
      path.push(endMatch.position);

      let rejectedSegment = -1;
      if (options.routeSegmentAllowed) {
        for (let index = 0; index < path.length - 1; index += 1) {
          if (!options.routeSegmentAllowed(path[index], path[index + 1])) {
            rejectedSegment = index;
            break;
          }
        }
      }
      if (rejectedSegment < 0) return Object.freeze(path);
      if (polygonIds.length <= 1 || reroute >= maximumReroutes) return null;

      const blockedPolygonId = rejectedSegment === 0
        ? polygonIds[1]
        : rejectedSegment === polygonIds.length - 1
          ? polygonIds[polygonIds.length - 2]
          : polygonIds[rejectedSegment];
      if (blocked[blockedPolygonId]) return null;
      blocked[blockedPolygonId] = 1;
    }
    return null;
  }
  if (!graph.triangles?.length) {
    return findPartyNavigationNodeSnappedPath(graph, start, end, options);
  }
  const maxSnapDistance = options.maxSnapDistance ?? Number.POSITIVE_INFINITY;
  const startMatch = findNearestPartyNavigationSurface(
    graph,
    start,
    options.maxStartSnapDistance ?? maxSnapDistance,
  );
  const endMatch = findNearestPartyNavigationSurface(
    graph,
    end,
    options.maxEndSnapDistance ?? maxSnapDistance,
  );
  if (!startMatch || !endMatch) return null;

  const triangleNodes = graph.triangleNodes?.length === graph.triangles.length
    ? graph.triangleNodes
    : buildPartyNavigationTriangleNodes(
      graph.triangles,
      graph.nodes.map((node) => node.position),
    );
  const blocked = new Uint8Array(triangleNodes.length);
  const maximumReroutes = Math.min(64, triangleNodes.length);
  for (let reroute = 0; reroute <= maximumReroutes; reroute += 1) {
    const triangleIds = findPartyNavigationTrianglePath(
      triangleNodes,
      startMatch.triangleId,
      endMatch.triangleId,
      blocked,
    );
    if (!triangleIds) return null;

    const path: PartyNavigationPosition[] = [startMatch.position];
    for (let index = 0; index < triangleIds.length - 1; index += 1) {
      const from = triangleIds[index];
      const to = triangleIds[index + 1];
      const transition = triangleNodes[from].neighbors.find((edge) => edge.to === to);
      if (!transition) return null;
      const [a, b] = transition.portal;
      const left = graph.nodes[a].position;
      const right = graph.nodes[b].position;
      path.push(Object.freeze([
        (left[0] + right[0]) / 2,
        (left[1] + right[1]) / 2,
        (left[2] + right[2]) / 2,
      ]) as PartyNavigationPosition);
    }
    path.push(endMatch.position);

    let rejectedSegment = -1;
    if (options.routeSegmentAllowed) {
      for (let index = 0; index < path.length - 1; index += 1) {
        if (!options.routeSegmentAllowed(path[index], path[index + 1])) {
          rejectedSegment = index;
          break;
        }
      }
    }
    if (rejectedSegment < 0) return Object.freeze(path);
    if (triangleIds.length <= 1 || reroute >= maximumReroutes) return null;

    const blockedTriangleId = rejectedSegment === 0
      ? triangleIds[1]
      : rejectedSegment === triangleIds.length - 1
        ? triangleIds[triangleIds.length - 2]
        : triangleIds[rejectedSegment];
    if (blocked[blockedTriangleId]) return null;
    blocked[blockedTriangleId] = 1;
  }

  return null;
}