@knowvah/dot-engine 0.1.0

package/src/pathplan/triang.ts

@@ -0,0 +1,221 @@
+// SPDX-License-Identifier: EPL-2.0
+/** @see lib/pathplan/triang.c */
+
+import type { Point, Poly } from './types.js';
+
+export const ISCCW = 1;
+export const ISCW = 2;
+export const ISON = 3;
+
+// ---------------------------------------------------------------------------
+// FMA-faithful ccw sign
+//
+// The oracle's triang.c:ccw is compiled by clang (arm64, default
+// -ffp-contract=on) into `fnmul d0,(p1.x-p2.x),(p3.y-p2.y)` followed by
+// `fmadd d0,(p1.y-p2.y),(p3.x-p2.x),d0` — i.e. the FIRST product is EXACT
+// (fused) while the second is rounded:
+//
+//   d = exact((p1.y-p2.y)·(p3.x-p2.x)) − fl((p3.y-p2.y)·(p1.x-p2.x))
+//
+// This is not the C source's arithmetic: for a query point bit-equal to a
+// segment endpoint the two products are identical, the source says d == 0
+// (ISON), but the compiled binary returns the (negated) rounding error of
+// the product — flipping ISON to ISCW/ISCCW. shortest.c's pointintri then
+// rejects polygon-vertex endpoints ("destination point not in any
+// triangle"), makeMultiSpline fails, and every circo/twopi 2-cycle falls
+// back to plain routing. Matching the oracle therefore REQUIRES emulating
+// the contraction, not the source. JS has no fma, so: plain double
+// evaluation with a conservative error bound decides the easy cases (where
+// plain and fused signs provably agree), and an exact Dekker-product +
+// dyadic-BigInt path decides the near-zero ones.
+// ---------------------------------------------------------------------------
+
+const SPLIT = 134217729; // 2^27 + 1 (Dekker)
+
+/** Exact residue of a*b given its rounded product P: a*b = P + err. */
+function twoProductErr(a: number, b: number, P: number): number {
+  const ac = SPLIT * a;
+  const ahi = ac - (ac - a);
+  const alo = a - ahi;
+  const bc = SPLIT * b;
+  const bhi = bc - (bc - b);
+  const blo = b - bhi;
+  return ((ahi * bhi - P) + ahi * blo + alo * bhi) + alo * blo;
+}
+
+/** Exact tail of P - D given its rounded difference s: P - D = s + tail. */
+function twoDiffTail(P: number, D: number, s: number): number {
+  const bvirt = P - s;
+  const avirt = s + bvirt;
+  const bround = bvirt - D;
+  const around = P - avirt;
+  return around + bround;
+}
+
+const dyadicView = new DataView(new ArrayBuffer(8));
+
+/** Decompose a finite double into m·2^e with m a (sign-carrying) BigInt. */
+function dyadic(x: number): { m: bigint; e: number } {
+  if (x === 0) return { m: 0n, e: 0 };
+  dyadicView.setFloat64(0, x);
+  const hi = dyadicView.getUint32(0);
+  const lo = dyadicView.getUint32(4);
+  const sign = hi >>> 31 ? -1n : 1n;
+  const biasedExp = (hi >>> 20) & 0x7ff;
+  let mant = (BigInt(hi & 0xfffff) << 32n) | BigInt(lo);
+  let e: number;
+  if (biasedExp === 0) {
+    e = -1074; // subnormal
+  } else {
+    mant |= 1n << 52n;
+    e = biasedExp - 1075;
+  }
+  return { m: sign * mant, e };
+}
+
+/** Exact sign of a + b + c over arbitrary finite doubles. */
+function signOfSum3(a: number, b: number, c: number): number {
+  const da = dyadic(a);
+  const db = dyadic(b);
+  const dc = dyadic(c);
+  const e = Math.min(da.e, db.e, dc.e);
+  const sum = (da.m << BigInt(da.e - e)) + (db.m << BigInt(db.e - e)) +
+    (dc.m << BigInt(dc.e - e));
+  return sum > 0n ? 1 : (sum < 0n ? -1 : 0);
+}
+
+class TriangHelper {
+  static ccw(p1: Point, p2: Point, p3: Point): number {
+    const a = p1.y - p2.y;
+    const b = p3.x - p2.x;
+    const c = p3.y - p2.y;
+    const e = p1.x - p2.x;
+    const P = a * b;
+    const D = c * e;
+    const d = P - D;
+    // Fast path: when |d| clearly exceeds the worst-case slack between the
+    // plain and fused evaluations (product residue ≤ ulp(P)/2 plus the
+    // subtraction's own rounding), both agree on the sign.
+    const bound = 2.3e-16 * (Math.abs(P) + Math.abs(D));
+    if (d > bound) return ISCW;
+    if (d < -bound) return ISCCW;
+    // Exact: sign of exact(a·b) − D = (P − D) + twoProductErr.
+    const err = twoProductErr(a, b, P);
+    const s = P - D;
+    const tail = twoDiffTail(P, D, s);
+    const sign = signOfSum3(s, tail, err);
+    return sign > 0 ? ISCW : (sign < 0 ? ISCCW : ISON);
+  }
+
+  /** Is pc on segment [pa, pb]? */
+  static between(pa: Point, pb: Point, pc: Point): boolean {
+    if (TriangHelper.ccw(pa, pb, pc) !== ISON) return false;
+    const pba = { x: pb.x - pa.x, y: pb.y - pa.y };
+    const pca = { x: pc.x - pa.x, y: pc.y - pa.y };
+    return pca.x * pba.x + pca.y * pba.y >= 0 &&
+      pca.x * pca.x + pca.y * pca.y <= pba.x * pba.x + pba.y * pba.y;
+  }
+
+  static anyCollinear(pa: Point, pb: Point, pc: Point, pd: Point): boolean {
+    const c = TriangHelper.ccw;
+    return c(pa,pb,pc)===ISON || c(pa,pb,pd)===ISON ||
+           c(pc,pd,pa)===ISON || c(pc,pd,pb)===ISON;
+  }
+
+  static collinearIntersects(pa: Point, pb: Point, pc: Point, pd: Point): boolean {
+    const b = TriangHelper.between;
+    return b(pa,pb,pc) || b(pa,pb,pd) || b(pc,pd,pa) || b(pc,pd,pb);
+  }
+
+  static crossIntersects(pa: Point, pb: Point, pc: Point, pd: Point): boolean {
+    const c = TriangHelper.ccw;
+    const c1 = c(pa,pb,pc)===ISCCW ? 1 : 0;
+    const c2 = c(pa,pb,pd)===ISCCW ? 1 : 0;
+    const c3 = c(pc,pd,pa)===ISCCW ? 1 : 0;
+    const c4 = c(pc,pd,pb)===ISCCW ? 1 : 0;
+    return (c1^c2) !== 0 && (c3^c4) !== 0;
+  }
+
+  static intersects(pa: Point, pb: Point, pc: Point, pd: Point): boolean {
+    if (TriangHelper.anyCollinear(pa, pb, pc, pd))
+      return TriangHelper.collinearIntersects(pa, pb, pc, pd);
+    return TriangHelper.crossIntersects(pa, pb, pc, pd);
+  }
+
+  static isdiagNeighborhood(i: number, ip2: number, get: (n: number) => Point, pointn: number): boolean {
+    const ip1 = (i + 1) % pointn;
+    const im1 = (i + pointn - 1) % pointn;
+    const c = TriangHelper.ccw;
+    if (c(get(im1), get(i), get(ip1)) === ISCCW) {
+      return c(get(i), get(ip2), get(im1)) === ISCCW &&
+             c(get(ip2), get(i), get(ip1)) === ISCCW;
+    }
+    return c(get(i), get(ip2), get(ip1)) === ISCW;
+  }
+
+  static isdiagonal(i: number, ip2: number, get: (n: number) => Point, pointn: number): boolean {
+    if (!TriangHelper.isdiagNeighborhood(i, ip2, get, pointn)) return false;
+    for (let j = 0; j < pointn; j++) {
+      const jp1 = (j + 1) % pointn;
+      if (j !== i && jp1 !== i && j !== ip2 && jp1 !== ip2) {
+        if (TriangHelper.intersects(get(i), get(ip2), get(j), get(jp1)))
+          return false;
+      }
+    }
+    return true;
+  }
+
+  static triangulateInner(pts: Point[], fn: (t: [Point,Point,Point]) => void): boolean {
+    // C Ptriangulate's helper is tail-recursive (recurses on the polygon with the
+    // clipped ear removed). The recursion depth equals the vertex count, so a
+    // large polygon overflows V8's stack (no TCO). Faithful loop form per the
+    // AD-3 recursion->iteration pattern: same ear per pass, same emit order,
+    // same return value. Mirrors shortest.ts:triInner.
+    let cur = pts;
+    while (cur.length > 3) {
+      const n = cur.length;
+      let cut = -1;
+      for (let i = 0; i < n; i++) {
+        const ip2 = (i + 2) % n;
+        if (TriangHelper.isdiagonal(i, ip2, k => cur[k], n)) {
+          fn([cur[i], cur[(i+1)%n], cur[ip2]]);
+          cut = (i + 1) % n;
+          break;
+        }
+      }
+      if (cut === -1) return false;
+      cur = cur.filter((_, idx) => idx !== cut);
+    }
+    fn([cur[0], cur[1], cur[2]]);
+    return true;
+  }
+}
+
+export { TriangHelper };
+
+function signedArea(ps: Point[]): number {
+  let sum = 0;
+  for (let i = 0; i < ps.length; i++) {
+    const j = (i + 1) % ps.length;
+    sum += ps[i].x * ps[j].y - ps[j].x * ps[i].y;
+  }
+  return sum;
+}
+
+/** @see lib/pathplan/triang.c:Ptriangulate */
+export function triangulate(poly: Poly, fn: (t: [Point,Point,Point]) => void): number {
+  const ps = poly.ps.slice();
+  if (signedArea(ps) > 0) ps.reverse();
+  if (!TriangHelper.triangulateInner(ps, fn)) return 1;
+  return 0;
+}
+
+/** @see lib/pathplan/triang.c:ccw */
+export function ccw(p1: Point, p2: Point, p3: Point): number {
+  return TriangHelper.ccw(p1, p2, p3);
+}
+
+/** @see lib/pathplan/tri.h:isdiagonal */
+export function isdiagonal(i: number, ip2: number, get: (n: number) => Point, pointn: number): boolean {
+  return TriangHelper.isdiagonal(i, ip2, get, pointn);
+}

