@mark1russell7/highways 0.0.0-stage → 0.1.1

package/src/sections.ts

@@ -0,0 +1,82 @@
+import { center, type Box } from "./geom.ts";
+import type { Node } from "./model.ts";
+
+/** A section of the plane: a cell of a quadtree, with the nodes whose centers are in it. */
+export interface Section {
+  /** The path from the root: "s", then one digit from 0 to 3 for each step down. */
+  readonly id: string;
+  readonly depth: number;
+  readonly cell: Box;
+  /** The indices of the nodes, in the order of the graph. */
+  readonly nodes: readonly number[];
+  readonly children: readonly Section[];
+}
+
+/** The options of the tree of sections. */
+export interface SectionOptions {
+  /** A section with more nodes splits. The default is 8. */
+  readonly cap?: number;
+  /** The tree stops at this depth. The default is 16. */
+  readonly maxDepth?: number;
+}
+
+/**
+ * This function makes the tree of sections of the nodes. A cell with more than `cap` nodes splits in four.
+ * When the nodes of a cell are all in one quarter, the cell becomes that quarter and does not make a level.
+ */
+export function buildSections(nodes: readonly Node[], options: SectionOptions = {}): Section {
+  const cap = Math.max(1, options.cap ?? 8);
+  const maxDepth = options.maxDepth ?? 16;
+  const centers = nodes.map((n) => center(n.box));
+  let x0 = Infinity;
+  let y0 = Infinity;
+  let x1 = -Infinity;
+  let y1 = -Infinity;
+  for (const n of nodes) {
+    x0 = Math.min(x0, n.box.x);
+    y0 = Math.min(y0, n.box.y);
+    x1 = Math.max(x1, n.box.x + n.box.w);
+    y1 = Math.max(y1, n.box.y + n.box.h);
+  }
+  const root: Box = nodes.length ? { x: x0, y: y0, w: x1 - x0, h: y1 - y0 } : { x: 0, y: 0, w: 0, h: 0 };
+  const make = (id: string, depth: number, cell: Box, list: readonly number[]): Section => {
+    let box = cell;
+    for (let guard = 0; list.length > cap && depth < maxDepth && guard < 32; guard++) {
+      const mx = box.x + box.w / 2;
+      const my = box.y + box.h / 2;
+      const parts: number[][] = [[], [], [], []];
+      for (const i of list) parts[(centers[i]!.x < mx ? 0 : 1) + (centers[i]!.y < my ? 0 : 2)]!.push(i);
+      const quarter = (q: number): Box => ({
+        x: q % 2 === 0 ? box.x : mx,
+        y: q < 2 ? box.y : my,
+        w: box.w / 2,
+        h: box.h / 2,
+      });
+      const full = parts.map((p, q) => [p, q] as const).filter(([p]) => p.length > 0);
+      if (full.length === 1) {
+        box = quarter(full[0]![1]);
+        continue;
+      }
+      const children = full.map(([p, q]) => make(`${id}${q}`, depth + 1, quarter(q), p));
+      return { id, depth, cell: box, nodes: list, children };
+    }
+    return { id, depth, cell: box, nodes: list, children: [] };
+  };
+  return make("s", 0, root, nodes.map((_, i) => i));
+}
+
+/** This function gives the sections of the tree, parents before children. */
+export function allSections(root: Section): Section[] {
+  const out: Section[] = [];
+  const walk = (s: Section): void => {
+    out.push(s);
+    for (const c of s.children) walk(c);
+  };
+  walk(root);
+  return out;
+}
+
+/** This function gives the depth of the deepest section. */
+export function treeDepth(root: Section): number {
+  return root.children.reduce((d, c) => Math.max(d, treeDepth(c)), root.depth);
+}

package/src/segments.ts

