Initial project import

src/routing/vec.ts

@@ -0,0 +1,70 @@
+import type { Vec3 } from '../types';
+
+/** Tolerance used for geometric comparisons throughout the router (ft). */
+export const EPS = 1e-6;
+
+export const add = (a: Vec3, b: Vec3): Vec3 => [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
+export const sub = (a: Vec3, b: Vec3): Vec3 => [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
+export const scale = (a: Vec3, s: number): Vec3 => [a[0] * s, a[1] * s, a[2] * s];
+export const dot = (a: Vec3, b: Vec3) => a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+export const cross = (a: Vec3, b: Vec3): Vec3 => [
+  a[1] * b[2] - a[2] * b[1],
+  a[2] * b[0] - a[0] * b[2],
+  a[0] * b[1] - a[1] * b[0],
+];
+export const length = (a: Vec3) => Math.hypot(a[0], a[1], a[2]);
+export const dist = (a: Vec3, b: Vec3) => Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
+export const manhattan = (a: Vec3, b: Vec3) =>
+  Math.abs(a[0] - b[0]) + Math.abs(a[1] - b[1]) + Math.abs(a[2] - b[2]);
+export const vecEquals = (a: Vec3, b: Vec3, eps = EPS) =>
+  Math.abs(a[0] - b[0]) < eps && Math.abs(a[1] - b[1]) < eps && Math.abs(a[2] - b[2]) < eps;
+
+/**
+ * The six axis-aligned unit directions. Index layout: even = positive,
+ * odd = negative, so `idx >> 1` is the axis (0=X 1=Y 2=Z) and `idx ^ 1`
+ * is the reversed direction.
+ */
+export const AXIS_DIRS: readonly Vec3[] = [
+  [1, 0, 0],
+  [-1, 0, 0],
+  [0, 1, 0],
+  [0, -1, 0],
+  [0, 0, 1],
+  [0, 0, -1],
+];
+
+export const axisOfDir = (dirIdx: number) => dirIdx >> 1;
+export const signOfDir = (dirIdx: number) => (dirIdx & 1 ? -1 : 1);
+export const reverseDir = (dirIdx: number) => dirIdx ^ 1;
+
+/** Index into AXIS_DIRS for an (exactly) axis-aligned unit vector, or -1. */
+export function dirIndexOf(v: Vec3, eps = EPS): number {
+  for (let i = 0; i < 6; i++) {
+    const d = AXIS_DIRS[i];
+    if (Math.abs(v[0] - d[0]) < eps && Math.abs(v[1] - d[1]) < eps && Math.abs(v[2] - d[2]) < eps) {
+      return i;
+    }
+  }
+  return -1;
+}
+
+/**
+ * Snap a roughly axis-aligned direction to the nearest exact axis direction.
+ * Returns null when the vector is degenerate or too far from any axis
+ * (more than ~25° off).
+ */
+export function snapAxisDir(v: Vec3): Vec3 | null {
+  const l = length(v);
+  if (l < EPS) return null;
+  const n: Vec3 = [v[0] / l, v[1] / l, v[2] / l];
+  let best = -1;
+  let bestDot = 0.9; // cos(~25°) — refuse wildly diagonal directions
+  for (let i = 0; i < 6; i++) {
+    const d = dot(n, AXIS_DIRS[i]);
+    if (d > bestDot) {
+      bestDot = d;
+      best = i;
+    }
+  }
+  return best >= 0 ? ([...AXIS_DIRS[best]] as Vec3) : null;
+}