package/src/pathplan/types.ts

@@ -0,0 +1,16 @@
+// SPDX-License-Identifier: EPL-2.0
+/** @see lib/pathplan/pathgeom.h, lib/pathplan/vis.h */
+
+export interface Point { x: number; y: number; }
+export interface Poly { ps: Point[]; }
+export interface Edge { a: Point; b: Point; }
+
+export interface VConfig {
+  N: number;
+  Npoly: number;
+  P: Point[];
+  start: number[];
+  next: number[];
+  prev: number[];
+  vis: number[][];
+}

package/src/pathplan/visibility.ts

@@ -0,0 +1,170 @@
+// SPDX-License-Identifier: EPL-2.0
+/** @see lib/pathplan/visibility.c */
+
+import type { Point, VConfig } from './types.js';
+
+export const POLYID_NONE = -1111;
+export const POLYID_UNKNOWN = -2222;
+
+class VisHelper {
+  static wind(a: Point, b: Point, c: Point): number {
+    const w = (a.y - b.y) * (c.x - b.x) - (c.y - b.y) * (a.x - b.x);
+    return w > 0.0001 ? 1 : (w < -0.0001 ? -1 : 0);
+  }
+
+  static dist2(a: Point, b: Point): number {
+    const dx = a.x - b.x, dy = a.y - b.y;
+    return dx * dx + dy * dy;
+  }
+
+  static dist(a: Point, b: Point): number {
+    return Math.sqrt(VisHelper.dist2(a, b));
+  }
+
+  static inBetween(a: Point, b: Point, c: Point): boolean {
+    if (a.x !== b.x)
+      return (a.x < c.x && c.x < b.x) || (b.x < c.x && c.x < a.x);
+    return (a.y < c.y && c.y < b.y) || (b.y < c.y && c.y < a.y);
+  }
+
+  static intersect(a: Point, b: Point, c: Point, d: Point): boolean {
+    const w = VisHelper.wind;
+    const a_abc = w(a, b, c);
+    if (a_abc === 0 && VisHelper.inBetween(a, b, c)) return true;
+    const a_abd = w(a, b, d);
+    if (a_abd === 0 && VisHelper.inBetween(a, b, d)) return true;
+    const a_cda = w(c, d, a);
+    const a_cdb = w(c, d, b);
+    return a_abc * a_abd < 0 && a_cda * a_cdb < 0;
+  }
+
+  static inCone(a0: Point, a1: Point, a2: Point, b: Point): boolean {
+    const m = VisHelper.wind(b, a0, a1);
+    const p = VisHelper.wind(b, a1, a2);
+    if (VisHelper.wind(a0, a1, a2) > 0) return m >= 0 && p >= 0;
+    return m >= 0 || p >= 0;
+  }
+
+  static clear(pti: Point, ptj: Point, start: number, end: number, conf: VConfig): boolean {
+    const pts = conf.P, nextPt = conf.next, V = conf.N;
+    for (let k = 0; k < start; k++) {
+      if (VisHelper.intersect(pti, ptj, pts[k], pts[nextPt[k]])) return false;
+    }
+    for (let k = end; k < V; k++) {
+      if (VisHelper.intersect(pti, ptj, pts[k], pts[nextPt[k]])) return false;
+    }
+    return true;
+  }
+
+  static compVis(conf: VConfig): void {
+    const V = conf.N, pts = conf.P, nextPt = conf.next, prevPt = conf.prev;
+    const wadj = conf.vis;
+    for (let i = 0; i < V; i++) {
+      const previ = prevPt[i];
+      const d = VisHelper.dist(pts[i], pts[previ]);
+      wadj[i][previ] = d; wadj[previ][i] = d;
+      const jstart = previ === i - 1 ? i - 2 : i - 1;
+      for (let j = jstart; j >= 0; j--) {
+        if (VisHelper.inCone(pts[prevPt[i]], pts[i], pts[nextPt[i]], pts[j]) &&
+            VisHelper.inCone(pts[prevPt[j]], pts[j], pts[nextPt[j]], pts[i]) &&
+            VisHelper.clear(pts[i], pts[j], V, V, conf)) {
+          const dij = VisHelper.dist(pts[i], pts[j]);
+          wadj[i][j] = dij; wadj[j][i] = dij;
+        }
+      }
+    }
+  }
+
+  static allocVis(N: number): number[][] {
+    const vis: number[][] = [];
+    for (let i = 0; i < N; i++) vis.push(new Array<number>(N).fill(0));
+    vis.push(null as unknown as number[]);
+    vis.push(null as unknown as number[]);
+    return vis;
+  }
+
+  static polyhit(conf: VConfig, p: Point): number {
+    for (let i = 0; i < conf.Npoly; i++) {
+      const s = conf.start[i], e = conf.start[i + 1];
+      if (inPolyHelper(conf.P.slice(s, e), p)) return i;
+    }
+    return POLYID_NONE;
+  }
+
+  static ptVisRange(conf: VConfig, p: Point, start: number, end: number, vadj: number[]): void {
+    const pts = conf.P, nextPt = conf.next, prevPt = conf.prev, V = conf.N;
+    for (let k = 0; k < V; k++) {
+      if (k >= start && k < end) { vadj[k] = 0; continue; }
+      const pk = pts[k];
+      if (VisHelper.inCone(pts[prevPt[k]], pk, pts[nextPt[k]], p) &&
+          VisHelper.clear(p, pk, start, end, conf)) {
+        vadj[k] = VisHelper.dist(p, pk);
+      } else {
+        vadj[k] = 0;
+      }
+    }
+    vadj[V] = 0; vadj[V + 1] = 0;
+  }
+
+  static computeRanges(pp: number, qp: number, conf: VConfig): [number, number, number, number] {
+    if (pp < 0) {
+      if (qp < 0) return [0, 0, 0, 0];
+      return [0, 0, conf.start[qp], conf.start[qp + 1]];
+    }
+    if (qp < 0) return [0, 0, conf.start[pp], conf.start[pp + 1]];
+    if (pp <= qp)
+      return [conf.start[pp], conf.start[pp+1], conf.start[qp], conf.start[qp+1]];
+    return [conf.start[qp], conf.start[qp+1], conf.start[pp], conf.start[pp+1]];
+  }
+}
+
+function inPolyHelper(ps: Point[], q: Point): boolean {
+  const n = ps.length;
+  for (let i = 0; i < n; i++) {
+    const i1 = (i + n - 1) % n;
+    if (VisHelper.wind(ps[i1], ps[i], q) === 1) return false;
+  }
+  return true;
+}
+
+export function wind(a: Point, b: Point, c: Point): number {
+  return VisHelper.wind(a, b, c);
+}
+
+export function inPoly(ps: Point[], q: Point): boolean {
+  return inPolyHelper(ps, q);
+}
+
+/** @see lib/pathplan/visibility.c:visibility */
+export function visibility(conf: VConfig): void {
+  conf.vis = VisHelper.allocVis(conf.N);
+  VisHelper.compVis(conf);
+}
+
+/** @see lib/pathplan/visibility.c:ptVis */
+export function ptVis(conf: VConfig, pp: number, p: Point): number[] {
+  const V = conf.N;
+  const vadj = new Array<number>(V + 2).fill(0);
+  let start: number, end: number;
+  if (pp === POLYID_UNKNOWN) pp = VisHelper.polyhit(conf, p);
+  if (pp >= 0) {
+    start = conf.start[pp]; end = conf.start[pp + 1];
+  } else {
+    start = V; end = V;
+  }
+  VisHelper.ptVisRange(conf, p, start, end, vadj);
+  return vadj;
+}
+
+/** @see lib/pathplan/visibility.c:directVis */
+export function directVis(p: Point, pp: number, q: Point, qp: number, conf: VConfig): boolean {
+  const [s1, e1, s2, e2] = VisHelper.computeRanges(pp, qp, conf);
+  const V = conf.N, pts = conf.P, nextPt = conf.next;
+  for (let k = 0; k < s1; k++)
+    if (VisHelper.intersect(p, q, pts[k], pts[nextPt[k]])) return false;
+  for (let k = e1; k < s2; k++)
+    if (VisHelper.intersect(p, q, pts[k], pts[nextPt[k]])) return false;
+  for (let k = e2; k < V; k++)
+    if (VisHelper.intersect(p, q, pts[k], pts[nextPt[k]])) return false;
+  return true;
+}