@@ -0,0 +1,250 @@
+import { dist, type Point } from "./geom.ts";
+import type { Result, Route } from "./model.ts";
+
+/** A piece of drawn way. A shared segment carries several edges, in lane order. A private segment carries one edge. */
+export interface Segment {
+  readonly id: string;
+  readonly points: readonly Point[];
+  /** The edges in lane order. Lane 0 is on the left of the segment, from its first point to its last. The y axis points down. */
+  readonly edges: readonly string[];
+  /** The number of edges. A renderer shows it on a fat edge. */
+  readonly count: number;
+}
+
+/** One step of a path: a segment, the way through it, and the lane of the edge in it. */
+export interface SegmentRef {
+  readonly segment: string;
+  /** This value is true when the edge goes from the last point of the segment to its first point. */
+  readonly reverse: boolean;
+  readonly lane: number;
+}
+
+/** The route of one edge as a list of segments. */
+export interface Path {
+  readonly edge: string;
+  readonly segments: readonly SegmentRef[];
+}
+
+/** The routes of a result as shared geometry. A renderer can draw each segment one time, with a count. */
+export interface SegmentGraph {
+  readonly segments: readonly Segment[];
+  readonly paths: readonly Path[];
+}
+
+const SAME = 1e-6;
+
+/** This function gives the index where `p` holds the points of `part` in a row, from `from` on, or -1. */
+function indexOf(p: readonly Point[], part: readonly Point[], from: number): number {
+  for (let i = from; i + part.length <= p.length; i++) {
+    let ok = true;
+    for (let j = 0; ok && j < part.length; j++) ok = dist(p[i + j]!, part[j]!) <= SAME;
+    if (ok) return i;
+  }
+  return -1;
+}
+
+/**
+ * This function breaks each route into segments. The runs and the trunks of the bundles become shared segments.
+ * The other parts of each route become private segments. The segments of a path, in order, give the points of its route again.
+ */
+export function segmentsOf(result: Result): SegmentGraph {
+  const shared = [
+    ...(result.runs ?? []).map((r) => ({ id: r.id, points: r.points, edges: r.edges })),
+    ...(result.bundles ?? []).map((b) => ({ id: b.id, points: b.trunk, edges: b.edges })),
+  ].filter((s) => s.points.length >= 2);
+  const onEdge = new Map<string, { id: string; points: readonly Point[]; lane: number }[]>();
+  for (const s of shared) {
+    s.edges.forEach((edge, lane) => {
+      const list = onEdge.get(edge);
+      if (list) list.push({ id: s.id, points: s.points, lane });
+      else onEdge.set(edge, [{ id: s.id, points: s.points, lane }]);
+    });
+  }
+  const segments: Segment[] = shared.map((s) => ({ id: s.id, points: s.points, edges: s.edges, count: s.edges.length }));
+  const paths: Path[] = [];
+  for (const route of result.routes) {
+    const p = route.points;
+    // The place of each shared segment in the route, in the order of the route.
+    const found: { at: number; end: number; ref: SegmentRef }[] = [];
+    for (const s of onEdge.get(route.edge) ?? []) {
+      let at = indexOf(p, s.points, 0);
+      let reverse = false;
+      if (at < 0) {
+        at = indexOf(p, [...s.points].reverse(), 0);
+        reverse = true;
+      }
+      if (at >= 0) found.push({ at, end: at + s.points.length - 1, ref: { segment: s.id, reverse, lane: s.lane } });
+    }
+    found.sort((a, b) => a.at - b.at);
+    const refs: SegmentRef[] = [];
+    let i = 0;
+    const own = (to: number): void => {
+      if (to <= i) return;
+      const id = `${route.edge}/${refs.length}`;
+      segments.push({ id, points: p.slice(i, to + 1), edges: [route.edge], count: 1 });
+      refs.push({ segment: id, reverse: false, lane: 0 });
+    };
+    for (const f of found) {
+      if (f.at < i) continue;
+      own(f.at);
+      refs.push(f.ref);
+      i = f.end;
+    }
+    own(p.length - 1);
+    paths.push({ edge: route.edge, segments: refs });
+  }
+  return { segments, paths };
+}
+
+/** This function gives the points of a path, with the segments in the order and the way of the path. */
+export function pathPoints(graph: SegmentGraph, path: Path): Point[] {
+  const byId = new Map(graph.segments.map((s) => [s.id, s]));
+  const out: Point[] = [];
+  for (const ref of path.segments) {
+    const s = byId.get(ref.segment);
+    if (!s) continue;
+    const pts = ref.reverse ? [...s.points].reverse() : s.points;
+    for (const q of pts) if (out.length === 0 || dist(q, out[out.length - 1]!) > SAME) out.push(q);
+  }
+  return out;
+}
+
+/**
+ * This function moves each point of a polyline by `d` to the left, in a frame with the y axis down.
+ * At an inner point, it moves along the bisector, at most 2 × `d` far.
+ */
+export function offsetPolyline(p: readonly Point[], d: number): Point[] {
+  if (d === 0 || p.length < 2) return [...p];
+  const normals: Point[] = [];
+  for (let i = 1; i < p.length; i++) {
+    const dx = p[i]!.x - p[i - 1]!.x;
+    const dy = p[i]!.y - p[i - 1]!.y;
+    const l = Math.hypot(dx, dy);
+    normals.push(l > 0 ? { x: dy / l, y: -dx / l } : (normals[normals.length - 1] ?? { x: 0, y: 0 }));
+  }
+  return p.map((q, i) => {
+    const a = normals[Math.max(0, i - 1)]!;
+    const b = normals[Math.min(normals.length - 1, i)]!;
+    const mx = a.x + b.x;
+    const my = a.y + b.y;
+    const ml = Math.hypot(mx, my);
+    if (ml < 1e-9) return { x: q.x + a.x * d, y: q.y + a.y * d };
+    const cos = (mx * a.x + my * a.y) / ml;
+    const k = Math.min(2, 1 / Math.max(cos, 1e-9)) * d;
+    return { x: q.x + (mx / ml) * k, y: q.y + (my / ml) * k };
+  });
+}
+
+/** The options of `pullApart`. */
+export interface PullApartOptions {
+  /** The distance between two lanes. */
+  readonly spacing: number;
+  /** The largest width of a segment, from its outer lane on the left to its outer lane on the right. */
+  readonly maxWidth: number;
+}
+
+/**
+ * This function pulls the lanes of each shared segment apart, and gives a route for each edge.
+ * The lanes keep the lane order, so they do not cross in a segment. The two ends of each route stay at their plugs.
+ */
+export function pullApart(graph: SegmentGraph, options: PullApartOptions): Route[] {
+  return lanePaths(graph, (s) => (s.count > 1 ? Math.min(options.spacing, options.maxWidth / (s.count - 1)) : 0));
+}
+
+/** This function gives the key of a shared run: the ids of its edges, sorted and joined with commas. */
+export function runKey(edges: readonly string[]): string {
+  return [...edges].sort().join(",");
+}
+
+/**
+ * This function opens the shared segments whose key is in `open`: each edge of an open segment gets its own lane.
+ * The lanes are `laneWidth` apart, and an open segment is at most `maxWidth` wide. The other segments stay as one way.
+ *
+ * Lane 0 is on the left of the segment, from its first point to its last, with the y axis down.
+ * The function takes a result or its segments. It gives a route for each edge, with the ends at their plugs.
+ * The key of a segment is `runKey` of its edges.
+ */
+export function laneRoutes(source: Result | SegmentGraph, open: ReadonlySet<string>, laneWidth: number, maxWidth: number = Infinity): Route[] {
+  const graph = "paths" in source ? source : segmentsOf(source);
+  if (open.size === 0) return graph.paths.map((path) => ({ edge: path.edge, points: pathPoints(graph, path) }));
+  return lanePaths(graph, (s) => (s.count > 1 && open.has(runKey(s.edges)) ? Math.min(laneWidth, maxWidth / (s.count - 1)) : 0));
+}
+
+/** This function gives the route of each path, with the lanes of each segment at the spacing that `spacingOf` gives. */
+function lanePaths(graph: SegmentGraph, spacingOf: (s: Segment) => number): Route[] {
+  const byId = new Map(graph.segments.map((s) => [s.id, s]));
+  const routes: Route[] = [];
+  for (const path of graph.paths) {
+    const ends = pathPoints(graph, path);
+    const out: Point[] = [];
+    for (const ref of path.segments) {
+      const s = byId.get(ref.segment);
+      if (!s) continue;
+      const spacing = spacingOf(s);
+      const moved = offsetPolyline(s.points, ((s.count - 1) / 2 - ref.lane) * spacing);
+      for (const q of ref.reverse ? moved.reverse() : moved) if (out.length === 0 || dist(q, out[out.length - 1]!) > SAME) out.push(q);
+    }
+    if (out.length >= 2 && ends.length >= 2) {
+      out[0] = ends[0]!;
+      out[out.length - 1] = ends[ends.length - 1]!;
+    }
+    routes.push({ edge: path.edge, points: out });
+  }
+  return routes;
+}
+
+/** The angle, from -π to π, of the turn at `base` from the direction `from` → `base` to the point `to`. */
+function turn(from: Point, base: Point, to: Point): number {
+  const dx = base.x - from.x;
+  const dy = base.y - from.y;
+  const vx = to.x - base.x;
+  const vy = to.y - base.y;
+  return Math.atan2(dx * vy - dy * vx, dx * vx + dy * vy);
+}
+
+/**
+ * This function gives the lane order of the edges on a shared way `run` (metro-line order). Two routes go on together
+ * past the last point of the run until they part. The route that turns more to the left there gets the lane more to the left.
+ * Two routes can part only before the first point. Then the route that comes from more to the left gets the lane more to the left.
+ * A route that does not hold the run goes last.
+ */
+export function orderLanes(run: readonly Point[], routes: readonly Route[]): string[] {
+  const placed: { edge: string; p: readonly Point[]; s: number; e: number }[] = [];
+  const lost: string[] = [];
+  for (const r of routes) {
+    let at = indexOf(r.points, run, 0);
+    if (at >= 0) placed.push({ edge: r.edge, p: r.points, s: at, e: at + run.length - 1 });
+    else {
+      const p = [...r.points].reverse();
+      at = indexOf(p, run, 0);
+      if (at >= 0) placed.push({ edge: r.edge, p, s: at, e: at + run.length - 1 });
+      else lost.push(r.edge);
+    }
+  }
+  const same = (a: Point, b: Point): boolean => dist(a, b) <= SAME;
+  placed.sort((P, Q) => {
+    for (let j = 1; ; j++) {
+      const a = P.p[P.e + j];
+      const b = Q.p[Q.e + j];
+      if (a === undefined || b === undefined) break;
+      if (!same(a, b)) {
+        const d = turn(P.p[P.e + j - 2]!, P.p[P.e + j - 1]!, a) - turn(P.p[P.e + j - 2]!, P.p[P.e + j - 1]!, b);
+        if (d !== 0) return d;
+        break;
+      }
+    }
+    for (let j = 1; ; j++) {
+      const a = P.p[P.s - j];
+      const b = Q.p[Q.s - j];
+      if (a === undefined || b === undefined) break;
+      if (!same(a, b)) {
+        // Seen backward, the left side is the right side. Thus the order turns around.
+        const d = turn(P.p[P.s - j + 2]!, P.p[P.s - j + 1]!, b) - turn(P.p[P.s - j + 2]!, P.p[P.s - j + 1]!, a);
+        if (d !== 0) return d;
+        break;
+      }
+    }
+    return P.edge < Q.edge ? -1 : P.edge > Q.edge ? 1 : 0;
+  });
+  return [...placed.map((x) => x.edge), ...lost.sort()];
+}

package/src/visibility.ts

@@ -0,0 +1,325 @@
+import { dist, inflate, segmentHitsRect, type Point } from "./geom.ts";
+import { Grid } from "./grid.ts";
+import type { Node } from "./model.ts";
+
+/** The nodes as obstacles, with a grid for fast tests. */
+export class Obstacles {
+  readonly #grid: Grid<Node>;
+
+  /** This constructor indexes the boxes of the nodes. */
+  constructor(nodes: readonly Node[], cell: number) {
+    this.#grid = new Grid<Node>(cell);
+    for (const n of nodes) this.#grid.add(n, n.box);
+  }
+
+  /** This method tells if the segment `ab` stays out of each box, made larger by `pad`. The nodes in `skip` do not count. */
+  clear(a: Point, b: Point, pad: number, skip?: ReadonlySet<Node>): boolean {
+    let ok = true;
+    this.#grid.querySegment(a, b, pad, (n) => {
+      if (ok && !skip?.has(n)) {
+        const x = n.box;
+        if (segmentHitsRect(a, b, x.x - pad, x.y - pad, x.x + x.w + pad, x.y + x.h + pad)) ok = false;
+      }
+    });
+    return ok;
+  }
+
+  /** This method tells if the polyline stays out of each box, made larger by `pad`. The nodes in `skip` do not count. */
+  clearPath(p: readonly Point[], pad: number, skip?: ReadonlySet<Node>): boolean {
+    for (let i = 1; i < p.length; i++) if (!this.clear(p[i - 1]!, p[i]!, pad, skip)) return false;
+    return true;
+  }
+
+  /** This method tells if the point is inside a box, made larger by `pad`. The node `skip` does not count. */
+  inside(p: Point, pad: number, skip?: Node): boolean {
+    let hit = false;
+    this.#grid.query({ x: p.x, y: p.y, w: 0, h: 0 }, (n) => {
+      if (!hit && n !== skip) {
+        const b = n.box;
+        hit = p.x > b.x - pad && p.x < b.x + b.w + pad && p.y > b.y - pad && p.y < b.y + b.h + pad;
+      }
+    });
+    return hit;
+  }
+}
+
+/** The options of the corner graph. */
+export interface CornerGraphOptions {
+  /** The distance from a box to its corners. */
+  readonly margin: number;
+  /** The number of visible neighbours of each corner. */
+  readonly k: number;
+  /**
+   * The spacing of a lattice of free points over the plane, in units of the mean node size. The default is 3, and 0 turns it off.
+   * The lattice links groups of nodes across empty space. Without it, the nearest corners of a group are all in that group.
+   */
+  readonly lattice?: number;
+}
+
+/**
+ * A sparse visibility graph. Its vertices are the corners of the boxes, made larger by `margin`.
+ * Each corner links to its `k` nearest corners that it can see. A lattice of free points links the groups of nodes.
+ * A path goes from corner to corner around the boxes.
+ * The obstacle test uses half of the margin, so a path keeps a clearance of at least half of the margin.
+ */
+export class CornerGraph {
+  readonly points: Point[] = [];
+  readonly #adj: number[][] = [];
+  // The length of each link, in the order of #adj.
+  readonly #len: number[][] = [];
+  readonly #obstacles: Obstacles;
+  readonly #pointGrid: Grid<number>;
+  readonly #cell: number;
+  readonly #pad: number;
+  readonly #k: number;
+  // The work arrays of A*, made once and cleared by a stamp.
+  #g = new Float64Array(0);
+  #parent = new Int32Array(0);
+  #open = new Int32Array(0);
+  #closed = new Int32Array(0);
+  #stamp = 0;
+  // The stamp of the last call of nearestVisible that tested each corner.
+  #tested = new Int32Array(0);
+  #testStamp = 0;
+
+  /** This constructor makes the graph of the nodes. */
+  constructor(nodes: readonly Node[], obstacles: Obstacles, options: CornerGraphOptions) {
+    this.#obstacles = obstacles;
+    this.#pad = options.margin / 2;
+    this.#k = options.k;
+    let size = 0;
+    for (const n of nodes) size += (n.box.w + n.box.h) / 2;
+    this.#cell = Math.max(1, size / Math.max(1, nodes.length) + 2 * options.margin);
+    this.#pointGrid = new Grid<number>(this.#cell);
+    for (const n of nodes) {
+      const b = inflate(n.box, options.margin);
+      for (const c of [
+        { x: b.x, y: b.y },
+        { x: b.x + b.w, y: b.y },
+        { x: b.x + b.w, y: b.y + b.h },
+        { x: b.x, y: b.y + b.h },
+      ]) {
+        if (!obstacles.inside(c, this.#pad)) this.#addPoint(c);
+      }
+    }
+    const spacing = (options.lattice ?? 3) * this.#cell;
+    if (spacing > 0 && nodes.length > 0) {
+      let x0 = Infinity;
+      let y0 = Infinity;
+      let x1 = -Infinity;
+      let y1 = -Infinity;
+      for (const n of nodes) {
+        x0 = Math.min(x0, n.box.x);
+        y0 = Math.min(y0, n.box.y);
+        x1 = Math.max(x1, n.box.x + n.box.w);
+        y1 = Math.max(y1, n.box.y + n.box.h);
+      }
+      for (let x = x0 - spacing / 2; x <= x1 + spacing; x += spacing) {
+        for (let y = y0 - spacing / 2; y <= y1 + spacing; y += spacing) {
+          const c = { x, y };
+          if (!obstacles.inside(c, options.margin)) this.#addPoint(c);
+        }
+      }
+    }
+    const links: Set<number>[] = this.points.map(() => new Set<number>());
+    for (let i = 0; i < this.points.length; i++) {
+      for (const j of this.nearestVisible(this.points[i]!, this.#k, i)) {
+        links[i]!.add(j);
+        links[j]!.add(i);
+      }
+    }
+    links.forEach((s, i) => {
+      const adj = [...s].sort((a, b) => a - b);
+      this.#adj.push(adj);
+      this.#len.push(adj.map((j) => dist(this.points[i]!, this.points[j]!)));
+    });
+  }
+
+  #addPoint(c: Point): void {
+    this.#pointGrid.add(this.points.length, { x: c.x, y: c.y, w: 0, h: 0 });
+    this.points.push(c);
+  }
+
+  /** This method gives the number of links of the graph. */
+  get linkCount(): number {
+    let n = 0;
+    for (const a of this.#adj) n += a.length;
+    return n / 2;
+  }
+
+  /** This method gives the index of the corner nearest to `p`, seen or not, or -1 when the graph has no corner near it. */
+  nearest(p: Point): number {
+    for (let r = this.#cell; r <= this.#cell * 64; r *= 2) {
+      let best = -1;
+      let bestD = Infinity;
+      this.#pointGrid.query({ x: p.x - r, y: p.y - r, w: 2 * r, h: 2 * r }, (j) => {
+        const d = dist(p, this.points[j]!);
+        if (d < bestD || (d === bestD && j < best)) {
+          bestD = d;
+          best = j;
+        }
+      });
+      // A corner beyond r can be nearer than a corner in a corner of the square. Within r, none is.
+      if (best >= 0 && bestD <= r) return best;
+    }
+    return -1;
+  }
+
+  /** This method gives at most `k` corners that `p` can see, nearest first. The corner `self` does not count. */
+  nearestVisible(p: Point, k: number, self = -1): number[] {
+    const out: number[] = [];
+    if (this.#tested.length < this.points.length) this.#tested = new Int32Array(this.points.length);
+    const tested = this.#tested;
+    const stamp = ++this.#testStamp;
+    for (let r = this.#cell; r <= this.#cell * 8; r *= 2) {
+      const cands: [number, number][] = [];
+      this.#pointGrid.query({ x: p.x - r, y: p.y - r, w: 2 * r, h: 2 * r }, (j) => {
+        if (j !== self && tested[j] !== stamp) cands.push([dist(p, this.points[j]!), j]);
+      });
+      cands.sort((a, b) => a[0] - b[0] || a[1] - b[1]);
+      for (const [d, j] of cands) {
+        // A corner beyond r can be nearer than a corner of the next ring, so it waits for that ring.
+        if (d > r) break;
+        tested[j] = stamp;
+        if (this.#obstacles.clear(p, this.points[j]!, this.#pad)) out.push(j);
+        if (out.length >= k) return out;
+      }
+    }
+    return out;
+  }
+
+  /**
+   * This method gives a shortest path from `from` to `to` in the graph (A*), with the two points as its ends.
+   * It gives `undefined` when the graph links no corner near `from` to a corner near `to`.
+   * The function `cost` can change the cost of the link from corner `i` to corner `j`, of length `d`.
+   * It must give at least `floor` × `d`, so that the estimate of A* stays below the true cost.
+   */
+  path(from: Point, to: Point, cost?: (i: number, j: number, d: number) => number, floor: number = 1): Point[] | undefined {
+    if (this.#obstacles.clear(from, to, this.#pad)) return [from, to];
+    const n = this.points.length;
+    if (this.#g.length < n) {
+      this.#g = new Float64Array(n);
+      this.#parent = new Int32Array(n);
+      this.#open = new Int32Array(n);
+      this.#closed = new Int32Array(n);
+    }
+    const stamp = ++this.#stamp;
+    const goal = new Map<number, number>();
+    for (const j of this.nearestVisible(to, this.#k * 2)) goal.set(j, dist(this.points[j]!, to));
+    if (goal.size === 0) return undefined;
+    const heap = new MinHeap();
+    const h = (i: number): number => floor * dist(this.points[i]!, to);
+    const relax = (j: number, g: number, parent: number): void => {
+      if (this.#closed[j] === stamp) return;
+      if (this.#open[j] === stamp && this.#g[j]! <= g) return;
+      this.#open[j] = stamp;
+      this.#g[j] = g;
+      this.#parent[j] = parent;
+      heap.push(g + h(j), j);
+    };
+    for (const j of this.nearestVisible(from, this.#k * 2)) relax(j, dist(from, this.points[j]!), -1);
+    let best = Infinity;
+    let bestEnd = -1;
+    while (heap.size > 0) {
+      if (heap.topKey >= best) break;
+      const i = heap.pop();
+      if (this.#closed[i] === stamp) continue;
+      this.#closed[i] = stamp;
+      const gi = this.#g[i]!;
+      const tail = goal.get(i);
+      if (tail !== undefined && gi + tail < best) {
+        best = gi + tail;
+        bestEnd = i;
+      }
+      const adj = this.#adj[i]!;
+      const len = this.#len[i]!;
+      for (let m = 0; m < adj.length; m++) {
+        const j = adj[m]!;
+        const d = len[m]!;
+        relax(j, gi + (cost ? cost(i, j, d) : d), i);
+      }
+    }
+    if (bestEnd < 0) return undefined;
+    const out: Point[] = [to];
+    for (let i = bestEnd; i >= 0; i = this.#parent[i]!) out.push(this.points[i]!);
+    out.push(from);
+    return out.reverse();
+  }
+
+  /** This method removes the vertices that a later vertex makes needless: from each vertex, it goes to the farthest vertex in view. */
+  pull(p: readonly Point[], lookahead = 16): Point[] {
+    if (p.length < 3) return [...p];
+    const out: Point[] = [p[0]!];
+    let i = 0;
+    while (i < p.length - 1) {
+      let j = Math.min(p.length - 1, i + lookahead);
+      while (j > i + 1 && !this.#obstacles.clear(p[i]!, p[j]!, this.#pad)) j--;
+      out.push(p[j]!);
+      i = j;
+    }
+    return out;
+  }
+}
+
+/** A binary min-heap of (key, value) pairs. Equal keys come out by the smaller value, so the order is deterministic. */
+class MinHeap {
+  readonly #k: number[] = [];
+  readonly #v: number[] = [];
+
+  get size(): number {
+    return this.#k.length;
+  }
+
+  #less(a: number, b: number): boolean {
+    return this.#k[a]! < this.#k[b]! || (this.#k[a] === this.#k[b] && this.#v[a]! < this.#v[b]!);
+  }
+
+  #swap(a: number, b: number): void {
+    const k = this.#k[a]!;
+    this.#k[a] = this.#k[b]!;
+    this.#k[b] = k;
+    const v = this.#v[a]!;
+    this.#v[a] = this.#v[b]!;
+    this.#v[b] = v;
+  }
+
+  /** The smallest key. */
+  get topKey(): number {
+    return this.#k[0]!;
+  }
+
+  push(key: number, value: number): void {
+    this.#k.push(key);
+    this.#v.push(value);
+    let i = this.#k.length - 1;
+    while (i > 0) {
+      const p = (i - 1) >> 1;
+      if (!this.#less(i, p)) break;
+      this.#swap(i, p);
+      i = p;
+    }
+  }
+
+  /** This method removes the pair with the smallest key and gives its value. */
+  pop(): number {
+    const top = this.#v[0]!;
+    const k = this.#k.pop()!;
+    const v = this.#v.pop()!;
+    if (this.#k.length > 0) {
+      this.#k[0] = k;
+      this.#v[0] = v;
+      let i = 0;
+      for (;;) {
+        const l = 2 * i + 1;
+        const r = l + 1;
+        let m = i;
+        if (l < this.#k.length && this.#less(l, m)) m = l;
+        if (r < this.#k.length && this.#less(r, m)) m = r;
+        if (m === i) break;
+        this.#swap(i, m);
+        i = m;
+      }
+    }
+    return top;
+  }
+}