package/src/pathplan/vispath.ts

@@ -0,0 +1,118 @@
+// SPDX-License-Identifier: EPL-2.0
+/** @see lib/pathplan/shortestpth.c, lib/pathplan/cvt.c */
+
+import type { Point, Poly, VConfig } from './types.js';
+import { ptVis, directVis, visibility } from './visibility.js';
+
+const UNSEEN = Number.MAX_SAFE_INTEGER;
+
+class VisPathHelper {
+  static relaxNode(k: number, V: number, dad: number[], vl: Float64Array, wadj: number[][]): number {
+    let min = -1;
+    for (let t = 0; t < V; t++) {
+      if (vl[t + 1] < 0) {
+        const wkt = k >= t ? wadj[k][t] : wadj[t][k];
+        const newpri = -(vl[k + 1] + wkt);
+        if (wkt !== 0 && vl[t + 1] < newpri) { vl[t + 1] = newpri; dad[t] = k; }
+        if (vl[t + 1] > vl[min + 1]) min = t;
+      }
+    }
+    return min;
+  }
+
+  static dijkstra(root: number, target: number, V: number, wadj: number[][]): number[] {
+    const dad = new Array<number>(V).fill(-1);
+    const vl = new Float64Array(V + 1);
+    for (let k = 0; k < V; k++) vl[k + 1] = -UNSEEN;
+    vl[0] = -(UNSEEN + 1);
+    let min = root;
+    while (min !== target) {
+      const k = min;
+      vl[k + 1] *= -1;
+      if (vl[k + 1] === UNSEEN) vl[k + 1] = 0;
+      min = VisPathHelper.relaxNode(k, V, dad, vl, wadj);
+      if (min === -1) break;
+    }
+    return dad;
+  }
+
+  static countPath(dad: number[], V: number): number {
+    let n = 2;
+    for (let i = dad[V]; i !== undefined && i !== -1 && i !== V + 1; i = dad[i]) n++;
+    return n;
+  }
+
+  static extractPath(dad: number[], V: number, p0: Point, p1: Point, conf: VConfig): Point[] {
+    if (dad[V] === -1) return [p0, p1];
+    const n = VisPathHelper.countPath(dad, V);
+    const ops = new Array<Point>(n);
+    let j = n - 1;
+    ops[j--] = p1;
+    for (let i = dad[V]; i !== undefined && i !== -1 && i !== V + 1; i = dad[i]) {
+      ops[j--] = conf.P[i];
+    }
+    ops[0] = p0;
+    return ops;
+  }
+
+  static fillArrays(conf: VConfig, polys: Poly[]): void {
+    let i = 0;
+    for (let pi = 0; pi < polys.length; pi++) {
+      const start = i;
+      conf.start[pi] = start;
+      const pts = polys[pi].ps;
+      const end = start + pts.length - 1;
+      for (let pt = 0; pt < pts.length; pt++) {
+        conf.P[i] = pts[pt];
+        conf.next[i] = i + 1;
+        conf.prev[i] = i - 1;
+        i++;
+      }
+      conf.next[end] = start;
+      conf.prev[start] = end;
+    }
+    conf.start[polys.length] = i;
+  }
+
+  static open(polys: Poly[]): VConfig {
+    const n = polys.reduce((s, p) => s + p.ps.length, 0);
+    const conf: VConfig = {
+      N: n, Npoly: polys.length,
+      P: new Array<Point>(n),
+      start: new Array<number>(polys.length + 1),
+      next: new Array<number>(n),
+      prev: new Array<number>(n),
+      vis: [],
+    };
+    VisPathHelper.fillArrays(conf, polys);
+    visibility(conf);
+    return conf;
+  }
+
+  static path(conf: VConfig, p0: Point, poly0: number, p1: Point, poly1: number): Point[] {
+    const pvis = ptVis(conf, poly0, p0);
+    const qvis = ptVis(conf, poly1, p1);
+    const V = conf.N;
+    let dad: number[];
+    if (directVis(p0, poly0, p1, poly1, conf)) {
+      dad = new Array<number>(V + 2).fill(-1);
+      dad[V] = V + 1;
+    } else {
+      conf.vis[V] = qvis;
+      conf.vis[V + 1] = pvis;
+      dad = VisPathHelper.dijkstra(V + 1, V, V + 2, conf.vis);
+    }
+    return VisPathHelper.extractPath(dad, V, p0, p1, conf);
+  }
+}
+
+/** @see lib/pathplan/cvt.c:Pobsopen */
+export function obsOpen(polys: Poly[]): VConfig { return VisPathHelper.open(polys); }
+
+/** @see lib/pathplan/cvt.c:Pobsclose */
+export function obsClose(_conf: VConfig): void { /* GC handles memory */ }
+
+/** @see lib/pathplan/cvt.c:Pobspath */
+export function obsPath(conf: VConfig, p0: Point, poly0: number, p1: Point, poly1: number): Point[] {
+  return VisPathHelper.path(conf, p0, poly0, p1, poly1);
+}